[Feature] Hardware Components Grouping (ros-controls#1458)

saikishor · web-flow · commit 6ae39507231e · 2024-05-22T18:38:18.000+01:00
diff --git a/hardware_interface/doc/hardware_interface_types_userdoc.rst b/hardware_interface/doc/hardware_interface_types_userdoc.rst
@@ -41,6 +41,12 @@ The ``<gpio>`` tag can be used as a child of all three types of hardware compone
 Because ports implemented as ``<gpio>``-tag are typically very application-specific, there exists no generic publisher
 within the ros2_control framework. A custom gpio-controller has to be implemented for each application. As an example, see :ref:`the GPIO controller example <ros2_control_demos_example_10_userdoc>` as part of the demo repository.
 
+Hardware Groups
+*****************************
+Hardware Component Groups serve as a critical organizational mechanism within complex systems, facilitating error handling and fault tolerance. By grouping related hardware components together, such as actuators within a manipulator, users can establish a unified framework for error detection and response.
+
+Hardware Component Groups play a vital role in propagating errors across interconnected hardware components. For instance, in a manipulator system, grouping actuators together allows for error propagation. If one actuator fails within the group, the error can propagate to the other actuators, signaling a potential issue across the system. By default, the actuator errors are isolated to their own hardware component, allowing the rest to continue operation unaffected. In the provided ros2_control configuration, the ``<group>`` tag within each ``<ros2_control>`` block signifies the grouping of hardware components, enabling error propagation mechanisms within the system.
+
 Examples
 *****************************
 The following examples show how to use the different hardware interface types in a ``ros2_control`` URDF.
@@ -152,3 +158,66 @@ They can be combined together within the different hardware component types (sys
         <state_interface name="calibration_matrix_nr"/>
       </gpio>
     </ros2_control>
+
+4. Robot with multiple hardware components belonging to same group : ``Group1``
+
+   - RRBot System 1 and 2
+   - Digital: Total 4 inputs and 2 outputs
+   - Analog: Total 2 inputs and 1 output
+   - Vacuum valve at the flange (on/off)
+   - Group: Group1
+
+  .. code:: xml
+
+    <ros2_control name="RRBotSystem1" type="system">
+      <hardware>
+        <plugin>ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware</plugin>
+        <group>Group1</group>
+        <param name="example_param_hw_start_duration_sec">2.0</param>
+        <param name="example_param_hw_stop_duration_sec">3.0</param>
+        <param name="example_param_hw_slowdown">2.0</param>
+      </hardware>
+      <joint name="joint1">
+        <command_interface name="position">
+          <param name="min">-1</param>
+          <param name="max">1</param>
+        </command_interface>
+        <state_interface name="position"/>
+      </joint>
+      <gpio name="flange_analog_IOs">
+        <command_interface name="analog_output1"/>
+        <state_interface name="analog_output1">    <!-- Needed to know current state of the output -->
+          <param name="initial_value">3.1</param>  <!-- Optional initial value for mock_hardware -->
+        </state_interface>
+        <state_interface name="analog_input1"/>
+        <state_interface name="analog_input2"/>
+      </gpio>
+      <gpio name="flange_vacuum">
+        <command_interface name="vacuum"/>
+        <state_interface name="vacuum"/>    <!-- Needed to know current state of the output -->
+      </gpio>
+    </ros2_control>
+    <ros2_control name="RRBotSystem2" type="system">
+      <hardware>
+        <plugin>ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware</plugin>
+        <group>Group1</group>
+        <param name="example_param_hw_start_duration_sec">2.0</param>
+        <param name="example_param_hw_stop_duration_sec">3.0</param>
+        <param name="example_param_hw_slowdown">2.0</param>
+      </hardware>
+      <joint name="joint2">
+        <command_interface name="position">
+          <param name="min">-1</param>
+          <param name="max">1</param>
+        </command_interface>
+        <state_interface name="position"/>
+      </joint>
+      <gpio name="flange_digital_IOs">
+        <command_interface name="digital_output1"/>
+        <state_interface name="digital_output1"/>    <!-- Needed to know current state of the output -->
+        <command_interface name="digital_output2"/>
+        <state_interface name="digital_output2"/>
+        <state_interface name="digital_input1"/>
+        <state_interface name="digital_input2"/>
+      </gpio>
+    </ros2_control>
diff --git a/hardware_interface/include/hardware_interface/actuator.hpp b/hardware_interface/include/hardware_interface/actuator.hpp
@@ -83,6 +83,9 @@ class Actuator final
   HARDWARE_INTERFACE_PUBLIC
   std::string get_name() const;
 
+  HARDWARE_INTERFACE_PUBLIC
+  std::string get_group_name() const;
+
   HARDWARE_INTERFACE_PUBLIC
   const rclcpp_lifecycle::State & get_state() const;
 
diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp
@@ -190,6 +190,12 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
    */
   virtual std::string get_name() const { return info_.name; }
 
+  /// Get name of the actuator hardware group to which it belongs to.
+  /**
+   * \return group name.
+   */
+  virtual std::string get_group_name() const { return info_.group; }
+
   /// Get life-cycle state of the actuator hardware.
   /**
    * \return state.
diff --git a/hardware_interface/include/hardware_interface/hardware_component_info.hpp b/hardware_interface/include/hardware_interface/hardware_component_info.hpp
@@ -39,6 +39,9 @@ struct HardwareComponentInfo
   /// Component "classification": "actuator", "sensor" or "system"
   std::string type;
 
+  /// Component group
+  std::string group;
+
   /// Component pluginlib plugin name.
   std::string plugin_name;
 
diff --git a/hardware_interface/include/hardware_interface/hardware_info.hpp b/hardware_interface/include/hardware_interface/hardware_info.hpp
@@ -133,6 +133,8 @@ struct HardwareInfo
   std::string name;
   /// Type of the hardware: actuator, sensor or system.
   std::string type;
+  ///  Hardware group to which the hardware belongs.
+  std::string group;
   /// Component is async
   bool is_async;
   /// Name of the pluginlib plugin of the hardware that will be loaded.
diff --git a/hardware_interface/include/hardware_interface/sensor.hpp b/hardware_interface/include/hardware_interface/sensor.hpp
@@ -71,6 +71,9 @@ class Sensor final
   HARDWARE_INTERFACE_PUBLIC
   std::string get_name() const;
 
+  HARDWARE_INTERFACE_PUBLIC
+  std::string get_group_name() const;
+
   HARDWARE_INTERFACE_PUBLIC
   const rclcpp_lifecycle::State & get_state() const;
 
diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp
@@ -129,6 +129,12 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
    */
   virtual std::string get_name() const { return info_.name; }
 
+  /// Get name of the actuator hardware group to which it belongs to.
+  /**
+   * \return group name.
+   */
+  virtual std::string get_group_name() const { return info_.group; }
+
   /// Get life-cycle state of the actuator hardware.
   /**
    * \return state.
diff --git a/hardware_interface/include/hardware_interface/system.hpp b/hardware_interface/include/hardware_interface/system.hpp
@@ -84,6 +84,9 @@ class System final
   HARDWARE_INTERFACE_PUBLIC
   std::string get_name() const;
 
+  HARDWARE_INTERFACE_PUBLIC
+  std::string get_group_name() const;
+
   HARDWARE_INTERFACE_PUBLIC
   const rclcpp_lifecycle::State & get_state() const;
 
diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp
@@ -191,6 +191,12 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
    */
   virtual std::string get_name() const { return info_.name; }
 
+  /// Get name of the actuator hardware group to which it belongs to.
+  /**
+   * \return group name.
+   */
+  virtual std::string get_group_name() const { return info_.group; }
+
   /// Get life-cycle state of the actuator hardware.
   /**
    * \return state.
diff --git a/hardware_interface/src/actuator.cpp b/hardware_interface/src/actuator.cpp
@@ -214,6 +214,8 @@ return_type Actuator::perform_command_mode_switch(
 
 std::string Actuator::get_name() const { return impl_->get_name(); }
 
+std::string Actuator::get_group_name() const { return impl_->get_group_name(); }
+
 const rclcpp_lifecycle::State & Actuator::get_state() const { return impl_->get_state(); }
 
 return_type Actuator::read(const rclcpp::Time & time, const rclcpp::Duration & period)
diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp
@@ -36,6 +36,7 @@ constexpr const auto kROS2ControlTag = "ros2_control";
 constexpr const auto kHardwareTag = "hardware";
 constexpr const auto kPluginNameTag = "plugin";
 constexpr const auto kParamTag = "param";
+constexpr const auto kGroupTag = "group";
 constexpr const auto kActuatorTag = "actuator";
 constexpr const auto kJointTag = "joint";
 constexpr const auto kSensorTag = "sensor";
@@ -578,6 +579,11 @@ HardwareInfo parse_resource_from_xml(
       const auto * type_it = ros2_control_child_it->FirstChildElement(kPluginNameTag);
       hardware.hardware_plugin_name =
         get_text_for_element(type_it, std::string("hardware ") + kPluginNameTag);
+      const auto * group_it = ros2_control_child_it->FirstChildElement(kGroupTag);
+      if (group_it)
+      {
+        hardware.group = get_text_for_element(group_it, std::string("hardware.") + kGroupTag);
+      }
       const auto * params_it = ros2_control_child_it->FirstChildElement(kParamTag);
       if (params_it)
       {
diff --git a/hardware_interface/src/mock_components/generic_system.cpp b/hardware_interface/src/mock_components/generic_system.cpp
@@ -459,7 +459,8 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur
     return return_type::OK;
   }
 
-  auto mirror_command_to_state = [](auto & states_, auto commands_, size_t start_index = 0)
+  auto mirror_command_to_state =
+    [](auto & states_, auto commands_, size_t start_index = 0) -> return_type
   {
     for (size_t i = start_index; i < states_.size(); ++i)
     {
@@ -469,8 +470,13 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur
         {
           states_[i][j] = commands_[i][j];
         }
+        if (std::isinf(commands_[i][j]))
+        {
+          return return_type::ERROR;
+        }
       }
     }
+    return return_type::OK;
   };
 
   for (size_t j = 0; j < joint_states_[POSITION_INTERFACE_INDEX].size(); ++j)
@@ -556,13 +562,11 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur
 
   // do loopback on all other interfaces - starts from 1 or 3 because 0, 1, 3 are position,
   // velocity, and acceleration interface
-  if (calculate_dynamics_)
-  {
-    mirror_command_to_state(joint_states_, joint_commands_, 3);
-  }
-  else
+  if (
+    mirror_command_to_state(joint_states_, joint_commands_, calculate_dynamics_ ? 3 : 1) !=
+    return_type::OK)
   {
-    mirror_command_to_state(joint_states_, joint_commands_, 1);
+    return return_type::ERROR;
   }
 
   for (const auto & mimic_joint : info_.mimic_joints)
diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp
@@ -133,10 +133,12 @@ class ResourceStorage
         HardwareComponentInfo component_info;
         component_info.name = hardware_info.name;
         component_info.type = hardware_info.type;
+        component_info.group = hardware_info.group;
         component_info.plugin_name = hardware_info.hardware_plugin_name;
         component_info.is_async = hardware_info.is_async;
 
         hardware_info_map_.insert(std::make_pair(component_info.name, component_info));
+        hw_group_state_.insert(std::make_pair(component_info.group, return_type::OK));
         hardware_used_by_controllers_.insert(
           std::make_pair(component_info.name, std::vector<std::string>()));
         is_loaded = true;
@@ -298,6 +300,10 @@ class ResourceStorage
         async_component_threads_.at(hardware.get_name()).register_component(&hardware);
       }
     }
+    if (!hardware.get_group_name().empty())
+    {
+      hw_group_state_[hardware.get_group_name()] = return_type::OK;
+    }
     return result;
   }
 
@@ -380,6 +386,10 @@ class ResourceStorage
     {
       remove_all_hardware_interfaces_from_available_list(hardware.get_name());
     }
+    if (!hardware.get_group_name().empty())
+    {
+      hw_group_state_[hardware.get_group_name()] = return_type::OK;
+    }
     return result;
   }
 
@@ -414,6 +424,10 @@ class ResourceStorage
       // deimport_non_movement_command_interfaces(hardware);
       // deimport_state_interfaces(hardware);
       // use remove_command_interfaces(hardware);
+      if (!hardware.get_group_name().empty())
+      {
+        hw_group_state_[hardware.get_group_name()] = return_type::OK;
+      }
     }
     return result;
   }
@@ -884,6 +898,27 @@ class ResourceStorage
     }
   }
 
+  /**
+   * Returns the return type of the hardware component group state, if the return type is other
+   * than OK, then updates the return type of the group to the respective one
+   */
+  return_type update_hardware_component_group_state(
+    const std::string & group_name, const return_type & value)
+  {
+    // This is for the components that has no configured group
+    if (group_name.empty())
+    {
+      return value;
+    }
+    // If it is anything other than OK, change the return type of the hardware group state
+    // to the respective return type
+    if (value != return_type::OK)
+    {
+      hw_group_state_.at(group_name) = value;
+    }
+    return hw_group_state_.at(group_name);
+  }
+
   // hardware plugins
   pluginlib::ClassLoader<ActuatorInterface> actuator_loader_;
   pluginlib::ClassLoader<SensorInterface> sensor_loader_;
@@ -898,6 +933,7 @@ class ResourceStorage
   std::vector<System> async_systems_;
 
   std::unordered_map<std::string, HardwareComponentInfo> hardware_info_map_;
+  std::unordered_map<std::string, hardware_interface::return_type> hw_group_state_;
 
   /// Mapping between hardware and controllers that are using it (accessing data from it)
   std::unordered_map<std::string, std::vector<std::string>> hardware_used_by_controllers_;
@@ -1575,6 +1611,9 @@ HardwareReadWriteStatus ResourceManager::read(
       try
       {
         ret_val = component.read(time, period);
+        const auto component_group = component.get_group_name();
+        ret_val =
+          resource_storage_->update_hardware_component_group_state(component_group, ret_val);
       }
       catch (const std::exception & e)
       {
@@ -1633,6 +1672,9 @@ HardwareReadWriteStatus ResourceManager::write(
       try
       {
         ret_val = component.write(time, period);
+        const auto component_group = component.get_group_name();
+        ret_val =
+          resource_storage_->update_hardware_component_group_state(component_group, ret_val);
       }
       catch (const std::exception & e)
       {
diff --git a/hardware_interface/src/sensor.cpp b/hardware_interface/src/sensor.cpp
@@ -191,6 +191,8 @@ std::vector<StateInterface> Sensor::export_state_interfaces()
 
 std::string Sensor::get_name() const { return impl_->get_name(); }
 
+std::string Sensor::get_group_name() const { return impl_->get_group_name(); }
+
 const rclcpp_lifecycle::State & Sensor::get_state() const { return impl_->get_state(); }
 
 return_type Sensor::read(const rclcpp::Time & time, const rclcpp::Duration & period)
diff --git a/hardware_interface/src/system.cpp b/hardware_interface/src/system.cpp
@@ -210,6 +210,8 @@ return_type System::perform_command_mode_switch(
 
 std::string System::get_name() const { return impl_->get_name(); }
 
+std::string System::get_group_name() const { return impl_->get_group_name(); }
+
 const rclcpp_lifecycle::State & System::get_state() const { return impl_->get_state(); }
 
 return_type System::read(const rclcpp::Time & time, const rclcpp::Duration & period)
diff --git a/hardware_interface/test/mock_components/test_generic_system.cpp b/hardware_interface/test/mock_components/test_generic_system.cpp
diff --git a/hardware_interface/test/test_component_parser.cpp b/hardware_interface/test/test_component_parser.cpp
diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp
