Removed some exceptions specific to a single error.

Exported the Segway RMP200 events to this driver.
Added a new event to notify the availability of the two status structures.

src/examples/test_segwayRMP400.cpp

@@ -7,13 +7,12 @@ using namespace std;
 int main(int argc, char *argv[])
 { 
   cout << "TEST SEGWAY RMP 400 DRIVER" << endl;
-  CSegwayRMP400 *segway;
+  CSegwayRMP400 segway;
 
-  segway=new CSegwayRMP400();
   try
   {
-    segway->start();
-    segway->reset_integrators();
+    segway.connect("00000269","00000261");
+    segway.reset_integrators();
     cout << "Platform started" << endl;
   }
   catch (CException & e)
@@ -38,39 +37,38 @@ int main(int argc, char *argv[])
     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);
-    segway->move(0.05,0.0); // 5cm per second 10 seconds = half a meter
+    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;
+      cout << segway << endl;
     }
-    segway->stop();
+    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 
+    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)
-    segway->stop();
+    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
+    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();
+    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
+    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
   }
 
-  segway->stop();
-  delete segway;
+  segway.stop();
   return 0;
 }

src/segway_RMP400.cpp

 #include "segway_RMP400.h"
 #include "segway_RMP400_exception.h"
-#include <iridrivers/segway_rmp200_exceptions.h>
+#include "segway_rmp200_exceptions.h"
 #include <cmath>
 
 CSegwayRMP400::CSegwayRMP400() :
-    status_(rmp400_off),
-    forward_displacement_(0.0),
-    yaw_displacement_(0.0),
-    yaw_rate_ (0.0),
-    wheel_radius_(default_wheel_radius_),
-    use_correction_wheel_factor_(true)
+  forward_displacement(0.0),
+  yaw_displacement(0.0),
+  yaw_rate(0.0)
 {
-    scan_ftdi_for_segways();
-    for (unsigned int i = 0;i < NUM_SEGWAY_200 ;i++) {
-      CSegwayRMP200 * segway = new CSegwayRMP200();
-      segways_.push_back(segway);
-    }
+  this->connected=false;
 }
 
-CSegwayRMP400::CSegwayRMP400(const std::string serial1, const std::string serial2) :
-    status_(rmp400_off),
-    forward_displacement_(0.0),
-    yaw_displacement_(0.0),
-    yaw_rate_ (0.0),
-    wheel_radius_(default_wheel_radius_),
-    use_correction_wheel_factor_(true)
+CSegwayRMP400::CSegwayRMP400(const std::string &front, const std::string &rear) :
+  forward_displacement(0.0),
+  yaw_displacement(0.0),
+  yaw_rate(0.0)
 {
-    if (serial1 == serial2)
-        throw RMP400IdemSerialException(_HERE_, serial1);
-
-    scan_ftdi_for_segways();
-
-    for (unsigned int i = 0; i < serial_ftdi_segway_devices_.size(); i++) {
-        std::string serial = serial_ftdi_segway_devices_[i];
-
-        if ((serial != serial1) && (serial != serial2))
-            throw RMP400NotFoundSerialException(_HERE_, serial);
-    }
+  this->connected=false;
+  this->connect(front,rear);
 }
 
-void
-CSegwayRMP400::scan_ftdi_for_segways()
+void *CSegwayRMP400::monitor_thread(void *param)
 {
-    CFTDIServer * ftdi_server_ = CFTDIServer::instance();
-    ftdi_server_->add_custom_PID(CSegwayRMP200::pid);
-
-    std::vector<int> ftdi_devs = ftdi_server_->get_ids_by_description(CSegwayRMP200::description);
-
-    for (unsigned int i = 0;i < ftdi_devs.size();i++) {
-        serial_ftdi_segway_devices_.push_back(
-                  ftdi_server_->get_serial_number(ftdi_devs.at(i)));
-    }
-
-    if (serial_ftdi_segway_devices_.size() != NUM_SEGWAY_200)
-        throw RMP400NotValidNumberRMP200Exception(_HERE_, serial_ftdi_segway_devices_.size());
-}
+  CSegwayRMP400 *segway=(CSegwayRMP400 *)param;
 
-void
-CSegwayRMP400::start()
-{
-  for (unsigned int i = 0;i < segways_.size();i++) {
-      try
-      {
-        segways_[i]->connect(serial_ftdi_segway_devices_[i]);
-      }
-      catch (CSegwayRMP200Exception & e)
-      {
-        // Something failed while starting engines
-        // clean up previous (if any) started engine
-        for (unsigned int i = 0; i < segways_.size(); i++) {
-            segways_[i]->close();
-        }
-
-        status_ = rmp400_off;
-        throw;
-      }
-    }
-    
-    status_ = rmp400_connected;
+  pthread_exit(NULL);
 }
 
-void
-CSegwayRMP400::close()
+void CSegwayRMP400::init_events_threads(void)
 {
-  for (unsigned int i = 0;i < segways_.size();i++) {
-    segways_[i]->close();
-  }
+  std::string id="segway_"+this->front_serial+"_"+this->rear_serial;
+
+  /* initialize threads */
+  this->thread_server=CThreadServer::instance();
+  this->monitor_thread_id=id+"_monitor_thread";
+  this->thread_server->create_thread(this->monitor_thread_id);
+  this->thread_server->attach_thread(this->monitor_thread_id,this->monitor_thread,this);
+  /* initialize events */
+  this->event_server=CEventServer::instance();
+  this->finish_thread_event_id=id+"_finish_thread_event";
+  this->event_server->create_event(this->finish_thread_event_id);
+  this->front_cable_disconnect_event_id=id+"_front_disconnect_event";
+  this->event_server->create_event(this->front_cable_disconnect_event_id);
+  this->rear_cable_disconnect_event_id=id+"_rear_disconnect_event";
+  this->event_server->create_event(this->rear_cable_disconnect_event_id);
+  this->front_power_off_event_id=id+"_front_power_off_event";
+  this->event_server->create_event(this->front_power_off_event_id);
+  this->rear_power_off_event_id=id+"_rear_power_off_event";
+  this->event_server->create_event(this->rear_power_off_event_id);
+  this->front_no_heartbeat_event_id=id+"_front_no_heartbeat_event";
+  this->event_server->create_event(this->front_no_heartbeat_event_id);
+  this->rear_no_heartbeat_event_id=id+"_rear_no_heartbeat_event";
+  this->event_server->create_event(this->rear_no_heartbeat_event_id);
+  this->new_status_event_id=id+"_new_status_event";
+  this->event_server->create_event(this->new_status_event_id);
+  // start the internal thread
+  this->thread_server->start_thread(this->monitor_thread_id);
 }
 
-std::ostream &
-operator<< (std::ostream& data, CSegwayRMP400 & segway)
+void CSegwayRMP400::connect(const std::string &front, const std::string &rear)
 {
-  for (unsigned int i=0; i<segway.segways_.size(); i++)
+  if(!this->connected)
   {
-    data << "Segway unit: "              << i << std::endl;
-    data << "Pitch angle: "              << segway.segways_[i]->get_pitch_angle() << " degrees" << std::endl;
-    data << "Pitch rate "                << segway.segways_[i]->get_pitch_rate() << " degrees/s" << std::endl;
-    data << "Roll angle: "               << segway.segways_[i]->get_roll_angle() << " degrees" << std::endl;
-    data << "Roll rate: "                << segway.segways_[i]->get_roll_rate() << " degrees/s" << std::endl;
-    data << "Left wheel velocity: "      << segway.segways_[i]->get_left_wheel_velocity() << " m/s" << std::endl;
-    data << "Right wheel velocity: "     << segway.segways_[i]->get_right_wheel_velocity() << " m/s" << std::endl;
-    data << "Yaw rate: "                 << segway.segways_[i]->get_yaw_rate() << " degrees/s" << std::endl;
-    data << "Up time: "                  << segway.segways_[i]->get_uptime() << " s" << std::endl;
-    data << "Left wheel displacement: "  << segway.segways_[i]->get_left_wheel_displacement() << " m" << std::endl;
-    data << "Right wheel displacement: " << segway.segways_[i]->get_right_wheel_displacement() << " m" << std::endl;
-    data << "Forward displacement: "     << segway.segways_[i]->get_forward_displacement() << " m" << std::endl;
-    data << "Yaw displacement: "         << segway.segways_[i]->get_yaw_displacement() << " rev" << std::endl;
-    data << "Left motor torque: "        << segway.segways_[i]->get_left_motor_torque() << " Nm" << std::endl;
-    data << "Right motor torque: "       << segway.segways_[i]->get_right_motor_torque() << " Nm" << std::endl;
-    if(segway.segways_[i]->get_operation_mode()==tractor)
-      data << "Operation mode: tractor" << std::endl;
-    else
-      data << "Operation mode: power down" << std::endl;
-    if(segway.segways_[i]->get_gain_schedule()==light)
-      data << "Gain schedule: light" << std::endl;
-    else if(segway.segways_[i]->get_gain_schedule()==tall)
-      data << "Gain schedule: tall" << std::endl;
+    if(front==rear)
+      throw CSegwayRMP400Exception(_HERE_,"Rear and front serial number can't be the same",front);
     else
-      data << "Gain schedule: heavy" << std::endl;
-    data << "UI battery voltage: "        << segway.segways_[i]->get_ui_battery_voltage() << " V" << std::endl;
-    data << "Powerbase battery voltage: " << segway.segways_[i]->get_powerbase_battery_voltage() << " V" << std::endl << std::endl;
-  }
-  data << std::endl;
-
-  return data;
-}
-
-void
-CSegwayRMP400::set_operation_mode(op_mode mode)
-{
-    if (mode == balance)
-        throw RMP400InvalidBalanceModeOperation(_HERE_);
-
-    for (unsigned int i=0;i < segways_.size();i++)
     {
-      segways_[i]->set_operation_mode(mode);
-      usleep(10000);
+      try{
+        this->segways[front_segway].connect(front);
+        this->front_serial=front;
+        this->segways[rear_segway].connect(rear);
+        this->rear_serial=rear;
+        this->init_events_threads();
+        this->connected=true;
+      }catch(CException &e){
+        this->close();
+        throw e;
+      }
     }
+  }
+  else
+    throw CSegwayRMP400Exception(_HERE_,"The driver is already connected",front+"_"+rear);
 }
 
-void
-CSegwayRMP400::need_to_be_connected() const
+void CSegwayRMP400::close()
 {
-    if (! is_connected())
-        throw RMP400NoConnected(_HERE_);
+  // signal the thread to end
+  if(this->thread_server->get_thread_state(this->monitor_thread_id)==starting ||
+     this->thread_server->get_thread_state(this->monitor_thread_id)==active)
+  {
+    this->event_server->set_event(this->finish_thread_event_id);
+    this->thread_server->end_thread(this->monitor_thread_id);
+    this->thread_server->detach_thread(this->monitor_thread_id);
+    this->thread_server->delete_thread(this->monitor_thread_id);
+    this->monitor_thread_id.clear();
+    this->event_server->delete_event(this->finish_thread_event_id);
+    this->finish_thread_event_id.clear();
+    this->event_server->delete_event(this->front_cable_disconnect_event_id);
+    this->front_cable_disconnect_event_id.clear();
+    this->event_server->delete_event(this->rear_cable_disconnect_event_id);
+    this->rear_cable_disconnect_event_id.clear();
+    this->event_server->delete_event(this->front_power_off_event_id);
+    this->front_power_off_event_id.clear();
+    this->event_server->delete_event(this->rear_power_off_event_id);
+    this->rear_power_off_event_id.clear();
+    this->event_server->delete_event(this->front_no_heartbeat_event_id);
+    this->front_no_heartbeat_event_id.clear();
+    this->event_server->delete_event(this->rear_no_heartbeat_event_id);
+    this->rear_no_heartbeat_event_id.clear();
+    this->event_server->delete_event(this->new_status_event_id);
+    this->new_status_event_id.clear();
+  }
+  if(this->front_serial.size()>0) 
+  {
+    this->segways[front_segway].close();
+    this->front_serial.clear();
+  }
+  if(this->rear_serial.size()>0)
+  {
+    this->segways[rear_segway].close();
+    this->rear_serial.clear();
+  }
+  this->connected=false;
 }
 
-void
-CSegwayRMP400::disable_corretion_wheel_factor()
+std::ostream & operator<< (std::ostream& data, CSegwayRMP400 & segway)
 {
-    use_correction_wheel_factor_ = false;
-}
+  data << "Front segway status data:" << std::endl;
+  data << segway.segways[front_segway] << std::endl;
+  data << "Rear segway status data:" << std::endl;
+  data << segway.segways[rear_segway] << std::endl;
 
-void
-CSegwayRMP400::fix_with_wheel_factor(float & value)
-{
-   /* corretion factor is the diference between real wheel and supposed
-    * firmware wheel (probaby segway using the rmp200 value) */
-    value = value * (wheel_radius_ / RMP200_DEFAULT_WHEEL_RADIUS);
-}
-
-void
-CSegwayRMP400::set_wheel_radius(const float radius)
-{
-    wheel_radius_ = radius;
+  return data;
 }
 
-TSegwayRMP400Status
-CSegwayRMP400::get_status()
+TSegwayRMP400Status CSegwayRMP400::get_status()
 {
-    TSegwayRMP400Status status;
-    status.rmp200[0] = segways_[0]->get_status();
-    status.rmp200[1] = segways_[1]->get_status();
-
-    return status;
+  return this->status;
 }
 
-float
-CSegwayRMP400::get_forward_displacement()
+double CSegwayRMP400::get_forward_displacement()
 {
-    need_to_be_connected();
-
-    // 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_displ_front,fwd_displ_rear; 
 
-    double fwd_displ_segway0 = (segways_[0]->get_right_wheel_displacement() +
-                               segways_[0]->get_left_wheel_displacement()) / 2;
-    double fwd_displ_segway1 = (segways_[1]->get_right_wheel_displacement() +
-                               segways_[1]->get_left_wheel_displacement()) / 2;
+  // The get_forward_displacement function in segway RMP 200 doesn't
+  // return the accumulate value of displacement, wheel_displacement
+  // function do. Use them instead.
 
-    forward_displacement_ = (fwd_displ_segway0 + fwd_displ_segway1) / 2;
+  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;
 
-    if (use_correction_wheel_factor_)
-        fix_with_wheel_factor(forward_displacement_);
+    this->forward_displacement=(fwd_displ_front+fwd_displ_rear)/2;
 
-    return forward_displacement_;
+    return this->forward_displacement;
+  }
+  else
+    throw CSegwayRMP400Exception(_HERE_,"The driver is not connected","");
 }
 
-float
-CSegwayRMP400::get_yaw_displacement()
+double CSegwayRMP400::get_yaw_displacement()
 {
-    need_to_be_connected();
-
-    float  yaw_displacement_tmp = (segways_[0]->get_yaw_displacement() +
-                                   segways_[1]->get_yaw_displacement()) / 2;
-
-    if (use_correction_wheel_factor_)
-        fix_with_wheel_factor(yaw_displacement_tmp);
+  if(this->connected)
+  {
+    this->yaw_displacement=(this->segways[front_segway].get_yaw_displacement()+this->segways[rear_segway].get_yaw_displacement())/2;
 
-    yaw_displacement_ = yaw_displacement_tmp;
-    return yaw_displacement_;
+    return this->yaw_displacement;
+  }
+  else
+    throw CSegwayRMP400Exception(_HERE_,"The driver is not connected","");
 }
 
-float
-CSegwayRMP400::get_yaw_rate()
+double CSegwayRMP400::get_yaw_rate()
 {
-    need_to_be_connected();
-
-    yaw_rate_ = (segways_[0]->get_yaw_rate() +
-                 segways_[1]->get_yaw_rate()) / 2;
+  if(this->connected)
+  {
+    this->yaw_rate=(this->segways[front_segway].get_yaw_rate()+this->segways[rear_segway].get_yaw_rate())/2;
 
-    return yaw_rate_;
+    return this->yaw_rate;
+  }
+  else
+    throw CSegwayRMP400Exception(_HERE_,"The driver is not connected","");
 }
 
-void
-CSegwayRMP400::reset_integrators()
+void CSegwayRMP400::reset_integrators()
 {
-    need_to_be_connected();
+  double fd=0.0,yd=0.0;
+  unsigned int i=0;
+  bool reset=false;
 
+  if(this->connected)
+  {
     do {
-      for (unsigned int i=0; i < segways_.size(); i++) {
-          segways_[i]->reset_right_wheel_integrator();
-          segways_[i]->reset_left_wheel_integrator();
-          segways_[i]->reset_yaw_integrator();
-          segways_[i]->reset_forward_integrator();
+      for(i=0;i<this->segways.size();i++)
+      {
+        this->segways[i].reset_right_wheel_integrator();
+        this->segways[i].reset_left_wheel_integrator();
+        this->segways[i].reset_yaw_integrator();
+        this->segways[i].reset_forward_integrator();
       }
       // Need to leave the segwayrmp 200 driver to read need values
       sleep(1);
-    } while (! is_valid_reset_integrators());
+      fd=this->get_forward_displacement();
+      yd=this->get_yaw_displacement();
+      if(std::fabs(fd)<0.1 && std::fabs(yd)<0.1)
+        reset=true;
+      else
+        reset=false;
+    }while(!reset);
+  }
+  else
+    throw CSegwayRMP400Exception(_HERE_,"The driver is not connected","");
 }
 
-bool
-CSegwayRMP400::is_valid_reset_integrators()
+std::string CSegwayRMP400::get_front_cable_disconnected_event_id(void)
 {
-    float fd = get_forward_displacement();
-    float yd = get_yaw_displacement();
+  return this->front_cable_disconnect_event_id;
+}
 
-    if (std::fabs(0.0 - fd) > 0.1) {
-        std::cerr << "[segway] Invalid forward reset value "  << fd << std::endl;
-        return false;
-    }
+std::string CSegwayRMP400::get_rear_cable_disconnected_event_id(void)
+{
+  return this->rear_cable_disconnect_event_id;
+}
 
-    if (std::fabs(0.0 - yd) > 0.1) {
-        std::cerr << "[segway] Invalid yaw reset value "  << yd << std::endl;
-        return false;
-    }
+std::string CSegwayRMP400::get_front_power_off_event_id(void)
+{
+  return this->front_power_off_event_id;
+}
 
-    return true;
+std::string CSegwayRMP400::get_rear_power_off_event_id(void)
+{
+  return this->rear_power_off_event_id;
 }
 
-void
-CSegwayRMP400::move(float vT, float vR)
+std::string CSegwayRMP400::get_front_no_heartbeat_event_id(void)
 {
-    need_to_be_connected();
+  return this->front_no_heartbeat_event_id;
+}
 
-    for (unsigned int i=0 ;i < segways_.size(); i++)
-        segways_[i]->move(vT,vR);
+std::string CSegwayRMP400::get_rear_no_heartbeat_event_id(void)
+{
+  return this->rear_no_heartbeat_event_id;
 }
 
-void
-CSegwayRMP400::stop()
+std::string CSegwayRMP400::get_new_status_event_id(void)
 {
-    need_to_be_connected();
+  return this->new_status_event_id;
+}
 
-    for (unsigned int i=0;i < segways_.size();i++)
-        segways_[i]->stop();
+void CSegwayRMP400::move(double vT, double vR)
+{
+  if(this->connected)
+  {
+    this->segways[front_segway].move(vT,vR);
+    this->segways[rear_segway].move(vT,vR);
+  }
+  else
+    throw CSegwayRMP400Exception(_HERE_,"The driver is not connected","");
+}
 
-    //status_ = rmp400_off;
+void CSegwayRMP400::stop()
+{
+  if(this->connected)
+  {
+    this->segways[front_segway].stop();
+    this->segways[rear_segway].stop();
+  }
+  else
+    throw CSegwayRMP400Exception(_HERE_,"The driver is not connected","");
 }
 
 CSegwayRMP400::~CSegwayRMP400()
 {
-    for (unsigned int i=0;i < segways_.size(); i++)
-        delete segways_[i];
+  this->close();
 }

src/segway_RMP400.h

 #ifndef _SEGWAYRMP400_H
 #define _SEGWAYRMP400_H
 
-#include <segway_rmp200.h>
-#include <ftdiserver.h>
+#include "segway_rmp200.h"
+#include "ftdiserver.h"
+#include "threadserver.h"
+#include "eventserver.h"
+#include "mutex.h"
 #include <vector>
 #include <string>
 #include <iostream>
 
-#define RMP400_b 0.54 // distance between weels for one rmp unit in meters
-#define RMP200_DEFAULT_WHEEL_RADIUS 0.265
-
 const unsigned int NUM_SEGWAY_200 = 2;
 
-enum SegwayRMP400_status { rmp400_off, rmp400_connected };
+typedef enum {front_segway=0,rear_segway} segway_id_t;
 
 struct TSegwayRMP400Status
 {
-    TSegwayRMP200Status rmp200[2];
+  TSegwayRMP200Status rmp200[NUM_SEGWAY_200];
 };
 
 class CSegwayRMP400
 {
   private:
-    std::vector<CSegwayRMP200 *> segways_;
-    /* vector of valid segway serials found in the ftdi bus */
-    std::vector<std::string> serial_ftdi_segway_devices_;
-
-    /**
-     * \brief Segway status
-     */
-    SegwayRMP400_status status_;
-
+    std::vector<CSegwayRMP200> segways;
     /**
      * \brief forward displacement
      *
      * This value has the displacement of the whole platform in meters
      * TODO: should be a thread periodically updating this value
      */
-    float forward_displacement_;