Added a watchdof in the cmd_vel topic to stop the robot when the communication is lost.

cfg/SegwayRMP200.cfg

@@ -59,5 +59,6 @@ gen.add("gain_schedule",              int_t,    SensorLevels.RECONFIGURE_STOP,
 gen.add("operation_mode",             int_t,    SensorLevels.RECONFIGURE_STOP,   "Operation mode",                       1,       1,    3, edit_method=enum_operation_mode)
 gen.add("balance_lockout",            bool_t,   SensorLevels.RECONFIGURE_STOP,   "Lock balance",                         False)
 gen.add("serial_number",              str_t,    SensorLevels.RECONFIGURE_CLOSE,  "Serial number of the robot",           "NULL")           
+gen.add("cmd_vel_watchdog",           double_t, SensorLevels.RECONFIGURE_CLOSE,  "Maximum time between cmd_vel topics",  1.0,     0.1,  2.0)           
 
 exit(gen.generate(PACKAGE, "SegwayRmp200Driver", "SegwayRMP200"))

include/segway_rmp200_driver.h

@@ -181,6 +181,7 @@ class SegwayRmp200Driver : public iri_base_driver::IriBaseDriver
     const std::list<std::string> & getSegwayEvents() const;
     void setSegwayEvents(void);
     static const std::string & getSegwayEventName(const unsigned int & ev);
+    double get_cmd_vel_watchdog(void);
 
    /**
     * \brief Destructor

include/segway_rmp200_driver_node.h

@@ -85,6 +85,12 @@ class SegwayRmp200DriverNode : public iri_base_driver::IriBaseNodeDriver<SegwayR
     */
     void postNodeOpenHook(void);
 
+    void reset_cmd_vel_watchdog(void);
+    bool cmd_vel_watchdog_active(void);
+    void update_cmd_vel_watchdog(void);
+    ros::Duration cmd_vel_watchdog;
+    CMutex cmd_vel_watchdog_access;
+
   public:
    /**
     * \brief constructor

src/segway_rmp200_driver.cpp

@@ -418,3 +418,7 @@ const std::string & SegwayRmp200Driver::getSegwayEventName(const unsigned int &
     return names[4];
 }
 
+double SegwayRmp200Driver::get_cmd_vel_watchdog(void)
+{
+  return this->config_.cmd_vel_watchdog;
+}

src/segway_rmp200_driver_node.cpp

@@ -60,6 +60,10 @@ void SegwayRmp200DriverNode::mainNodeThread(void)
       status_publisher_.publish(this->segway_status_msg_);
       break;
   }
+  // chek wether it is receiving cmd_vel topics or not
+  this->update_cmd_vel_watchdog();
+  if(this->cmd_vel_watchdog_active())
+    this->driver_.move_platform(0.0,0.0);
   
   // [fill msg Header if necessary]
 
@@ -87,6 +91,7 @@ void SegwayRmp200DriverNode::cmd_platform_callback(const geometry_msgs::Twist::C
 
   //lock access to driver if necessary 
   this->driver_.lock();
+  this->reset_cmd_vel_watchdog();
 
   if(this->driver_.isRunning())
   {
@@ -203,6 +208,40 @@ SegwayRmp200DriverNode::~SegwayRmp200DriverNode()
   //[free dynamic memory]
 }
 
+void SegwayRmp200DriverNode::reset_cmd_vel_watchdog(void)
+{
+  this->cmd_vel_watchdog_access.enter();
+  this->cmd_vel_watchdog=ros::Duration(this->driver_.get_cmd_vel_watchdog());
+  this->cmd_vel_watchdog_access.exit();
+}
+
+bool SegwayRmp200DriverNode::cmd_vel_watchdog_active(void)
+{
+  this->cmd_vel_watchdog_access.enter();
+  if(this->cmd_vel_watchdog.toSec()<=0.0)
+  {
+    this->cmd_vel_watchdog_access.exit();
+    return true;
+  }
+  else
+  {
+    this->cmd_vel_watchdog_access.exit();
+    return false;
+  }
+}
+
+void SegwayRmp200DriverNode::update_cmd_vel_watchdog(void)
+{
+  static ros::Time start_time=ros::Time::now();
+  ros::Time current_time=ros::Time::now();
+
+  this->cmd_vel_watchdog_access.enter();
+  this->cmd_vel_watchdog-=(current_time-start_time);
+  start_time=current_time;
+  this->cmd_vel_watchdog_access.exit();
+}
+
+
 /* main function */
 int main(int argc,char *argv[])
 {