From 73b44836bb3c962abe6294a63ae64575ae4a4e12 Mon Sep 17 00:00:00 2001
From: Bo Chen <bo@enway.ai>
Date: Mon, 17 Sep 2018 15:27:25 +0200
Subject: [PATCH] Add method to check for error in robotHW component

---
 .../include/combined_robot_hw/combined_robot_hw.h  |  5 +++++
 combined_robot_hw/src/combined_robot_hw.cpp        | 14 ++++++++++++++
 .../include/hardware_interface/robot_hw.h          |  6 ++++++
 3 files changed, 25 insertions(+)

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
index e53ce02af..2b89403b7 100644
--- 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
index 2ed6215af..04fed6b37 100644
--- 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
index f8bdc6d44..e59b03248 100644
--- 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;