Changeset 828

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:

• PROTOCOL.txt (modified) (6 diffs)
• Player2SamgarModule.cpp (modified) (1 diff)
• Player2SamgarModule.h (modified) (3 diffs)
• Player2SamgarThread.cpp (modified) (1 diff)

libs/SAMGAR/trunk/Modules/Samgar2Player/PROTOCOL.txt

@@ -9 +9 @@
 * Laser
 * Map
+* Sonar
+* Bumper
 
 Devices Identifiying constants:
@@ -19 +21 @@
         Map        = 4
         Sonar      = 5
+        Bumper     = 6
 
 PlayerProxy_FIELDS:
@@ -37 +40 @@
 // enum PlayerProxy_Map_DATA {};
 // enum PlayerProxy_Sonar_CMD {};
+// enum PlayerProxy_Bumper_CMD {};
 
 
@@ -44 +48 @@
 enum PlayerProxy_Map_DATA   {Map_MAP=2};
 enum PlayerProxy_Sonar_DATA {Sonar_POSES=2, Sonar_RANGES};
+enum PlayerProxy_Bumper_DATA {Bumper_POSES=2, Bumper_BUMPED};
 
 
@@ -471 +476 @@
 +------++---------------+---------------+---------------+
 
-Below is a list of posibble PlayerPoxy_Laser_DATA frames:
+Below is a list of posibble PlayerPoxy_Sonar_DATA frames:
 
 +------++---------------+-------+---------------+-------+---------------+
@@ -489 +494 @@
 +------++---------------+-------+---------------+-------+---------------+
 
+
+##########
+# Bumper #
+##########
+
+PlayerProxy_Bumper_CMD:
+        SET_REQ                 = 0
+
+PlayerProxy_Bumper_DATA:
+        Error                   = 0
+        Ack                     = 1
+        Bumper_POSES            = 2
+        Bumper_BUMPED           = 3
+
+Below is a list of posibble PlayerPoxy_Bumper_CMD frames:
+
++------++---------------+---------------+---------------+
+| desc || command_id    | Bumper_POSES  | Bumper_BUMPED |
++------++---------------+---------------+---------------+
+| type || SET_REQ       | bool          | bool          |
++------++---------------+---------------+---------------+
+| info ||               |               |               |
++------++---------------+---------------+---------------+
+
+Below is a list of posibble PlayerPoxy_Bumper_DATA frames:
+
++------++---------------+-------+---------------+---------------+---------------+-------+---------------+---------------+---------------+
+| desc || command_id    | count | pose[1]       | length[1]     | radius[1]     |       | pose[count]   | length[count] | radius[count] |
++------++---------------+-------+---------------+---------------+---------------+       +---------------+---------------+---------------+
+| type || Bumper_POSES  | int   | coord_type    | double        | double        | ...   | coord_type    | double        | double        |
++------++---------------+-------+---------------+---------------+---------------+       +---------------+---------------+---------------+
+| info ||               |       |               | m             | m             |       |               | m             | m             |
++------++---------------+-------+---------------+---------------+---------------+-------+---------------+---------------+---------------+
+
++------++---------------+-------+---------------+-------+---------------+
+| desc || command_id    | count | bumped[1]     |       | bumped[count] |
++------++---------------+-------+---------------+       +---------------+
+| type || Bumper_BUMPED | int   | bool          | ...   | bool          |
++------++---------------+-------+---------------+       +---------------+
+| info ||               |       |               |       | m             |
++------++---------------+-------+---------------+-------+---------------+
+

libs/SAMGAR/trunk/Modules/Samgar2Player/Player2SamgarModule.cpp

@@ -172 +172 @@
           pthread_create(threads.back(), NULL, &SonarThread, &(*it));
         }
+      if (it->interfName.compare("bumper")==0)
+        {
+          threads.push_back(new pthread_t);
+          pthread_create(threads.back(), NULL, &BumperThread, &(*it));
+        }
     }
 }

libs/SAMGAR/trunk/Modules/Samgar2Player/Player2SamgarModule.h

@@ -13 +13 @@
 
 
-enum PlayerProxy_TYPE {Position2d, Localize, Planner, Laser, Map, Sonar};
+enum PlayerProxy_TYPE {Position2d, Localize, Planner, Laser, Map, Sonar, Bumper};
 
 enum PlayerProxy_FIELDS {TYPE=0, CMD};
@@ -31 +31 @@
 enum PlayerProxy_Map_DATA   {Map_MAP=2};
 enum PlayerProxy_Sonar_DATA {Sonar_POSES=2, Sonar_RANGES};
+enum PlayerProxy_Bumper_DATA {Bumper_POSES=2, Bumper_BUMPED};
 
 class PlayerDriver_t 
@@ -94 +95 @@
 void* MapThread(void * param);
 
+/** \brief Thread for communication with Bumper Player devices
+ */
+void* BumperThread(void * param);
+
 /** \brief Thread for communication with Map Player devices
  */

libs/SAMGAR/trunk/Modules/Samgar2Player/Player2SamgarThread.cpp

@@ -645 +645 @@
   return NULL;
 }
+
+void* BumperThread(void * param){
+  bool active=true;
+  PlayerCc::PlayerClient* player;
+  PlayerDriver_t * data = static_cast< PlayerDriver_t*>(param);
+
+  // player configuration
+  try
+    {
+      // Connect to Player server
+      player = new  PlayerCc::PlayerClient(data->player_hostname, data->player_port);
+    }
+  catch (PlayerCc::PlayerError e)
+    {
+      std::cerr << e << std::endl;
+      throw e;
+      //return -1;
+    }
+
+  PlayerCc::BumperProxy pp(player,data->index);
+  
+  while(active)
+    {
+      // read data from Samgar
+      yarp::os::Bottle *input = data->samgarPort->read(true); // blocking read
+      // analize data from samgar
+      if (input->get(TYPE).asInt() == Bumper)
+        {
+          // read state from Player
+          player->Read();
+          
+          // perform Samgar command
+          if (input->get(CMD).asInt() == SET_REQ)
+            {
+              if (input->get(Bumper_POSES).asInt())
+                {
+                  pp.RequestBumperConfig();
+                  yarp::os::Bottle& B = data->samgarPort->prepare();      // prepare the bottle/port
+                  B.clear();
+                  B.addInt(Bumper);
+                  B.addInt(Bumper_POSES);
+                  int poseCount = pp.GetPoseCount();
+                  player_pose_t pose;
+                  B.addInt(poseCount);
+                  for (int i=0;i<poseCount; i++)
+                    {
+                      pose = pp.GetPose(i);
+                      B.addDouble(pose.pose.px);
+                      B.addDouble(pose.pose.py);
+                      B.addDouble(pose.pose.pa);
+                      B.addDouble(pose.length);
+                      B.addDouble(pose.radius);
+                    }
+                  data->samgarPort->write();
+                }
+              if (input->get(Bumper_BUMPED).asInt())
+                {
+                  yarp::os::Bottle& B = data->samgarPort->prepare();      // prepare the bottle/port
+                  B.clear();
+                  B.addInt(Bumper);
+                  B.addInt(Bumper_BUMPED);
+                  int bumperCount = pp.GetCount();
+                  B.addInt(bumperCount);
+                  for (int i=0;i<bumperCount; i++){
+                    B.addInt(pp.IsBumped(i));;
+                  }
+                  data->samgarPort->write();
+                }
+
+            }
+          
+          // send data to Samgar
+          yarp::os::Bottle& B = data->samgarPort->prepare();      // prepare the bottle/port
+          B.clear();
+          B.addInt(Bumper);
+          B.addInt(Ack);
+          data->samgarPort->write();
+            
+        }
+      else{
+        yarp::os::Bottle& B = data->samgarPort->prepare();        // prepare the bottle/port
+        B.clear();
+        B.addInt(Bumper);
+        B.addInt(Error);
+        B.addString("Wrong device");
+        data->samgarPort->write();
+      }
+    } // while(1)
+  return NULL;
+}