Changeset 828


Ignore:
Timestamp:
01/14/2011 01:14:08 PM (10 years ago)
Author:
lmalek
Message:

Add bumper interface to Samgar2Player module

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

Legend:

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

    r819 r828  
    99* Laser 
    1010* Map 
     11* Sonar 
     12* Bumper 
    1113 
    1214Devices Identifiying constants: 
     
    1921        Map        = 4 
    2022        Sonar      = 5 
     23        Bumper     = 6 
    2124 
    2225PlayerProxy_FIELDS: 
     
    3740// enum PlayerProxy_Map_DATA {}; 
    3841// enum PlayerProxy_Sonar_CMD {}; 
     42// enum PlayerProxy_Bumper_CMD {}; 
    3943 
    4044 
     
    4448enum PlayerProxy_Map_DATA   {Map_MAP=2}; 
    4549enum PlayerProxy_Sonar_DATA {Sonar_POSES=2, Sonar_RANGES}; 
     50enum PlayerProxy_Bumper_DATA {Bumper_POSES=2, Bumper_BUMPED}; 
    4651 
    4752 
     
    471476+------++---------------+---------------+---------------+ 
    472477 
    473 Below is a list of posibble PlayerPoxy_Laser_DATA frames: 
     478Below is a list of posibble PlayerPoxy_Sonar_DATA frames: 
    474479 
    475480+------++---------------+-------+---------------+-------+---------------+ 
     
    489494+------++---------------+-------+---------------+-------+---------------+ 
    490495 
     496 
     497########## 
     498# Bumper # 
     499########## 
     500 
     501PlayerProxy_Bumper_CMD: 
     502        SET_REQ                 = 0 
     503 
     504PlayerProxy_Bumper_DATA: 
     505        Error                   = 0 
     506        Ack                     = 1 
     507        Bumper_POSES            = 2 
     508        Bumper_BUMPED           = 3 
     509 
     510Below is a list of posibble PlayerPoxy_Bumper_CMD frames: 
     511 
     512+------++---------------+---------------+---------------+ 
     513| desc || command_id    | Bumper_POSES  | Bumper_BUMPED | 
     514+------++---------------+---------------+---------------+ 
     515| type || SET_REQ       | bool          | bool          | 
     516+------++---------------+---------------+---------------+ 
     517| info ||               |               |               | 
     518+------++---------------+---------------+---------------+ 
     519 
     520Below is a list of posibble PlayerPoxy_Bumper_DATA frames: 
     521 
     522+------++---------------+-------+---------------+---------------+---------------+-------+---------------+---------------+---------------+ 
     523| desc || command_id    | count | pose[1]       | length[1]     | radius[1]     |       | pose[count]   | length[count] | radius[count] | 
     524+------++---------------+-------+---------------+---------------+---------------+       +---------------+---------------+---------------+ 
     525| type || Bumper_POSES  | int   | coord_type    | double        | double        | ...   | coord_type    | double        | double        | 
     526+------++---------------+-------+---------------+---------------+---------------+       +---------------+---------------+---------------+ 
     527| info ||               |       |               | m             | m             |       |               | m             | m             | 
     528+------++---------------+-------+---------------+---------------+---------------+-------+---------------+---------------+---------------+ 
     529 
     530+------++---------------+-------+---------------+-------+---------------+ 
     531| desc || command_id    | count | bumped[1]     |       | bumped[count] | 
     532+------++---------------+-------+---------------+       +---------------+ 
     533| type || Bumper_BUMPED | int   | bool          | ...   | bool          | 
     534+------++---------------+-------+---------------+       +---------------+ 
     535| info ||               |       |               |       | m             | 
     536+------++---------------+-------+---------------+-------+---------------+ 
     537 
  • libs/SAMGAR/trunk/Modules/Samgar2Player/Player2SamgarModule.cpp

    r819 r828  
    172172          pthread_create(threads.back(), NULL, &SonarThread, &(*it)); 
    173173        } 
     174      if (it->interfName.compare("bumper")==0) 
     175        { 
     176          threads.push_back(new pthread_t); 
     177          pthread_create(threads.back(), NULL, &BumperThread, &(*it)); 
     178        } 
    174179    } 
    175180} 
  • libs/SAMGAR/trunk/Modules/Samgar2Player/Player2SamgarModule.h

    r819 r828  
    1313 
    1414 
    15 enum PlayerProxy_TYPE {Position2d, Localize, Planner, Laser, Map, Sonar}; 
     15enum PlayerProxy_TYPE {Position2d, Localize, Planner, Laser, Map, Sonar, Bumper}; 
    1616 
    1717enum PlayerProxy_FIELDS {TYPE=0, CMD}; 
     
    3131enum PlayerProxy_Map_DATA   {Map_MAP=2}; 
    3232enum PlayerProxy_Sonar_DATA {Sonar_POSES=2, Sonar_RANGES}; 
     33enum PlayerProxy_Bumper_DATA {Bumper_POSES=2, Bumper_BUMPED}; 
    3334 
    3435class PlayerDriver_t  
     
    9495void* MapThread(void * param); 
    9596 
     97/** \brief Thread for communication with Bumper Player devices 
     98 */ 
     99void* BumperThread(void * param); 
     100 
    96101/** \brief Thread for communication with Map Player devices 
    97102 */ 
  • libs/SAMGAR/trunk/Modules/Samgar2Player/Player2SamgarThread.cpp

    r819 r828  
    645645  return NULL; 
    646646} 
     647 
     648void* BumperThread(void * param){ 
     649  bool active=true; 
     650  PlayerCc::PlayerClient* player; 
     651  PlayerDriver_t * data = static_cast< PlayerDriver_t*>(param); 
     652 
     653  // player configuration 
     654  try 
     655    { 
     656      // Connect to Player server 
     657      player = new  PlayerCc::PlayerClient(data->player_hostname, data->player_port); 
     658    } 
     659  catch (PlayerCc::PlayerError e) 
     660    { 
     661      std::cerr << e << std::endl; 
     662      throw e; 
     663      //return -1; 
     664    } 
     665 
     666  PlayerCc::BumperProxy pp(player,data->index); 
     667   
     668  while(active) 
     669    { 
     670      // read data from Samgar 
     671      yarp::os::Bottle *input = data->samgarPort->read(true); // blocking read 
     672      // analize data from samgar 
     673      if (input->get(TYPE).asInt() == Bumper) 
     674        { 
     675          // read state from Player 
     676          player->Read(); 
     677           
     678          // perform Samgar command 
     679          if (input->get(CMD).asInt() == SET_REQ) 
     680            { 
     681              if (input->get(Bumper_POSES).asInt()) 
     682                { 
     683                  pp.RequestBumperConfig(); 
     684                  yarp::os::Bottle& B = data->samgarPort->prepare();      // prepare the bottle/port 
     685                  B.clear(); 
     686                  B.addInt(Bumper); 
     687                  B.addInt(Bumper_POSES); 
     688                  int poseCount = pp.GetPoseCount(); 
     689                  player_pose_t pose; 
     690                  B.addInt(poseCount); 
     691                  for (int i=0;i<poseCount; i++) 
     692                    { 
     693                      pose = pp.GetPose(i); 
     694                      B.addDouble(pose.pose.px); 
     695                      B.addDouble(pose.pose.py); 
     696                      B.addDouble(pose.pose.pa); 
     697                      B.addDouble(pose.length); 
     698                      B.addDouble(pose.radius); 
     699                    } 
     700                  data->samgarPort->write(); 
     701                } 
     702              if (input->get(Bumper_BUMPED).asInt()) 
     703                { 
     704                  yarp::os::Bottle& B = data->samgarPort->prepare();      // prepare the bottle/port 
     705                  B.clear(); 
     706                  B.addInt(Bumper); 
     707                  B.addInt(Bumper_BUMPED); 
     708                  int bumperCount = pp.GetCount(); 
     709                  B.addInt(bumperCount); 
     710                  for (int i=0;i<bumperCount; i++){ 
     711                    B.addInt(pp.IsBumped(i));; 
     712                  } 
     713                  data->samgarPort->write(); 
     714                } 
     715 
     716            } 
     717           
     718          // send data to Samgar 
     719          yarp::os::Bottle& B = data->samgarPort->prepare();      // prepare the bottle/port 
     720          B.clear(); 
     721          B.addInt(Bumper); 
     722          B.addInt(Ack); 
     723          data->samgarPort->write(); 
     724             
     725        } 
     726      else{ 
     727        yarp::os::Bottle& B = data->samgarPort->prepare();        // prepare the bottle/port 
     728        B.clear(); 
     729        B.addInt(Bumper); 
     730        B.addInt(Error); 
     731        B.addString("Wrong device"); 
     732        data->samgarPort->write(); 
     733      } 
     734    } // while(1) 
     735  return NULL; 
     736} 
Note: See TracChangeset for help on using the changeset viewer.