Added recover and stop in RobotHW

combined_robot_hw/include/combined_robot_hw/combined_robot_hw.h

@@ -100,11 +100,22 @@ class CombinedRobotHW : public hardware_interface::RobotHW
    */
   virtual void write(const ros::Time& time, const ros::Duration& period);
 
+  /**
+   * Stops the robot HW
+   */
+  virtual bool stop();
+
+  /**
+   * Recovers/Reinitializes the robot HW
+   */
+  virtual bool recover();
+
 protected:
   ros::NodeHandle root_nh_;
   ros::NodeHandle robot_hw_nh_;
   pluginlib::ClassLoader<hardware_interface::RobotHW> robot_hw_loader_;
   std::vector<hardware_interface::RobotHWSharedPtr> robot_hw_list_;
+  std::vector<std::string> robot_hw_names;
 
   virtual bool loadRobotHW(const std::string& name);
 

combined_robot_hw/src/combined_robot_hw.cpp

@@ -39,16 +39,15 @@ namespace combined_robot_hw
     root_nh_ = root_nh;
     robot_hw_nh_ = robot_hw_nh;
 
-    std::vector<std::string> robots;
     std::string param_name = "robot_hardware";
-    if (!robot_hw_nh.getParam(param_name, robots))
+    if (!robot_hw_nh.getParam(param_name, robot_hw_names))
     {
       ROS_ERROR_STREAM("Could not find '" << param_name << "' parameter (namespace: " << robot_hw_nh.getNamespace() << ").");
       return false;
     }
 
     std::vector<std::string>::iterator it;
-    for (it = robots.begin(); it != robots.end(); it++)
+    for (it = robot_hw_names.begin(); it != robot_hw_names.end(); it++)
     {
       if (!loadRobotHW(*it))
       {
@@ -205,6 +204,40 @@ namespace combined_robot_hw
     }
   }
 
+  bool CombinedRobotHW::stop()
+  {
+    int i = 0;
+    bool stop_success = true;
+    std::vector<hardware_interface::RobotHWSharedPtr>::iterator robot_hw;
+    for (robot_hw = robot_hw_list_.begin(); robot_hw != robot_hw_list_.end(); ++robot_hw)
+    {
+      if ((*robot_hw)->stop() == false)
+      {
+        ROS_ERROR("Stopping robot HW '%s' failed", robot_hw_names[i].c_str());
+        stop_success = false;
+      }
+      i++;
+    }
+    return stop_success;
+  }
+
+  bool CombinedRobotHW::recover()
+  {
+    int i = 0;
+    bool recover_success = true;
+    std::vector<hardware_interface::RobotHWSharedPtr>::iterator robot_hw;
+    for (robot_hw = robot_hw_list_.begin(); robot_hw != robot_hw_list_.end(); ++robot_hw)
+    {
+      if ((*robot_hw)->recover() == false)
+      {
+        ROS_ERROR("Recovering/Reinitializing robot HW '%s' failed", robot_hw_names[i].c_str());
+        recover_success = false;
+      }
+      i++;
+    }
+    return recover_success;
+  }
+
   void CombinedRobotHW::filterControllerList(const std::list<hardware_interface::ControllerInfo>& list,
                                              std::list<hardware_interface::ControllerInfo>& filtered_list,
                                              hardware_interface::RobotHWSharedPtr robot_hw)

combined_robot_hw_tests/include/combined_robot_hw_tests/my_robot_hw_1.h

@@ -43,8 +43,11 @@ class MyRobotHW1 : public hardware_interface::RobotHW
   MyRobotHW1();
   virtual ~MyRobotHW1(){};
   virtual bool init(ros::NodeHandle& root_nh, ros::NodeHandle &robot_hw_nh);
+  void setInitValues();
   virtual void read(const ros::Time& time, const ros::Duration& period);
   virtual void write(const ros::Time& time, const ros::Duration& period);
+  virtual bool recover();
+  virtual bool stop();
   virtual bool prepareSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
                              const std::list<hardware_interface::ControllerInfo>& stop_list);
   virtual void doSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,

combined_robot_hw_tests/src/my_robot_hw_1.cpp

@@ -49,25 +49,10 @@ bool MyRobotHW1::init(ros::NodeHandle& root_nh, ros::NodeHandle &robot_hw_nh)
   joint_name_.resize(3);
 
   joint_name_[0] = "test_joint1";
-  joint_position_[0] = 1.0;
-  joint_velocity_[0] = 0.0;
-  joint_effort_[0] = 0.1;
-  joint_effort_command_[0] = 3.0;
-  joint_velocity_command_[0] = 0.0;
-
   joint_name_[1] = "test_joint2";
-  joint_position_[1] = 1.0;
-  joint_velocity_[1] = 0.0;
-  joint_effort_[1] = 0.1;
-  joint_effort_command_[1] = 0.0;
-  joint_velocity_command_[1] = 0.0;
-
   joint_name_[2] = "test_joint3";
-  joint_position_[2] = 1.0;
-  joint_velocity_[2] = 0.0;
-  joint_effort_[2] = 0.1;
-  joint_effort_command_[2] = 0.0;
-  joint_velocity_command_[2] = 0.0;
+
+  setInitValues();
 
   // Populate hardware interfaces
   js_interface_.registerHandle(JointStateHandle(joint_name_[0], &joint_position_[0], &joint_velocity_[0], &joint_effort_[0]));
@@ -101,6 +86,47 @@ void MyRobotHW1::write(const ros::Time& time, const ros::Duration& period)
   joint_effort_command_[1] = joint_effort_command_[0];
 }
 
+void MyRobotHW1::setInitValues()
+{
+  joint_position_[0] = 1.0;
+  joint_velocity_[0] = 0.0;
+  joint_effort_[0] = 0.1;
+  joint_effort_command_[0] = 3.0;
+  joint_velocity_command_[0] = 0.0;
+
+  joint_position_[1] = 1.0;
+  joint_velocity_[1] = 0.0;
+  joint_effort_[1] = 0.1;
+  joint_effort_command_[1] = 0.0;
+  joint_velocity_command_[1] = 0.0;
+
+  joint_position_[2] = 1.0;
+  joint_velocity_[2] = 0.0;
+  joint_effort_[2] = 0.1;
+  joint_effort_command_[2] = 0.0;
+  joint_velocity_command_[2] = 0.0;
+}
+
+bool MyRobotHW1::recover()
+{
+  if (joint_effort_command_[0] == 10.0)
+  {
+    return false;
+  }
+  setInitValues();
+  return true;
+}
+
+bool MyRobotHW1::stop()
+{
+  if (joint_effort_command_[0] == 10.0)
+  {
+    return false;
+  }
+  joint_effort_command_[0] = 0.0;
+  return true;
+}
+
 bool MyRobotHW1::prepareSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
                                const std::list<hardware_interface::ControllerInfo>& stop_list)
 {

combined_robot_hw_tests/test/combined_robot_hw_test.cpp

@@ -94,6 +94,28 @@ TEST(CombinedRobotHWTests, combinationOk)
   robot_hw.write(ros::Time::now(), period);
   ej_handle = ej_interface->getHandle("test_joint2");
   ASSERT_FLOAT_EQ(3.5, ej_handle.getCommand());
+
+  // Test recover function
+  ej_handle = ej_interface->getHandle("test_joint1");
+  ej_handle.setCommand(3.5);
+  bool recover_success = robot_hw.recover();
+  ASSERT_TRUE(recover_success);
+  ASSERT_FLOAT_EQ(3.0, ej_handle.getCommand());
+  ej_handle.setCommand(10.0);
+  recover_success = robot_hw.recover();
+  ASSERT_FALSE(recover_success);
+  ASSERT_FLOAT_EQ(10.0, ej_handle.getCommand());
+
+  // Test stop function
+  ej_handle = ej_interface->getHandle("test_joint1");
+  ej_handle.setCommand(3.5);
+  bool stop_success = robot_hw.stop();
+  ASSERT_TRUE(stop_success);
+  ASSERT_FLOAT_EQ(0.0, ej_handle.getCommand());
+  ej_handle.setCommand(10.0);
+  stop_success = robot_hw.stop();
+  ASSERT_FALSE(stop_success);
+  ASSERT_FLOAT_EQ(10.0, ej_handle.getCommand());
 }
 
 TEST(CombinedRobotHWTests, switchOk)

hardware_interface/include/hardware_interface/robot_hw.h

@@ -78,6 +78,21 @@ class RobotHW : public InterfaceManager
    * \returns True if initialization was successful
    */
   virtual bool init(ros::NodeHandle& root_nh, ros::NodeHandle &robot_hw_nh) {return true;}
+
+  /** \brief The stop function is called to stop the RobotHW from a
+   * non-realtime thread. Usually on controller manager shutdown.
+   *
+   * \returns True if stopping was successful
+   */
+  virtual bool stop() {return true;}
+
+  /** \brief The recover function is called to recover/reinitialize the RobotHW after and
+   * error state (e.g. emergency stop, safety stop, hardware limits) from a non-realtime 
+   * thread.
+   *
+   * \returns True if recovering was successful
+   */
+  virtual bool recover() {return true;}
 
   /** \name Resource Management
    *\{*/