Changeset 819


Ignore:
Timestamp:
01/13/2011 01:50:51 PM (10 years ago)
Author:
lmalek
Message:

Add sonar interface to Samgar2Player module

Location:
libs/SAMGAR/trunk/Modules/Samgar2Player
Files:
1 added
4 edited

Legend:

Unmodified
Added
Removed
  • libs/SAMGAR/trunk/Modules/Samgar2Player/PROTOCOL.txt

    r796 r819  
    1818        Laser      = 3 
    1919        Map        = 4 
     20        Sonar      = 5 
    2021 
    2122PlayerProxy_FIELDS: 
     
    3435enum PlayerProxy_Planner_CMD {Planner_SET_GOAL=1, Planner_SET_ENABLE}; 
    3536enum PlayerProxy_Laser_CMD {Laser_SET_CONFIG=1}; 
     37// enum PlayerProxy_Map_DATA {}; 
     38// enum PlayerProxy_Sonar_CMD {}; 
    3639 
    3740 
     
    4043enum PlayerProxy_Laser_DATA {Laser_CONFIG=2, Laser_SIZE, Laser_POSE, Laser_ROBOT_POSE, Laser_POINTS, Laser_RANGES}; 
    4144enum PlayerProxy_Map_DATA   {Map_MAP=2}; 
     45enum PlayerProxy_Sonar_DATA {Sonar_POSES=2, Sonar_RANGES}; 
    4246 
    4347 
     
    354358 
    355359+------++---------------+---------------+---------------+---------------+-----------------------+---------------+---------------+ 
    356 | desc || command_id    | Laser_CONFIGL | Laser_SIZE    | Laser_POSE    | Laser_ROBOT_POSE      | Laser_POINTS  | Laser_RANGES  | 
     360| desc || command_id    | Laser_CONFIG  | Laser_SIZE    | Laser_POSE    | Laser_ROBOT_POSE      | Laser_POINTS  | Laser_RANGES  | 
    357361+------++---------------+---------------+---------------+---------------+-----------------------+---------------+---------------+ 
    358362| type || SET_REQ       | bool          | bool          | bool          | bool                  | bool          | bool          | 
     
    444448+------++---------------+-------+-------+---------------+---------------+---------------+-------+-----------------------+ 
    445449 
    446  
     450######### 
     451# Sonar # 
     452######### 
     453 
     454PlayerProxy_Sonar_CMD: 
     455        SET_REQ                 = 0 
     456 
     457PlayerProxy_Sonar_DATA: 
     458        Error                   = 0 
     459        Ack                     = 1 
     460        Sonar_POSES             = 2 
     461        Sonar_RANGES            = 3 
     462 
     463Below is a list of posibble PlayerPoxy_Sonar_CMD frames: 
     464 
     465+------++---------------+---------------+---------------+ 
     466| desc || command_id    | Laser_POSES   | Laser_RANGES  | 
     467+------++---------------+---------------+---------------+ 
     468| type || SET_REQ       | bool          | bool          | 
     469+------++---------------+---------------+---------------+ 
     470| info ||               |               |               | 
     471+------++---------------+---------------+---------------+ 
     472 
     473Below is a list of posibble PlayerPoxy_Laser_DATA frames: 
     474 
     475+------++---------------+-------+---------------+-------+---------------+ 
     476| desc || command_id    | count | pose[1]       |       | pose[count]   | 
     477+------++---------------+-------+---------------+       +---------------+ 
     478| type || Sonar_POSES   | int   | coord_type    | ...   | coord_type    | 
     479+------++---------------+-------+---------------+       +---------------+ 
     480| info ||               |       |               |       |               | 
     481+------++---------------+-------+---------------+-------+---------------+ 
     482 
     483+------++---------------+-------+---------------+-------+---------------+ 
     484| desc || command_id    | count | range[1]      |       | range[count]  | 
     485+------++---------------+-------+---------------+       +---------------+ 
     486| type || Sonar_RANGES  | int   | double        | ...   | double        | 
     487+------++---------------+-------+---------------+       +---------------+ 
     488| info ||               |       | m             |       | m             | 
     489+------++---------------+-------+---------------+-------+---------------+ 
     490 
  • libs/SAMGAR/trunk/Modules/Samgar2Player/Player2SamgarModule.cpp

    r798 r819  
    151151          pthread_create(threads.back(), NULL, &LocalizeThread, &(*it)); 
    152152        } 
    153       // wafewront has a little buggy interface so there is a need of such a trick 
     153      // wafewront has a little bug in its interface so there is a need of such a trick 
    154154      if (it->interfName.compare("unknown")==0 && it->driverName.compare("wavewront")) 
    155155        { 
     
    157157          pthread_create(threads.back(), NULL, &PlannerThread, &(*it)); 
    158158        } 
     159      if (it->interfName.compare("laser")==0) 
     160        { 
     161          threads.push_back(new pthread_t); 
     162          pthread_create(threads.back(), NULL, &LaserThread, &(*it)); 
     163        } 
    159164      if (it->interfName.compare("map")==0) 
    160165        { 
    161166          threads.push_back(new pthread_t); 
    162167          pthread_create(threads.back(), NULL, &MapThread, &(*it)); 
     168        } 
     169      if (it->interfName.compare("sonar")==0) 
     170        { 
     171          threads.push_back(new pthread_t); 
     172          pthread_create(threads.back(), NULL, &SonarThread, &(*it)); 
    163173        } 
    164174    } 
  • libs/SAMGAR/trunk/Modules/Samgar2Player/Player2SamgarModule.h

    r798 r819  
    1313 
    1414 
    15 enum PlayerProxy_TYPE {Position2d, Localize, Planner, Laser, Map}; 
     15enum PlayerProxy_TYPE {Position2d, Localize, Planner, Laser, Map, Sonar}; 
    1616 
    1717enum PlayerProxy_FIELDS {TYPE=0, CMD}; 
     
    3030enum PlayerProxy_Laser_DATA {Laser_CONFIG=2, Laser_SIZE, Laser_POSE, Laser_ROBOT_POSE, Laser_POINTS, Laser_RANGES}; 
    3131enum PlayerProxy_Map_DATA   {Map_MAP=2}; 
     32enum PlayerProxy_Sonar_DATA {Sonar_POSES=2, Sonar_RANGES}; 
    3233 
    3334class PlayerDriver_t  
     
    9394void* MapThread(void * param); 
    9495 
     96/** \brief Thread for communication with Map Player devices 
     97 */ 
     98void* SonarThread(void * param); 
    9599 
    96100#endif 
  • libs/SAMGAR/trunk/Modules/Samgar2Player/Player2SamgarThread.cpp

    r798 r819  
    557557  return NULL; 
    558558} 
     559 
     560void* SonarThread(void * param){ 
     561  bool active=true; 
     562  PlayerCc::PlayerClient* player; 
     563  PlayerDriver_t * data = static_cast< PlayerDriver_t*>(param); 
     564 
     565  // player configuration 
     566  try 
     567    { 
     568      // Connect to Player server 
     569      player = new  PlayerCc::PlayerClient(data->player_hostname, data->player_port); 
     570    } 
     571  catch (PlayerCc::PlayerError e) 
     572    { 
     573      std::cerr << e << std::endl; 
     574      throw e; 
     575      //return -1; 
     576    } 
     577 
     578  PlayerCc::SonarProxy pp(player,data->index); 
     579   
     580  while(active) 
     581    { 
     582      // read data from Samgar 
     583      yarp::os::Bottle *input = data->samgarPort->read(true); // blocking read 
     584      // analize data from samgar 
     585      if (input->get(TYPE).asInt() == Laser) 
     586        { 
     587          // read state from Player 
     588          player->Read(); 
     589           
     590          // perform Samgar command 
     591          if (input->get(CMD).asInt() == SET_REQ) 
     592            { 
     593              if (input->get(Sonar_POSES).asInt()) 
     594                { 
     595                  pp.RequestGeom(); 
     596                  yarp::os::Bottle& B = data->samgarPort->prepare();      // prepare the bottle/port 
     597                  B.clear(); 
     598                  B.addInt(Sonar); 
     599                  B.addInt(Sonar_POSES); 
     600                  int poseCount = pp.GetPoseCount(); 
     601                  player_pose_t pose; 
     602                  B.addInt(poseCount); 
     603                  for (int i=0;i<poseCount; i++) 
     604                    { 
     605                      pose = pp.GetPose(i); 
     606                      B.addDouble(pose.px); 
     607                      B.addDouble(pose.py); 
     608                      B.addDouble(pose.pa); 
     609                    } 
     610                  data->samgarPort->write(); 
     611                } 
     612              if (input->get(Sonar_RANGES).asInt()) 
     613                { 
     614                  yarp::os::Bottle& B = data->samgarPort->prepare();      // prepare the bottle/port 
     615                  B.clear(); 
     616                  B.addInt(Sonar); 
     617                  B.addInt(Sonar_RANGES); 
     618                  int rangeCount = pp.GetCount(); 
     619                  B.addInt(rangeCount); 
     620                  for (int i=0;i<rangeCount; i++){ 
     621                    B.addDouble(pp.GetScan(i));; 
     622                  } 
     623                  data->samgarPort->write(); 
     624                } 
     625 
     626            } 
     627           
     628          // send data to Samgar 
     629          yarp::os::Bottle& B = data->samgarPort->prepare();      // prepare the bottle/port 
     630          B.clear(); 
     631          B.addInt(Sonar); 
     632          B.addInt(Ack); 
     633          data->samgarPort->write(); 
     634             
     635        } 
     636      else{ 
     637        yarp::os::Bottle& B = data->samgarPort->prepare();        // prepare the bottle/port 
     638        B.clear(); 
     639        B.addInt(Sonar); 
     640        B.addInt(Error); 
     641        B.addString("Wrong device"); 
     642        data->samgarPort->write(); 
     643      } 
     644    } // while(1) 
     645  return NULL; 
     646} 
Note: See TracChangeset for help on using the changeset viewer.