iri_segway_rmp200: added reset_integrators function

CMakeLists.txt

@@ -110,6 +110,8 @@ add_executable(${PROJECT_NAME} src/segway_rmp200_driver.cpp  src/segway_rmp200_d
 ## Add cmake target dependencies of the executable/library
 ## as an example, message headers may need to be generated before nodes
 # add_dependencies(foo_node foo_generate_messages_cpp)
+
+# add_dependencies(${PROJECT_NAME} <msg_package_name>_generate_messages_cpp)
 add_dependencies(${PROJECT_NAME} iri_segway_rmp_msgs_generate_messages_cpp)
 
 add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS})

include/segway_rmp200_driver.h

@@ -178,6 +178,7 @@ class SegwayRmp200Driver : public iri_base_driver::IriBaseDriver
     void reset(void);
     void move_platform(float vT, float vR);
     void stop_platform(void);
+    void reset_integrators(void);
     const std::list<std::string> & getSegwayEvents() const;
     void setSegwayEvents(void);
     static const std::string & getSegwayEventName(const unsigned int & ev);

src/segway_rmp200_driver.cpp

@@ -385,6 +385,17 @@ void SegwayRmp200Driver::stop_platform(void)
   }
 }
 
+void SegwayRmp200Driver::reset_integrators(void)
+{
+  if(segway_!=NULL)
+  {
+    segway_->reset_right_wheel_integrator();
+    segway_->reset_left_wheel_integrator();
+    segway_->reset_yaw_integrator();
+    segway_->reset_forward_integrator();
+  }
+}
+
 const std::list<std::string> & SegwayRmp200Driver::getSegwayEvents() const
 {
   return events_;

src/segway_rmp200_driver_node.cpp

@@ -26,7 +26,7 @@ SegwayRmp200DriverNode::SegwayRmp200DriverNode(ros::NodeHandle &nh) try :
 }
 catch (CException & e)
 {
-  ROS_FATAL("'%s'",e.what().c_str());
+  ROS_FATAL("SegwayRmp200DriverNode: '%s'",e.what().c_str());
 }
 
 void SegwayRmp200DriverNode::mainNodeThread(void)
@@ -39,16 +39,16 @@ void SegwayRmp200DriverNode::mainNodeThread(void)
   switch(segway_event_id_)
   {
     case SegwayRmp200Driver::NO_USB:
-      ROS_WARN("The USB cable has been disconnected");
+      ROS_WARN("SegwayRmp200DriverNode: The USB cable has been disconnected");
       driver_.goClosed();
       break;
 
     case SegwayRmp200Driver::POWER_OFF:
-      ROS_WARN("The segway platform power is off");
+      ROS_WARN("SegwayRmp200DriverNode: The segway platform power is off");
       break;
 
     case SegwayRmp200Driver::NO_HEART_BEAT:
-      ROS_WARN("No heart beat detected");
+      ROS_WARN("SegwayRmp200DriverNode: No heart beat detected");
       break;
 
     case SegwayRmp200Driver::NEW_STATUS:
@@ -87,7 +87,7 @@ void SegwayRmp200DriverNode::updateSegwayStatus()
 /*  [subscriber callbacks] */
 void SegwayRmp200DriverNode::cmd_platform_callback(const geometry_msgs::Twist::ConstPtr& msg) 
 {
-  ROS_DEBUG("SegwayRmp200DriverNode::cmd_platform_callback");
+  //ROS_DEBUG("SegwayRmp200DriverNode::cmd_platform_callback");
 
   //lock access to driver if necessary 
   this->driver_.lock();
@@ -97,14 +97,14 @@ void SegwayRmp200DriverNode::cmd_platform_callback(const geometry_msgs::Twist::C
   {
     double vT = msg->linear.x;//sqrt(msg->linear.x*msg->linear.x + msg->linear.y*msg->linear.y);
     double vR = msg->angular.z;
-    ROS_INFO("New Command Received: vT=%f vR=%f", vT, vR);
+    //ROS_INFO("SegwayRmp200DriverNode: New Command Received: vT=%f vR=%f", vT, vR);
 
     //request platform movement
     this->driver_.move_platform(vT, vR);
   }
   else
   {
-    ROS_WARN("Segway driver has not been started yet");
+    ROS_WARN("SegwayRmp200DriverNode: Segway driver has not been started yet");
   }
 
   //unlock access to driver if previously blocked