Changeset 872
- Timestamp:
- 01/25/2011 08:40:53 AM (10 years ago)
- Location:
- libs/SAMGAR/trunk/Modules/Samgar2Player
- Files:
-
- 4 edited
Legend:
- Unmodified
- Added
- Removed
-
libs/SAMGAR/trunk/Modules/Samgar2Player/PROTOCOL.txt
r828 r872 34 34 Ack = 1 35 35 36 36 enum PlayerProxy_Position2d_CMD {Position2d_SET_SPEED=1, Position2d_SET_GOAL, Position2d_SET_ODOM, Position2d_SET_MOTOR}; 37 37 enum PlayerProxy_Localize_CMD {Localize_SET_POSE=1}; 38 38 enum PlayerProxy_Planner_CMD {Planner_SET_GOAL=1, Planner_SET_ENABLE}; … … 43 43 44 44 45 enum PlayerProxy_Position2d_DATA {Position2d_SPEED=2, Position2d_POSE, Position2d_SIZE, Position2d_STALL}; 45 46 enum PlayerProxy_Localize_DATA {Localize_MAP_INFO=2, Localize_HYPOTHS}; 46 47 enum PlayerProxy_Planner_DATA {Planner_GOAL=2, Planner_POSE, Planner_CURR_WAYPOINT, Planner_PATH, Planner_WAYPOINTS}; … … 104 105 Ack = 1 105 106 Position2d_SPEED = 2 106 Position2d_POS ITION= 3107 Position2d_POSE = 3 107 108 Position2d_SIZE = 4 108 109 Position2d_STALL = 5 … … 143 144 144 145 +------++---------------+-----------------------+-----------------------+-----------------------+-----------------------+ 145 | desc || command_id | Position2d_SPEED | Position2d_POS ITION| Position2d_SIZE | Position2d_STALL |146 | desc || command_id | Position2d_SPEED | Position2d_POSE | Position2d_SIZE | Position2d_STALL | 146 147 +------++---------------+-----------------------+-----------------------+-----------------------+-----------------------+ 147 148 | type || SET_REQ | bool | bool | bool | bool | … … 161 162 162 163 +------++-----------------------+---------------+ 163 | desc || command_id | goal|164 +------++-----------------------+---------------+ 165 | type || Position2d_POS ITION| coord_type |164 | desc || command_id | pose | 165 +------++-----------------------+---------------+ 166 | type || Position2d_POSE | coord_type | 166 167 +------++-----------------------+---------------+ 167 168 | info || | | -
libs/SAMGAR/trunk/Modules/Samgar2Player/Player2SamgarModule.cpp
r828 r872 2 2 */ 3 3 #include "Player2SamgarModule.h" 4 //#include "Player2SamgarThread.h"5 4 #include <iostream> 6 5 … … 50 49 //return -1; 51 50 } 52 player->StartThread(); 51 player->SetDataMode(PLAYER_DATAMODE_PULL); 52 player->SetReplaceRule( 1 , PLAYER_MSGTYPE_DATA, -1 , -1) ; 53 54 //player->StartThread(); 53 55 54 56 std::list<playerc_device_info_t > devices; … … 73 75 currDriver.player_port=gPort; 74 76 75 name = currDriver.interfName; 77 name = currDriver.driverName+"-"; 78 name += currDriver.interfName; 76 79 name += "-"+itoa(currDriver.index); 77 80 … … 146 149 pthread_create(threads.back(), NULL, &Position2dThread, &(*it)); 147 150 } 148 if (it->interfName.compare("localize")==0)151 else if (it->interfName.compare("localize")==0) 149 152 { 150 153 threads.push_back(new pthread_t); … … 152 155 } 153 156 // wafewront has a little bug in its interface so there is a need of such a trick 154 if (it->interfName.compare("unknown")==0 && it->driverName.compare("wavewront"))157 else if (it->interfName.compare("unknown")==0 && it->driverName.compare("wavewront")) 155 158 { 156 159 threads.push_back(new pthread_t); 157 160 pthread_create(threads.back(), NULL, &PlannerThread, &(*it)); 158 161 } 159 if (it->interfName.compare("laser")==0)162 else if (it->interfName.compare("laser")==0) 160 163 { 161 164 threads.push_back(new pthread_t); 162 165 pthread_create(threads.back(), NULL, &LaserThread, &(*it)); 163 166 } 164 if (it->interfName.compare("map")==0)167 else if (it->interfName.compare("map")==0) 165 168 { 166 169 threads.push_back(new pthread_t); 167 170 pthread_create(threads.back(), NULL, &MapThread, &(*it)); 168 171 } 169 if (it->interfName.compare("sonar")==0)172 else if (it->interfName.compare("sonar")==0) 170 173 { 171 174 threads.push_back(new pthread_t); 172 175 pthread_create(threads.back(), NULL, &SonarThread, &(*it)); 173 176 } 174 if (it->interfName.compare("bumper")==0)177 else if (it->interfName.compare("bumper")==0) 175 178 { 176 179 threads.push_back(new pthread_t); -
libs/SAMGAR/trunk/Modules/Samgar2Player/Player2SamgarModule.h
r828 r872 13 13 14 14 15 enum PlayerProxy_TYPE {Position2d , Localize, Planner, Laser, Map, Sonar, Bumper};15 enum PlayerProxy_TYPE {Position2d=0, Localize, Planner, Laser, Map, Sonar, Bumper}; 16 16 17 17 enum PlayerProxy_FIELDS {TYPE=0, CMD}; … … 25 25 enum PlayerProxy_DATA {Error=0, Ack=1}; 26 26 27 enum PlayerProxy_Position2d_DATA {Position2d_SPEED=2, Position2d_POS ITION, Position2d_SIZE, Position2d_STALL};27 enum PlayerProxy_Position2d_DATA {Position2d_SPEED=2, Position2d_POSE, Position2d_SIZE, Position2d_STALL}; 28 28 enum PlayerProxy_Localize_DATA {Localize_MAP_INFO=2, Localize_HYPOTHS}; 29 29 enum PlayerProxy_Planner_DATA {Planner_GOAL=2, Planner_POSE, Planner_CURR_WAYPOINT, Planner_PATH, Planner_WAYPOINTS}; -
libs/SAMGAR/trunk/Modules/Samgar2Player/Player2SamgarThread.cpp
r829 r872 21 21 //return -1; 22 22 } 23 24 player->SetDataMode(PLAYER_DATAMODE_PULL); 25 player->SetReplaceRule( 1 , PLAYER_MSGTYPE_DATA, -1 , -1) ; 23 26 24 27 PlayerCc::Position2dProxy pp(player,data->index); … … 67 70 data->samgarPort->write(); 68 71 } 69 if (input->get(Position2d_POS ITION).asInt())72 if (input->get(Position2d_POSE).asInt()) 70 73 { 71 74 yarp::os::Bottle& B = data->samgarPort->prepare(); // prepare the bottle/port 72 75 B.clear(); 73 76 B.addInt(Position2d); 74 B.addInt(Position2d_POS ITION);77 B.addInt(Position2d_POSE); 75 78 B.addDouble(pp.GetXPos()); 76 79 B.addDouble(pp.GetYPos()); … … 108 111 109 112 } 113 #if DEBUG 110 114 else{ 111 115 yarp::os::Bottle& B = data->samgarPort->prepare(); // prepare the bottle/port … … 116 120 data->samgarPort->write(); 117 121 } 122 #endif 118 123 } // while(1) 119 124 return NULL; … … 138 143 //return -1; 139 144 } 145 146 player->SetDataMode(PLAYER_DATAMODE_PULL); 147 player->SetReplaceRule( 1 , PLAYER_MSGTYPE_DATA, -1 , -1) ; 140 148 141 149 PlayerCc::LocalizeProxy pp(player,data->index); … … 150 158 // read state from Player 151 159 player->Read(); 152 153 160 // perform Samgar command 154 161 if (input->get(CMD).asInt() == Localize_SET_POSE) … … 165 172 { 166 173 yarp::os::Bottle& B = data->samgarPort->prepare(); // prepare the bottle/port 174 //std::cout<<"MAP_INFO"<<std::endl; 167 175 B.clear(); 168 176 B.addInt(Localize); … … 174 182 B.addDouble(pp.GetMapScale()); 175 183 data->samgarPort->write(); 184 usleep(1000); 176 185 } 177 186 if (input->get(Localize_HYPOTHS).asInt()) 178 187 { 179 188 yarp::os::Bottle& B = data->samgarPort->prepare(); // prepare the bottle/port 189 //std::cout<<"HYPOTHS"<<std::endl; 180 190 B.clear(); 181 191 B.addInt(Localize); 182 192 B.addInt(Localize_HYPOTHS); 193 //std::cout<<pp.GetNumHypoths()<<" "<<pp.GetNumParticles()<<" "<<pp.GetParticles()<<" "<<pp.GetHypothCount()<<" "<< pp.GetPendingCount()<<std::endl; 183 194 int hypothsCount = pp.GetHypothCount(); 184 195 player_localize_hypoth_t hypoth; … … 192 203 } 193 204 data->samgarPort->write(); 205 usleep(1000); 194 206 } 195 207 } … … 203 215 204 216 } 217 #if DEBUG 205 218 else{ 206 219 yarp::os::Bottle& B = data->samgarPort->prepare(); // prepare the bottle/port … … 211 224 data->samgarPort->write(); 212 225 } 226 #endif 213 227 } // while(1) 214 228 return NULL; … … 232 246 //return -1; 233 247 } 248 249 player->SetDataMode(PLAYER_DATAMODE_PULL); 250 player->SetReplaceRule( 1 , PLAYER_MSGTYPE_DATA, -1 , -1) ; 234 251 235 252 PlayerCc::PlannerProxy pp(player,data->index); … … 244 261 // read state from Player 245 262 player->Read(); 246 247 263 // perform Samgar command 248 264 if (input->get(CMD).asInt() == Planner_SET_GOAL) … … 254 270 if (input->get(1).asInt() == Planner_SET_ENABLE) 255 271 { 256 pp.SetEnable(input->get(2).as Double());272 pp.SetEnable(input->get(2).asInt()); 257 273 } 258 274 … … 329 345 330 346 } 347 #if DEBUG 331 348 else{ 332 349 yarp::os::Bottle& B = data->samgarPort->prepare(); // prepare the bottle/port … … 337 354 data->samgarPort->write(); 338 355 } 356 #endif 339 357 } // while(1) 340 358 return NULL; … … 360 378 } 361 379 380 player->SetDataMode(PLAYER_DATAMODE_PULL); 381 player->SetReplaceRule( 1 , PLAYER_MSGTYPE_DATA, -1 , -1) ; 382 362 383 PlayerCc::LaserProxy pp(player,data->index); 363 384 … … 366 387 // read data from Samgar 367 388 yarp::os::Bottle *input = data->samgarPort->read(true); // blocking read 389 368 390 // analize data from samgar 369 391 if (input->get(TYPE).asInt() == Laser) … … 470 492 B.addInt(Laser); 471 493 B.addInt(Ack); 472 data->samgarPort->write(); 473 494 data->samgarPort->write(); 474 495 } 496 #if DEBUG 475 497 else{ 476 498 yarp::os::Bottle& B = data->samgarPort->prepare(); // prepare the bottle/port … … 481 503 data->samgarPort->write(); 482 504 } 505 #endif 483 506 } // while(1) 484 507 return NULL; … … 503 526 //return -1; 504 527 } 528 529 player->SetDataMode(PLAYER_DATAMODE_PULL); 530 player->SetReplaceRule( 1 , PLAYER_MSGTYPE_DATA, -1 , -1) ; 505 531 506 532 PlayerCc::MapProxy pp(player,data->index); … … 546 572 547 573 } 574 #if DEBUG 548 575 else{ 549 576 yarp::os::Bottle& B = data->samgarPort->prepare(); // prepare the bottle/port … … 554 581 data->samgarPort->write(); 555 582 } 583 #endif 556 584 } // while(1) 557 585 return NULL; … … 575 603 //return -1; 576 604 } 605 606 player->SetDataMode(PLAYER_DATAMODE_PULL); 607 player->SetReplaceRule( 1 , PLAYER_MSGTYPE_DATA, -1 , -1) ; 577 608 578 609 PlayerCc::SonarProxy pp(player,data->index); … … 634 665 635 666 } 667 #if DEBUG 636 668 else{ 637 669 yarp::os::Bottle& B = data->samgarPort->prepare(); // prepare the bottle/port … … 642 674 data->samgarPort->write(); 643 675 } 676 #endif 644 677 } // while(1) 645 678 return NULL; … … 663 696 //return -1; 664 697 } 698 699 player->SetDataMode(PLAYER_DATAMODE_PULL); 700 player->SetReplaceRule( 1 , PLAYER_MSGTYPE_DATA, -1 , -1) ; 665 701 666 702 PlayerCc::BumperProxy pp(player,data->index); … … 724 760 725 761 } 762 #if DEBUG 726 763 else{ 727 764 yarp::os::Bottle& B = data->samgarPort->prepare(); // prepare the bottle/port … … 732 769 data->samgarPort->write(); 733 770 } 771 #endif 734 772 } // while(1) 735 773 return NULL;
Note: See TracChangeset
for help on using the changeset viewer.