[RM] Isolate start and stop interfaces for each Hardware Component

doc/release_notes.rst

@@ -168,6 +168,7 @@ hardware_interface
 * With (`#1567 <https://github.com/ros-controls/ros2_control/pull/1567>`_) all the Hardware components now have a fully functional asynchronous functionality, by simply adding ``is_async`` tag to the ros2_control tag in the URDF. This will allow the hardware components to run in a separate thread, and the controller manager will be able to run the controllers in parallel with the hardware components.
 * The hardware components can be easily introspect the internal member variables using the macro ``REGISTER_ROS2_CONTROL_INTROSPECTION`` (`#1918 <https://github.com/ros-controls/ros2_control/pull/1918>`_)
 * Added new ``get_optional`` method that returns ``std::optional`` of the templated type, and this can be used to check if the value is available or not (`#1976 <https://github.com/ros-controls/ros2_control/pull/1976>`_ and `#2061 <https://github.com/ros-controls/ros2_control/pull/2061>`_)
+* The ``prepare_command_mode_switch`` and ``perform_command_mode_switch`` methods will now only receive the start/stop interfaces that belong to the hardware component instead of everything (`#2120 <https://github.com/ros-controls/ros2_control/pull/2120>`_)
 * Added hardware components execution time and periodicity statistics diagnostics (`#2086 <https://github.com/ros-controls/ros2_control/pull/2086>`_)
 
 joint_limits

hardware_interface/include/hardware_interface/actuator_interface.hpp

@@ -350,12 +350,10 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
    *
    * \note This is a non-realtime evaluation of whether a set of command interface claims are
    * possible, and call to start preparing data structures for the upcoming switch that will occur.
-   * \note All starting and stopping interface keys are passed to all components, so the function
-   * should return return_type::OK by default when given interface keys not relevant for this
-   * component. \param[in] start_interfaces vector of string identifiers for the command interfaces
-   * starting. \param[in] stop_interfaces vector of string identifiers for the command interfaces
-   * stopping. \return return_type::OK if the new command interface combination can be prepared, or
-   * if the interface key is not relevant to this system. Returns return_type::ERROR otherwise.
+   * \param[in] start_interfaces vector of string identifiers for the command interfaces starting.
+   * \param[in] stop_interfaces vector of string identifiers for the command interfaces stopping.
+   * \return return_type::OK if the new command interface combination can be prepared (or) if the
+   * interface key is not relevant to this actuator. Returns return_type::ERROR otherwise.
    */
   virtual return_type prepare_command_mode_switch(
     const std::vector<std::string> & /*start_interfaces*/,
@@ -369,12 +367,10 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
    * Perform the mode-switching for the new command interface combination.
    *
    * \note This is part of the realtime update loop, and should be fast.
-   * \note All starting and stopping interface keys are passed to all components, so the function
-   * should return return_type::OK by default when given interface keys not relevant for this
-   * component. \param[in] start_interfaces vector of string identifiers for the command interfaces
-   * starting. \param[in] stop_interfaces vector of string identifiers for the command interfaces
-   * stopping. \return return_type::OK if the new command interface combination can be switched to,
-   * or if the interface key is not relevant to this system. Returns return_type::ERROR otherwise.
+   * \param[in] start_interfaces vector of string identifiers for the command interfaces starting.
+   * \param[in] stop_interfaces vector of string identifiers for the command interfaces stopping.
+   * \return return_type::OK if the new command interface combination can be switched to (or) if the
+   * interface key is not relevant to this actuator. Returns return_type::ERROR otherwise.
    */
   virtual return_type perform_command_mode_switch(
     const std::vector<std::string> & /*start_interfaces*/,

hardware_interface/include/hardware_interface/system_interface.hpp

@@ -380,12 +380,10 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
    *
    * \note This is a non-realtime evaluation of whether a set of command interface claims are
    * possible, and call to start preparing data structures for the upcoming switch that will occur.
-   * \note All starting and stopping interface keys are passed to all components, so the function
-   * should return return_type::OK by default when given interface keys not relevant for this
-   * component. \param[in] start_interfaces vector of string identifiers for the command interfaces
-   * starting. \param[in] stop_interfaces vector of string identifiers for the command interfaces
-   * stopping. \return return_type::OK if the new command interface combination can be prepared, or
-   * if the interface key is not relevant to this system. Returns return_type::ERROR otherwise.
+   * \param[in] start_interfaces vector of string identifiers for the command interfaces starting.
+   * \param[in] stop_interfaces vector of string identifiers for the command interfaces stopping.
+   * \return return_type::OK if the new command interface combination can be prepared (or) if the
+   * interface key is not relevant to this system. Returns return_type::ERROR otherwise.
    */
   virtual return_type prepare_command_mode_switch(
     const std::vector<std::string> & /*start_interfaces*/,
@@ -399,12 +397,10 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
    * Perform the mode-switching for the new command interface combination.
    *
    * \note This is part of the realtime update loop, and should be fast.
-   * \note All starting and stopping interface keys are passed to all components, so the function
-   * should return return_type::OK by default when given interface keys not relevant for this
-   * component. \param[in] start_interfaces vector of string identifiers for the command interfaces
-   * starting. \param[in] stop_interfaces vector of string identifiers for the command interfaces
-   * stopping. \return return_type::OK if the new command interface combination can be switched to,
-   * or if the interface key is not relevant to this system. Returns return_type::ERROR otherwise.
+   * \param[in] start_interfaces vector of string identifiers for the command interfaces starting.
+   * \param[in] stop_interfaces vector of string identifiers for the command interfaces stopping.
+   * \return return_type::OK if the new command interface combination can be switched to (or) if the
+   * interface key is not relevant to this system. Returns return_type::ERROR otherwise.
    */
   virtual return_type perform_command_mode_switch(
     const std::vector<std::string> & /*start_interfaces*/,

hardware_interface/src/resource_manager.cpp

@@ -89,6 +89,22 @@ std::string interfaces_to_string(
   return ss.str();
 };
 
+void get_hardware_related_interfaces(
+  const std::vector<std::string> & hw_command_itfs,
+  const std::vector<std::string> & start_stop_interfaces_list,
+  std::vector<std::string> & hw_interfaces)
+{
+  hw_interfaces.clear();
+  for (const auto & interface : start_stop_interfaces_list)
+  {
+    if (
+      std::find(hw_command_itfs.begin(), hw_command_itfs.end(), interface) != hw_command_itfs.end())
+    {
+      hw_interfaces.push_back(interface);
+    }
+  }
+}
+
 class ResourceStorage
 {
   static constexpr const char * pkg_name = "hardware_interface";
@@ -666,6 +682,8 @@ class ResourceStorage
       auto interfaces = hardware.export_command_interfaces();
       hardware_info_map_[hardware.get_name()].command_interfaces =
         add_command_interfaces(interfaces);
+      start_interfaces_buffer_.reserve(start_interfaces_buffer_.capacity() + interfaces.size());
+      stop_interfaces_buffer_.reserve(stop_interfaces_buffer_.capacity() + interfaces.size());
       // TODO(Manuel) END: for backward compatibility
     }
     catch (const std::exception & ex)
@@ -1297,6 +1315,10 @@ class ResourceStorage
   /// The callback to be called when a component state is switched
   std::function<void()> on_component_state_switch_callback_ = nullptr;
 
+  // To be used with the prepare and perform command switch for the hardware components
+  std::vector<std::string> start_interfaces_buffer_;
+  std::vector<std::string> stop_interfaces_buffer_;
+
   // Update rate of the controller manager, and the clock interface of its node
   // Used by async components.
   unsigned int cm_update_rate_ = 100;
@@ -1861,12 +1883,24 @@ bool ResourceManager::prepare_command_mode_switch(
     return false;
   }
 
+  const auto & hardware_info_map = resource_storage_->hardware_info_map_;
   auto call_prepare_mode_switch =
-    [&start_interfaces, &stop_interfaces, logger = get_logger()](auto & components)
+    [&start_interfaces, &stop_interfaces, &hardware_info_map, logger = get_logger()](
+      auto & components, auto & start_interfaces_buffer, auto & stop_interfaces_buffer)
   {
     bool ret = true;
     for (auto & component : components)
     {
+      const auto & hw_command_itfs = hardware_info_map.at(component.get_name()).command_interfaces;
+      get_hardware_related_interfaces(hw_command_itfs, start_interfaces, start_interfaces_buffer);
+      get_hardware_related_interfaces(hw_command_itfs, stop_interfaces, stop_interfaces_buffer);
+      if (start_interfaces_buffer.empty() && stop_interfaces_buffer.empty())
+      {
+        RCLCPP_DEBUG(
+          logger, "Component '%s' after filtering has no command interfaces to switch",
+          component.get_name().c_str());
+        continue;
+      }
       if (
         component.get_lifecycle_state().id() ==
           lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
@@ -1876,12 +1910,12 @@ bool ResourceManager::prepare_command_mode_switch(
         {
           if (
             return_type::OK !=
-            component.prepare_command_mode_switch(start_interfaces, stop_interfaces))
+            component.prepare_command_mode_switch(start_interfaces_buffer, stop_interfaces_buffer))
           {
             RCLCPP_ERROR(
               logger, "Component '%s' did not accept command interfaces combination: \n%s",
               component.get_name().c_str(),
-              interfaces_to_string(start_interfaces, stop_interfaces).c_str());
+              interfaces_to_string(start_interfaces_buffer, stop_interfaces_buffer).c_str());
             ret = false;
           }
         }
@@ -1892,7 +1926,8 @@ bool ResourceManager::prepare_command_mode_switch(
             "Exception of type : %s occurred while preparing command mode switch for component "
             "'%s' for the interfaces: \n %s : %s",
             typeid(e).name(), component.get_name().c_str(),
-            interfaces_to_string(start_interfaces, stop_interfaces).c_str(), e.what());
+            interfaces_to_string(start_interfaces_buffer, stop_interfaces_buffer).c_str(),
+            e.what());
           ret = false;
         }
         catch (...)
@@ -1902,16 +1937,27 @@ bool ResourceManager::prepare_command_mode_switch(
             "Unknown exception occurred while preparing command mode switch for component '%s' for "
             "the interfaces: \n %s",
             component.get_name().c_str(),
-            interfaces_to_string(start_interfaces, stop_interfaces).c_str());
+            interfaces_to_string(start_interfaces_buffer, stop_interfaces_buffer).c_str());
           ret = false;
         }
       }
+      else
+      {
+        RCLCPP_WARN(
+          logger, "Component '%s' is not in INACTIVE or ACTIVE state, skipping the prepare switch",
+          component.get_name().c_str());
+        ret = false;
+      }
     }
     return ret;
   };
 
-  const bool actuators_result = call_prepare_mode_switch(resource_storage_->actuators_);
-  const bool systems_result = call_prepare_mode_switch(resource_storage_->systems_);
+  const bool actuators_result = call_prepare_mode_switch(
+    resource_storage_->actuators_, resource_storage_->start_interfaces_buffer_,
+    resource_storage_->stop_interfaces_buffer_);
+  const bool systems_result = call_prepare_mode_switch(
+    resource_storage_->systems_, resource_storage_->start_interfaces_buffer_,
+    resource_storage_->stop_interfaces_buffer_);
 
   return actuators_result && systems_result;
 }
@@ -1927,12 +1973,24 @@ bool ResourceManager::perform_command_mode_switch(
     return true;
   }
 
+  const auto & hardware_info_map = resource_storage_->hardware_info_map_;
   auto call_perform_mode_switch =
-    [&start_interfaces, &stop_interfaces, logger = get_logger()](auto & components)
+    [&start_interfaces, &stop_interfaces, &hardware_info_map, logger = get_logger()](
+      auto & components, auto & start_interfaces_buffer, auto & stop_interfaces_buffer)
   {
     bool ret = true;
     for (auto & component : components)
     {
+      const auto & hw_command_itfs = hardware_info_map.at(component.get_name()).command_interfaces;
+      get_hardware_related_interfaces(hw_command_itfs, start_interfaces, start_interfaces_buffer);
+      get_hardware_related_interfaces(hw_command_itfs, stop_interfaces, stop_interfaces_buffer);
+      if (start_interfaces_buffer.empty() && stop_interfaces_buffer.empty())
+      {
+        RCLCPP_DEBUG(
+          logger, "Component '%s' after filtering has no command interfaces to perform switch",
+          component.get_name().c_str());
+        continue;
+      }
       if (
         component.get_lifecycle_state().id() ==
           lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
@@ -1942,10 +2000,12 @@ bool ResourceManager::perform_command_mode_switch(
         {
           if (
             return_type::OK !=
-            component.perform_command_mode_switch(start_interfaces, stop_interfaces))
+            component.perform_command_mode_switch(start_interfaces_buffer, stop_interfaces_buffer))
           {
             RCLCPP_ERROR(
-              logger, "Component '%s' could not perform switch", component.get_name().c_str());
+              logger, "Component '%s' could not perform switch for the command interfaces: \n%s",
+              component.get_name().c_str(),
+              interfaces_to_string(start_interfaces_buffer, stop_interfaces_buffer).c_str());
             ret = false;
           }
         }
@@ -1956,7 +2016,8 @@ bool ResourceManager::perform_command_mode_switch(
             "Exception of type : %s occurred while performing command mode switch for component "
             "'%s' for the interfaces: \n %s : %s",
             typeid(e).name(), component.get_name().c_str(),
-            interfaces_to_string(start_interfaces, stop_interfaces).c_str(), e.what());
+            interfaces_to_string(start_interfaces_buffer, stop_interfaces_buffer).c_str(),
+            e.what());
           ret = false;
         }
         catch (...)
@@ -1967,16 +2028,27 @@ bool ResourceManager::perform_command_mode_switch(
             "for "
             "the interfaces: \n %s",
             component.get_name().c_str(),
-            interfaces_to_string(start_interfaces, stop_interfaces).c_str());
+            interfaces_to_string(start_interfaces_buffer, stop_interfaces_buffer).c_str());
           ret = false;
         }
       }
+      else
+      {
+        RCLCPP_WARN(
+          logger, "Component '%s' is not in INACTIVE or ACTIVE state, skipping the perform switch",
+          component.get_name().c_str());
+        ret = false;
+      }
     }
     return ret;
   };
 
-  const bool actuators_result = call_perform_mode_switch(resource_storage_->actuators_);
-  const bool systems_result = call_perform_mode_switch(resource_storage_->systems_);
+  const bool actuators_result = call_perform_mode_switch(
+    resource_storage_->actuators_, resource_storage_->start_interfaces_buffer_,
+    resource_storage_->stop_interfaces_buffer_);
+  const bool systems_result = call_perform_mode_switch(
+    resource_storage_->systems_, resource_storage_->start_interfaces_buffer_,
+    resource_storage_->stop_interfaces_buffer_);
 
   if (actuators_result && systems_result)
   {