Add method to check for error in robotHW component

combined_robot_hw/include/combined_robot_hw/combined_robot_hw.h

@@ -100,6 +100,11 @@ class CombinedRobotHW : public hardware_interface::RobotHW
    */
   virtual void write(const ros::Time& time, const ros::Duration& period);
 
+  /**
+   * Checks whether the robot HW components have errors
+   */
+  virtual bool hasError();
+
 protected:
   ros::NodeHandle root_nh_;
   ros::NodeHandle robot_hw_nh_;

combined_robot_hw/src/combined_robot_hw.cpp

@@ -205,6 +205,20 @@ namespace combined_robot_hw
     }
   }
 
+  bool CombinedRobotHW::hasError()
+  {
+    // Call the hasError method of the single RobotHW objects.
+    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)->hasError())
+      {
+        return true;
+      }
+    }
+    return false;
+  }
+
   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/CMakeLists.txt

@@ -25,6 +25,7 @@ add_library(${PROJECT_NAME}
   src/my_robot_hw_2.cpp
   src/my_robot_hw_3.cpp
   src/my_robot_hw_4.cpp
+  src/my_robot_hw_5.cpp
 )
 add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS})
 target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES})

combined_robot_hw_tests/include/combined_robot_hw_tests/my_robot_hw_5.h

@@ -0,0 +1,54 @@
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2015, Shadow Robot Company Ltd.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//   * Redistributions of source code must retain the above copyright notice,
+//     this list of conditions and the following disclaimer.
+//   * Redistributions in binary form must reproduce the above copyright
+//     notice, this list of conditions and the following disclaimer in the
+//     documentation and/or other materials provided with the distribution.
+//   * Neither the name of hiDOF, Inc. nor the names of its
+//     contributors may be used to endorse or promote products derived from
+//     this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//////////////////////////////////////////////////////////////////////////////
+
+
+
+#ifndef CONTROLLER_MANAGER_TESTS_MY_ROBOT_HW_5_H
+#define CONTROLLER_MANAGER_TESTS_MY_ROBOT_HW_5_H
+
+#include <hardware_interface/joint_command_interface.h>
+#include <hardware_interface/robot_hw.h>
+#include <pluginlib/class_list_macros.hpp>
+
+namespace combined_robot_hw_tests
+{
+
+class MyRobotHW5 : public hardware_interface::RobotHW
+{
+public:
+  MyRobotHW5();
+  virtual ~MyRobotHW5(){};
+  virtual bool init(ros::NodeHandle& root_nh, ros::NodeHandle &robot_hw_nh);
+  void read(const ros::Time& time, const ros::Duration& period);
+  void write(const ros::Time& time, const ros::Duration& period);
+  bool hasError();
+
+};
+}
+
+
+#endif

combined_robot_hw_tests/src/my_robot_hw_5.cpp

@@ -0,0 +1,61 @@
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2015, Shadow Robot Company Ltd.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//   * Redistributions of source code must retain the above copyright notice,
+//     this list of conditions and the following disclaimer.
+//   * Redistributions in binary form must reproduce the above copyright
+//     notice, this list of conditions and the following disclaimer in the
+//     documentation and/or other materials provided with the distribution.
+//   * Neither the name of Shadow Robot Company Ltd. nor the names of its
+//     contributors may be used to endorse or promote products derived from
+//     this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//////////////////////////////////////////////////////////////////////////////
+
+
+#include <combined_robot_hw_tests/my_robot_hw_5.h>
+
+namespace combined_robot_hw_tests
+{
+
+MyRobotHW5::MyRobotHW5()
+{
+}
+
+bool MyRobotHW5::init(ros::NodeHandle& root_nh, ros::NodeHandle &robot_hw_nh)
+{
+  return true;
+}
+
+
+void MyRobotHW5::read(const ros::Time& time, const ros::Duration& period)
+{
+
+}
+
+void MyRobotHW5::write(const ros::Time& time, const ros::Duration& period)
+{
+}
+
+bool MyRobotHW5::hasError()
+{
+  return true;
+}
+
+}
+
+PLUGINLIB_EXPORT_CLASS( combined_robot_hw_tests::MyRobotHW5, hardware_interface::RobotHW)
+

combined_robot_hw_tests/test/cm_test.test

@@ -20,6 +20,8 @@
       joint_name_filter: right_arm
     my_robot_hw_4:
       type: combined_robot_hw_tests/MyRobotHW4
+    my_robot_hw_5:
+      type: combined_robot_hw_tests/MyRobotHW5
     my_controller:
       type: controller_manager_tests/EffortTestController
     my_controller2:

combined_robot_hw_tests/test/combined_robot_hw_test.cpp

@@ -94,6 +94,9 @@ 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());
+
+  bool error = robot_hw.hasError();
+  ASSERT_TRUE(error);
 }
 
 TEST(CombinedRobotHWTests, switchOk)

combined_robot_hw_tests/test/combined_robot_hw_test.test

@@ -5,6 +5,7 @@
     - my_robot_hw_2
     - my_robot_hw_3
     - my_robot_hw_4
+    - my_robot_hw_5
     my_robot_hw_1:
       type: combined_robot_hw_tests/MyRobotHW1
     my_robot_hw_2:
@@ -18,6 +19,8 @@
       joint_name_filter: right_arm
     my_robot_hw_4:
       type: combined_robot_hw_tests/MyRobotHW4
+    my_robot_hw_5:
+      type: combined_robot_hw_tests/MyRobotHW5
   </rosparam>
 
   <test test-name="combined_robot_hw_tests" pkg="combined_robot_hw_tests" type="combined_robot_hw_test"/>

combined_robot_hw_tests/test_robot_hw_plugin.xml

@@ -22,4 +22,10 @@
     A type of RobotHW
   </description>
   </class>
+
+  <class name="combined_robot_hw_tests/MyRobotHW5" type="combined_robot_hw_tests::MyRobotHW5" base_class_type="hardware_interface::RobotHW">
+  <description>
+    A type of RobotHW
+  </description>
+  </class>
 </library>

hardware_interface/include/hardware_interface/robot_hw.h

@@ -163,6 +163,12 @@ class RobotHW : public InterfaceManager
    * \param period The time passed since the last call to \ref write
    */
   virtual void write(const ros::Time& time, const ros::Duration& period) {}
+
+  /**
+   * Check whether the robot HW has encountered an error
+   */
+  virtual bool hasError() { return false; };
+
 };
 
 typedef std::shared_ptr<RobotHW> RobotHWSharedPtr;