Optimize prepre/perform switch in RM and add additional documentation…

… about it.
ros-controls · Dec 18, 2023 · 0a6b107 · 0a6b107
1 parent 44e3b72
commit 0a6b107
Show file tree

Hide file tree

Showing 4 changed files with 65 additions and 47 deletions.
diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp
@@ -331,7 +331,9 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager
    * by default
    * \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 true if switch can be prepared, false if a component rejects switch request.
+   * \return true if switch can be prepared; false if a component rejects switch request, and if
+   * at least one of the input interfaces are not existing or not available (i.e., component is not
+   * in ACTIVE or INACTIVE state).
    */
   bool prepare_command_mode_switch(
     const std::vector<std::string> & start_interfaces,
@@ -344,6 +346,8 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager
    * \note this is intended for mode-switching when a hardware interface needs to change
    * control mode depending on which command interface is claimed.
    * \note this is for realtime switching of the command interface.
+   * \note is is assumed that `prepare_command_mode_switch` is called just before this methods
+   * with the same input arguments.
    * \param[in] start_interfaces vector of string identifiers for the command interfaces starting.
    * \param[in] stop_interfaces vector of string identifiers for the command interfacs stopping.
    * \return true if switch is performed, false if a component rejects switching.

diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp
@@ -1147,70 +1147,75 @@ bool ResourceManager::prepare_command_mode_switch(
     return false;
   }
 
-  using lifecycle_msgs::msg::State;
-
-  // Check now if component allows the given interface combination
-  for (auto & component : resource_storage_->actuators_)
-  {
-    if (
-      component.get_state().id() == State::PRIMARY_STATE_INACTIVE ||
-      component.get_state().id() == State::PRIMARY_STATE_ACTIVE)
-    {
-      if (
-        return_type::OK != component.prepare_command_mode_switch(start_interfaces, stop_interfaces))
-      {
-        RCUTILS_LOG_ERROR_NAMED(
-          "resource_manager", "Component '%s' did not accept command interfaces combination: \n%s",
-          component.get_name().c_str(), interfaces_to_string().c_str());
-        return false;
-      }
-    }
-  }
-  for (auto & component : resource_storage_->systems_)
+  auto call_method_on_components =
+    [&start_interfaces, &stop_interfaces, &interfaces_to_string](auto & components)
   {
-    if (
-      component.get_state().id() == State::PRIMARY_STATE_INACTIVE ||
-      component.get_state().id() == State::PRIMARY_STATE_ACTIVE)
+    bool ret = true;
+    for (auto & component : components)
     {
       if (
-        return_type::OK != component.prepare_command_mode_switch(start_interfaces, stop_interfaces))
+        component.get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
+        component.get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
       {
-        RCUTILS_LOG_ERROR_NAMED(
-          "resource_manager", "Component '%s' did not accept command interfaces combination: \n%s",
-          component.get_name().c_str(), interfaces_to_string().c_str());
-        return false;
+        if (
+          return_type::OK !=
+          component.prepare_command_mode_switch(start_interfaces, stop_interfaces))
+        {
+          RCUTILS_LOG_ERROR_NAMED(
+            "resource_manager",
+            "Component '%s' did not accept command interfaces combination: \n%s",
+            component.get_name().c_str(), interfaces_to_string().c_str());
+          ret = false;
+        }
       }
     }
-  }
-  return true;
+    return ret;
+  };
+
+  const bool actuators_result = call_method_on_components(resource_storage_->actuators_);
+  const bool systems_result = call_method_on_components(resource_storage_->systems_);
+
+  return actuators_result && systems_result;
 }
 
 // CM API: Called in "update"-thread
 bool ResourceManager::perform_command_mode_switch(
   const std::vector<std::string> & start_interfaces,
   const std::vector<std::string> & stop_interfaces)
 {
-  for (auto & component : resource_storage_->actuators_)
+  // When only broadcaster is activated then this lists are empty
+  if (start_interfaces.empty() && stop_interfaces.empty())
   {
-    if (return_type::OK != component.perform_command_mode_switch(start_interfaces, stop_interfaces))
-    {
-      RCUTILS_LOG_ERROR_NAMED(
-        "resource_manager", "Component '%s' could not perform switch",
-        component.get_name().c_str());
-      return false;
-    }
+    return true;
   }
-  for (auto & component : resource_storage_->systems_)
+
+  auto call_method_on_components = [&start_interfaces, &stop_interfaces](auto & components)
   {
-    if (return_type::OK != component.perform_command_mode_switch(start_interfaces, stop_interfaces))
+    bool ret = true;
+    for (auto & component : components)
     {
-      RCUTILS_LOG_ERROR_NAMED(
-        "resource_manager", "Component '%s' could not perform switch",
-        component.get_name().c_str());
-      return false;
+      if (
+        component.get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
+        component.get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
+      {
+        if (
+          return_type::OK !=
+          component.perform_command_mode_switch(start_interfaces, stop_interfaces))
+        {
+          RCUTILS_LOG_ERROR_NAMED(
+            "resource_manager", "Component '%s' could not perform switch",
+            component.get_name().c_str());
+          ret = false;
+        }
+      }
     }
-  }
-  return true;
+    return ret;
+  };
+
+  const bool actuators_result = call_method_on_components(resource_storage_->actuators_);
+  const bool systems_result = call_method_on_components(resource_storage_->systems_);
+
+  return actuators_result && systems_result;
 }
 
 // CM API: Called in "callback/slow"-thread

diff --git a/hardware_interface/test/test_components/test_actuator.cpp b/hardware_interface/test/test_components/test_actuator.cpp
@@ -82,6 +82,14 @@ class TestActuator : public ActuatorInterface
     return hardware_interface::return_type::OK;
   }
 
+  hardware_interface::return_type perform_command_mode_switch(
+    const std::vector<std::string> & /*start_interfaces*/,
+    const std::vector<std::string> & /*stop_interfaces*/) override
+  {
+    position_state_ += 100.0;
+    return hardware_interface::return_type::OK;
+  }
+
   return_type read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override
   {
     // simulate error on read

diff --git a/hardware_interface/test/test_hardware_components/test_system_with_command_modes.cpp b/hardware_interface/test/test_hardware_components/test_system_with_command_modes.cpp
@@ -168,6 +168,7 @@ class TestSystemCommandModes : public hardware_interface::SystemInterface
     const std::vector<std::string> & start_interfaces,
     const std::vector<std::string> & /*stop_interfaces*/) override
   {
+    acceleration_state_[0] += 100.0;
     // Test of failure in perform command mode switch
     // Fail if given an empty list.
     // This should never occur in a real system as the same start_interfaces list is sent to both