[Feature] Hardware Components Grouping

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>

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;
 

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.

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;
 

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.

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;
 

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.

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;
 

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.

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)

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)
       {

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)

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 > hw_group_state_.at(group_name))
+    {
+      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)
       {

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)

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)