Add additional checks for non existing and not available interfaces. (#…

…1218)

(cherry picked from commit 8c34ab6)

# Conflicts:
#	hardware_interface/test/mock_components/test_generic_system.cpp

hardware_interface/CMakeLists.txt

@@ -131,6 +131,10 @@ if(BUILD_TESTING)
   target_link_libraries(test_resource_manager hardware_interface)
   ament_target_dependencies(test_resource_manager ros2_control_test_assets)
 
+  ament_add_gmock(test_resource_manager_prepare_perform_switch test/test_resource_manager_prepare_perform_switch.cpp)
+  target_link_libraries(test_resource_manager_prepare_perform_switch hardware_interface)
+  ament_target_dependencies(test_resource_manager_prepare_perform_switch ros2_control_test_assets)
+
   ament_add_gmock(test_generic_system test/mock_components/test_generic_system.cpp)
   target_include_directories(test_generic_system PRIVATE include)
   target_link_libraries(test_generic_system hardware_interface)

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 it is assumed that `prepare_command_mode_switch` is called just before this method
+   * 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.

hardware_interface/src/resource_manager.cpp

@@ -287,6 +287,7 @@ class ResourceStorage
 
     if (result)
     {
+      remove_all_hardware_interfaces_from_available_list(hardware.get_name());
       async_component_threads_.erase(hardware.get_name());
       // TODO(destogl): change this - deimport all things if there is there are interfaces there
       // deimport_non_movement_command_interfaces(hardware);
@@ -1072,6 +1073,12 @@ bool ResourceManager::prepare_command_mode_switch(
   const std::vector<std::string> & start_interfaces,
   const std::vector<std::string> & stop_interfaces)
 {
+  // When only broadcaster is activated then this lists are empty
+  if (start_interfaces.empty() && stop_interfaces.empty())
+  {
+    return true;
+  }
+
   auto interfaces_to_string = [&]()
   {
     std::stringstream ss;
@@ -1090,55 +1097,125 @@ bool ResourceManager::prepare_command_mode_switch(
     return ss.str();
   };
 
-  for (auto & component : resource_storage_->actuators_)
+  // Check if interface exists
+  std::stringstream ss_not_existing;
+  ss_not_existing << "Not existing: " << std::endl << "[" << std::endl;
+  auto check_exist = [&](const std::vector<std::string> & list_to_check)
   {
-    if (return_type::OK != component.prepare_command_mode_switch(start_interfaces, stop_interfaces))
+    bool all_exist = true;
+    for (const auto & interface : list_to_check)
     {
-      RCUTILS_LOG_ERROR_NAMED(
-        "resource_manager", "Component '%s' did not accept new command resource combination: \n %s",
-        component.get_name().c_str(), interfaces_to_string().c_str());
-      return false;
+      if (!command_interface_exists(interface))
+      {
+        all_exist = false;
+        ss_not_existing << " " << interface << std::endl;
+      }
     }
+    return all_exist;
+  };
+  if (!(check_exist(start_interfaces) && check_exist(stop_interfaces)))
+  {
+    ss_not_existing << "]" << std::endl;
+    RCUTILS_LOG_ERROR_NAMED(
+      "resource_manager", "Not acceptable command interfaces combination: \n%s%s",
+      interfaces_to_string().c_str(), ss_not_existing.str().c_str());
+    return false;
   }
-  for (auto & component : resource_storage_->systems_)
+
+  // Check if interfaces are available
+  std::stringstream ss_not_available;
+  ss_not_available << "Not available: " << std::endl << "[" << std::endl;
+  auto check_available = [&](const std::vector<std::string> & list_to_check)
   {
-    if (return_type::OK != component.prepare_command_mode_switch(start_interfaces, stop_interfaces))
+    bool all_available = true;
+    for (const auto & interface : list_to_check)
     {
-      RCUTILS_LOG_ERROR_NAMED(
-        "resource_manager", "Component '%s' did not accept new command resource combination: \n %s",
-        component.get_name().c_str(), interfaces_to_string().c_str());
-      return false;
+      if (!command_interface_is_available(interface))
+      {
+        all_available = false;
+        ss_not_available << " " << interface << std::endl;
+      }
     }
+    return all_available;
+  };
+  if (!(check_available(start_interfaces) && check_available(stop_interfaces)))
+  {
+    ss_not_available << "]" << std::endl;
+    RCUTILS_LOG_ERROR_NAMED(
+      "resource_manager", "Not acceptable command interfaces combination: \n%s%s",
+      interfaces_to_string().c_str(), ss_not_available.str().c_str());
+    return false;
   }
-  return true;
+
+  auto call_prepare_mode_switch =
+    [&start_interfaces, &stop_interfaces, &interfaces_to_string](auto & components)
+  {
+    bool ret = true;
+    for (auto & component : components)
+    {
+      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.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 ret;
+  };
+
+  const bool actuators_result = call_prepare_mode_switch(resource_storage_->actuators_);
+  const bool systems_result = call_prepare_mode_switch(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_perform_mode_switch = [&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_perform_mode_switch(resource_storage_->actuators_);
+  const bool systems_result = call_perform_mode_switch(resource_storage_->systems_);
+
+  return actuators_result && systems_result;
 }
 
 // CM API: Called in "callback/slow"-thread