Add method to check for error in robotHW component

ros-controls · Sep 17, 2018 · 73b4483 · 73b4483
1 parent 7202777
commit 73b4483
Show file tree

Hide file tree

Showing 3 changed files with 25 additions and 0 deletions.
diff --git a/combined_robot_hw/include/combined_robot_hw/combined_robot_hw.h b/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_;

diff --git a/combined_robot_hw/src/combined_robot_hw.cpp b/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)

diff --git a/hardware_interface/include/hardware_interface/robot_hw.h b/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;