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

…(backport #1218) (#1292) * Add additional checks for non-existing and not available interfaces. (#1218) (cherry picked from commit 8c34ab6) * Resolve conflicts for backport. * Make tests to use hardware description in tests to be able to activate controllers. --------- Co-authored-by: Dr. Denis <[email protected]> Co-authored-by: Bence Magyar <[email protected]>
ros-controls · Jan 17, 2024 · d27bc21 · d27bc21
1 parent d4ea1ce
commit d27bc21
Show file tree

Hide file tree

Showing 13 changed files with 678 additions and 201 deletions.
diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp
@@ -1573,6 +1573,12 @@ void ControllerManager::activate_controllers(
         lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
     }
 
+    // if it is a chainable controller, make the reference interfaces available on activation
+    if (controller->is_chainable())
+    {
+      resource_manager_->make_controller_reference_interfaces_available(controller_name);
+    }
+
     if (controller->is_async())
     {
       async_controller_threads_.at(controller_name)->activate();

diff --git a/controller_manager/test/controller_manager_test_common.hpp b/controller_manager/test/controller_manager_test_common.hpp
@@ -164,7 +164,11 @@ class TestControllerManagerSrvs
 : public ControllerManagerFixture<controller_manager::ControllerManager>
 {
 public:
-  TestControllerManagerSrvs() {}
+  TestControllerManagerSrvs()
+  : ControllerManagerFixture<controller_manager::ControllerManager>(
+      ros2_control_test_assets::minimal_robot_urdf, true)
+  {
+  }
 
   void SetUp() override
   {

diff --git a/controller_manager/test/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp
@@ -284,12 +284,20 @@ TEST_F(TestControllerManagerSrvs, list_chained_controllers_srv)
   ASSERT_EQ("joint1/position", result->controller[1].reference_interfaces[0]);
   ASSERT_EQ("joint1/velocity", result->controller[1].reference_interfaces[1]);
   // activate controllers
-  cm_->switch_controller(
+  auto res = cm_->switch_controller(
     {test_chainable_controller::TEST_CONTROLLER_NAME}, {},
     controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0));
-  cm_->switch_controller(
+  ASSERT_EQ(res, controller_interface::return_type::OK);
+  // we should here wait for the first controller to be activated, i.e., for its reference
+  // interface to become available (mail loop runs on 100 Hz) - so we check the status at least once
+  while (result->controller[1].state != "active")
+  {
+    result = call_service_and_wait(*client, request, srv_executor);
+  }
+  res = cm_->switch_controller(
     {test_controller::TEST_CONTROLLER_NAME}, {},
     controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0));
+  ASSERT_EQ(res, controller_interface::return_type::OK);
   // get controller list after activate
   result = call_service_and_wait(*client, request, srv_executor);
   // check test controller

diff --git a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp
@@ -106,6 +106,12 @@ class TestControllerChainingWithControllerManager
   public testing::WithParamInterface<Strictness>
 {
 public:
+  TestControllerChainingWithControllerManager()
+  : ControllerManagerFixture<TestableControllerManager>(
+      ros2_control_test_assets::minimal_robot_urdf, true)
+  {
+  }
+
   void SetUp()
   {
     executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
@@ -288,7 +294,7 @@ class TestControllerChainingWithControllerManager
   void check_after_de_activate(
     std::shared_ptr<T> & controller, const std::vector<std::string> & claimed_command_itfs,
     size_t expected_internal_counter, const controller_interface::return_type expected_return,
-    bool deactivated, bool claimed_interfaces_from_hw = false)
+    bool deactivated)
   {
     for (const auto & interface : claimed_command_itfs)
     {
@@ -301,14 +307,7 @@ class TestControllerChainingWithControllerManager
       }
       else
       {
-        if (claimed_interfaces_from_hw)
-        {
-          EXPECT_TRUE(cm_->resource_manager_->command_interface_is_available(interface));
-        }
-        else
-        {
-          EXPECT_FALSE(cm_->resource_manager_->command_interface_is_available(interface));
-        }
+        EXPECT_TRUE(cm_->resource_manager_->command_interface_is_available(interface));
         EXPECT_FALSE(cm_->resource_manager_->command_interface_is_claimed(interface));
       }
     }
@@ -350,14 +349,12 @@ class TestControllerChainingWithControllerManager
   void DeactivateAndCheckController(
     std::shared_ptr<T> & controller, const std::string & controller_name,
     const std::vector<std::string> & claimed_command_itfs, size_t expected_internal_counter = 0u,
-    const bool claimed_interfaces_from_hw = false,
     const controller_interface::return_type expected_return = controller_interface::return_type::OK)
   {
     switch_test_controllers(
       {}, {controller_name}, test_param.strictness, std::future_status::timeout, expected_return);
     check_after_de_activate(
-      controller, claimed_command_itfs, expected_internal_counter, expected_return, true,
-      claimed_interfaces_from_hw);
+      controller, claimed_command_itfs, expected_internal_counter, expected_return, true);
   }
 
   void DeactivateController(
@@ -684,9 +681,9 @@ TEST_P(
 
   // all controllers are deactivated --> chained mode is not changed
   DeactivateAndCheckController(
-    pid_left_wheel_controller, PID_LEFT_WHEEL, PID_LEFT_WHEEL_CLAIMED_INTERFACES, 14u, true);
+    pid_left_wheel_controller, PID_LEFT_WHEEL, PID_LEFT_WHEEL_CLAIMED_INTERFACES, 14u);
   DeactivateAndCheckController(
-    pid_right_wheel_controller, PID_RIGHT_WHEEL, PID_RIGHT_WHEEL_CLAIMED_INTERFACES, 14u, true);
+    pid_right_wheel_controller, PID_RIGHT_WHEEL, PID_RIGHT_WHEEL_CLAIMED_INTERFACES, 14u);
   EXPECT_FALSE(pid_left_wheel_controller->is_in_chained_mode());
   EXPECT_FALSE(pid_right_wheel_controller->is_in_chained_mode());
   ASSERT_FALSE(diff_drive_controller->is_in_chained_mode());

diff --git a/hardware_interface/CMakeLists.txt b/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)

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 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.

diff --git a/hardware_interface/src/resource_manager.cpp b/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