Implemented the main internal thread to handle the reception of new status...

Implemented the main internal thread to handle the reception of new status messages from both platforms and make the necessary computations.
Modified the example to use the new driver structure.

src/examples/test_segwayRMP400.cpp

 #include "segway_RMP400.h"
 #include "segway_RMP400_exception.h"
+#include "eventexceptions.h"
 #include <iostream>
 
 using namespace std;
 
 int main(int argc, char *argv[])
-{ 
-  cout << "TEST SEGWAY RMP 400 DRIVER" << endl;
-  CSegwayRMP400 segway;
-
+{
+  CEventServer *event_server=CEventServer::instance();
+  std::list<std::string> events;
+  unsigned int i=0;
+  
   try
   {
+    cout << "TEST SEGWAY RMP 400 DRIVER" << endl;
+    CSegwayRMP400 segway;
     segway.connect("00000269","00000261");
     segway.reset_integrators();
     cout << "Platform started" << endl;
-  }
-  catch (CException & e)
-  {
-    cout<< e.what().c_str()<<endl;
-    return false;
-  }
-
-  char c;
-  do{
-    cout << "Do you want to move the robot? (y/n):" << endl;
-    cin >> c;
-  }while(c!='y' && c!='n');
-  if(c=='n')
-  {
-    cout << "Press any key+intro to close the program" << endl;
-    cin >> c;
-  }else if(c=='y')
-  {
-    cout << "CAUTION!! THIS PROGRAM MOVES THE SEGWAYRMP400" << endl;
-    cout << "You need at least 4x4 meter clearance around the robot for these tests!" << endl;
-    cout << "No obstacle avoidance is implemented. If there is something in the way the robot will hit it!" << endl;
-    cout << "Moving 0.5 meters at 0.05m/sec" << endl;
     sleep(1);
+    cout << "start moving forward" << endl;
+    events.push_back(segway.get_new_status_event_id());
     segway.move(0.05,0.0); // 5cm per second 10 seconds = half a meter
-    int i;
     //for(i=0;i<10;i++)
     for(;;)
     {
-      sleep(1);
-      cout << segway << endl;
+      try{
+        event_server->wait_first(events,1000);
+        cout << segway << endl;
+      }catch(CEventTimeoutException &e){
+        std::cout << "No status data received for over a second" << std::endl;
+      }
     }
     segway.stop();
     sleep(2);
-
     cout << "Rotating 360 degrees with center of rotation on the side of the base" << endl;
-    sleep(1);
     segway.move(0.0,0.1); // one 10th rev per second, 10 seconds = 360 degrees turn 
                            // center right on the side of the base (about 54 cm from center of base)
+    for(i=0;i<10;i++)
+    {
+      sleep(1);
+      cout << segway << endl;
+    }
     segway.stop();
     sleep(2);
-
-    cout << "Rotating 360 degrees with center of rotation 1m away from the base center" << endl;
-    sleep(1);
-    segway.move(0.1,0.1); // one 10th rev per second, 10 seconds = 360 degrees turn
-                           // center of rotation vT/vR = 1meter from the center of the base
-   // tensecs(segway);
-    segway.stop();
-    sleep(2);
-
     cout << "Rotating backwards 360 degrees with center of rotation 1m away from the base center" << endl;
-    sleep(1);
     segway.move(-0.1,0.1); // one 10th rev per second, 10 seconds = 360 degrees turn BACKWARDS
-                            // center of rotation vT/vR = 1meter from the center of the base
+    for(i=0;i<10;i++)
+    {
+      sleep(1);
+      cout << segway << endl;
+    }
+  }
+  catch (CException & e)
+  {
+    cout<< e.what().c_str()<<endl;
   }
-
-  segway.stop();
-  return 0;
 }

src/segway_RMP400.cpp

@@ -9,6 +9,7 @@ CSegwayRMP400::CSegwayRMP400() :
   yaw_rate(0.0)
 {
   this->connected=false;
+  this->segways.resize(NUM_SEGWAY_200);
 }
 
 CSegwayRMP400::CSegwayRMP400(const std::string &front, const std::string &rear) :
@@ -17,12 +18,60 @@ CSegwayRMP400::CSegwayRMP400(const std::string &front, const std::string &rear)
   yaw_rate(0.0)
 {
   this->connected=false;
+  this->segways.resize(NUM_SEGWAY_200);
   this->connect(front,rear);
 }
 
 void *CSegwayRMP400::monitor_thread(void *param)
 {
   CSegwayRMP400 *segway=(CSegwayRMP400 *)param;
+  std::vector<bool> status_received(false,NUM_SEGWAY_200);
+  double fwd_displ_front,fwd_displ_rear; 
+  std::list<std::string> events;
+  int event_id=0;
+  bool end=false;
+
+  events.push_back(segway->finish_thread_event_id);
+  events.push_back(segway->segways[front_segway].get_new_status_event());
+  events.push_back(segway->segways[rear_segway].get_new_status_event());
+  while(!end)
+  {
+    event_id=segway->event_server->wait_first(events);
+    segway->status_access.enter();
+    try{
+      if(event_id==0)
+        end=true;
+      else if(event_id==1)// front status
+      {
+        status_received[front_segway]=true;
+        segway->status.rmp200[front_segway]=segway->segways[front_segway].get_status();
+      }
+      else if(event_id==2)// rear status
+      {
+        status_received[rear_segway]=true;
+        segway->status.rmp200[rear_segway]=segway->segways[rear_segway].get_status();
+      }
+      if(status_received[front_segway] && status_received[rear_segway])
+      {
+        /* compute the forward displacement  */
+        fwd_displ_front=(segway->segways[front_segway].get_right_wheel_displacement()+segway->segways[front_segway].get_left_wheel_displacement())/2;
+        fwd_displ_rear=(segway->segways[rear_segway].get_right_wheel_displacement()+segway->segways[rear_segway].get_left_wheel_displacement())/2;
+        segway->forward_displacement=(fwd_displ_front+fwd_displ_rear)/2;
+        /* compute yaw displacement*/
+        segway->yaw_displacement=(segway->segways[front_segway].get_yaw_displacement()+segway->segways[rear_segway].get_yaw_displacement())/2;
+        /* compute yaw rate */
+        segway->yaw_rate=(segway->segways[front_segway].get_yaw_rate()+segway->segways[rear_segway].get_yaw_rate())/2;
+        /* activate event */
+        if(!segway->event_server->event_is_set(segway->new_status_event_id))
+          segway->event_server->set_event(segway->new_status_event_id);
+        status_received[front_segway]=false;
+        status_received[rear_segway]=false;
+      }
+    }catch(CException &e){
+      std::cout << e.what() << std::endl;
+    }
+    segway->status_access.exit();
+  }
 
   pthread_exit(NULL);
 }
@@ -136,25 +185,31 @@ std::ostream & operator<< (std::ostream& data, CSegwayRMP400 & segway)
 
 TSegwayRMP400Status CSegwayRMP400::get_status()
 {
-  return this->status;
+  TSegwayRMP400Status status_tmp;
+
+  if(this->connected)
+  {
+    this->status_access.enter();
+    status_tmp=this->status;
+    this->status_access.exit();
+
+    return status_tmp;
+  }
+  else
+    throw CSegwayRMP400Exception(_HERE_,"The driver is not connected","");
 }
 
 double CSegwayRMP400::get_forward_displacement()
 {
-  double fwd_displ_front,fwd_displ_rear; 
-
-  // The get_forward_displacement function in segway RMP 200 doesn't
-  // return the accumulate value of displacement, wheel_displacement
-  // function do. Use them instead.
+  double fwd_disp;
 
   if(this->connected)
   {
-    fwd_displ_front=(this->segways[front_segway].get_right_wheel_displacement()+this->segways[front_segway].get_left_wheel_displacement())/2;
-    fwd_displ_rear=(this->segways[rear_segway].get_right_wheel_displacement()+this->segways[rear_segway].get_left_wheel_displacement())/2;
-
-    this->forward_displacement=(fwd_displ_front+fwd_displ_rear)/2;
+    this->status_access.enter();
+    fwd_disp=this->forward_displacement;
+    this->status_access.exit();
 
-    return this->forward_displacement;
+    return fwd_disp;
   }
   else
     throw CSegwayRMP400Exception(_HERE_,"The driver is not connected","");
@@ -162,11 +217,15 @@ double CSegwayRMP400::get_forward_displacement()
 
 double CSegwayRMP400::get_yaw_displacement()
 {
+  double yaw_disp;
+
   if(this->connected)
   {
-    this->yaw_displacement=(this->segways[front_segway].get_yaw_displacement()+this->segways[rear_segway].get_yaw_displacement())/2;
+    this->status_access.enter();
+    yaw_disp=this->yaw_displacement;
+    this->status_access.exit();
 
-    return this->yaw_displacement;
+    return yaw_disp;
   }
   else
     throw CSegwayRMP400Exception(_HERE_,"The driver is not connected","");
@@ -174,11 +233,15 @@ double CSegwayRMP400::get_yaw_displacement()
 
 double CSegwayRMP400::get_yaw_rate()
 {
+  double yaw_rate_tmp;
+
   if(this->connected)
   {
-    this->yaw_rate=(this->segways[front_segway].get_yaw_rate()+this->segways[rear_segway].get_yaw_rate())/2;
+    this->status_access.enter();
+    yaw_rate_tmp=this->yaw_rate;
+    this->status_access.exit();
 
-    return this->yaw_rate;
+    return yaw_rate_tmp;
   }
   else
     throw CSegwayRMP400Exception(_HERE_,"The driver is not connected","");