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

…(backport #1218) (#1291) * 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. * Fix variable name for clarity. --------- Co-authored-by: Dr. Denis <denis@stoglrobotics.de>
ros-controls · Jan 17, 2024 · a3bf280 · a3bf280
1 parent 7a3a4a1
commit a3bf280
Show file tree

Hide file tree

Showing 13 changed files with 665 additions and 182 deletions.
diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp
@@ -1257,17 +1257,17 @@ void ControllerManager::deactivate_controllers()
   std::vector<ControllerSpec> & rt_controller_list =
     rt_controllers_wrapper_.update_and_get_used_by_rt_list();
   // stop controllers
-  for (const auto & request : deactivate_request_)
+  for (const auto & controller_name : deactivate_request_)
   {
     auto found_it = std::find_if(
       rt_controller_list.begin(), rt_controller_list.end(),
-      std::bind(controller_name_compare, std::placeholders::_1, request));
+      std::bind(controller_name_compare, std::placeholders::_1, controller_name));
     if (found_it == rt_controller_list.end())
     {
       RCLCPP_ERROR(
         get_logger(),
         "Got request to stop controller '%s' but it is not in the realtime controller list",
-        request.c_str());
+        controller_name.c_str());
       continue;
     }
     auto controller = found_it->c;
@@ -1279,7 +1279,7 @@ void ControllerManager::deactivate_controllers()
       {
         RCLCPP_ERROR(
           get_logger(), "After deactivating, controller '%s' is in state '%s', expected Inactive",
-          request.c_str(), new_state.label().c_str());
+          controller_name.c_str(), new_state.label().c_str());
       }
     }
   }
@@ -1291,18 +1291,18 @@ void ControllerManager::switch_chained_mode(
   std::vector<ControllerSpec> & rt_controller_list =
     rt_controllers_wrapper_.update_and_get_used_by_rt_list();
 
-  for (const auto & request : chained_mode_switch_list)
+  for (const auto & controller_name : chained_mode_switch_list)
   {
     auto found_it = std::find_if(
       rt_controller_list.begin(), rt_controller_list.end(),
-      std::bind(controller_name_compare, std::placeholders::_1, request));
+      std::bind(controller_name_compare, std::placeholders::_1, controller_name));
     if (found_it == rt_controller_list.end())
     {
       RCLCPP_FATAL(
         get_logger(),
         "Got request to turn %s chained mode for controller '%s', but controller is not in the "
         "realtime controller list. (This should never happen!)",
-        (to_chained_mode ? "ON" : "OFF"), request.c_str());
+        (to_chained_mode ? "ON" : "OFF"), controller_name.c_str());
       continue;
     }
     auto controller = found_it->c;
@@ -1312,11 +1312,11 @@ void ControllerManager::switch_chained_mode(
       {
         if (to_chained_mode)
         {
-          resource_manager_->make_controller_reference_interfaces_available(request);
+          resource_manager_->make_controller_reference_interfaces_available(controller_name);
         }
         else
         {
-          resource_manager_->make_controller_reference_interfaces_unavailable(request);
+          resource_manager_->make_controller_reference_interfaces_unavailable(controller_name);
         }
       }
       else
@@ -1327,7 +1327,7 @@ void ControllerManager::switch_chained_mode(
           "it! The control will probably not work as expected. Try to restart all controllers. "
           "If "
           "the error persist check controllers' individual configuration.",
-          (to_chained_mode ? "ON" : "OFF"), request.c_str());
+          (to_chained_mode ? "ON" : "OFF"), controller_name.c_str());
       }
     }
     else
@@ -1336,7 +1336,7 @@ void ControllerManager::switch_chained_mode(
         get_logger(),
         "Got request to turn %s chained mode for controller '%s', but this can not happen if "
         "controller is in '%s' state. (This should never happen!)",
-        (to_chained_mode ? "ON" : "OFF"), request.c_str(),
+        (to_chained_mode ? "ON" : "OFF"), controller_name.c_str(),
         hardware_interface::lifecycle_state_names::ACTIVE);
     }
   }
@@ -1346,17 +1346,17 @@ void ControllerManager::activate_controllers()
 {
   std::vector<ControllerSpec> & rt_controller_list =
     rt_controllers_wrapper_.update_and_get_used_by_rt_list();
-  for (const auto & request : activate_request_)
+  for (const auto & controller_name : activate_request_)
   {
     auto found_it = std::find_if(
       rt_controller_list.begin(), rt_controller_list.end(),
-      std::bind(controller_name_compare, std::placeholders::_1, request));
+      std::bind(controller_name_compare, std::placeholders::_1, controller_name));
     if (found_it == rt_controller_list.end())
     {
       RCLCPP_ERROR(
         get_logger(),
         "Got request to activate controller '%s' but it is not in the realtime controller list",
-        request.c_str());
+        controller_name.c_str());
       continue;
     }
     auto controller = found_it->c;
@@ -1385,7 +1385,7 @@ void ControllerManager::activate_controllers()
         RCLCPP_ERROR(
           get_logger(),
           "Resource conflict for controller '%s'. Command interface '%s' is already claimed.",
-          request.c_str(), command_interface.c_str());
+          controller_name.c_str(), command_interface.c_str());
         assignment_successful = false;
         break;
       }
@@ -1395,7 +1395,8 @@ void ControllerManager::activate_controllers()
       }
       catch (const std::exception & e)
       {
-        RCLCPP_ERROR(get_logger(), "Can't activate controller '%s': %s", request.c_str(), e.what());
+        RCLCPP_ERROR(
+          get_logger(), "Can't activate controller '%s': %s", controller_name.c_str(), e.what());
         assignment_successful = false;
         break;
       }
@@ -1429,7 +1430,8 @@ void ControllerManager::activate_controllers()
       }
       catch (const std::exception & e)
       {
-        RCLCPP_ERROR(get_logger(), "Can't activate controller '%s': %s", request.c_str(), e.what());
+        RCLCPP_ERROR(
+          get_logger(), "Can't activate controller '%s': %s", controller_name.c_str(), e.what());
         assignment_successful = false;
         break;
       }
@@ -1451,6 +1453,12 @@ void ControllerManager::activate_controllers()
         hardware_interface::lifecycle_state_names::ACTIVE,
         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);
+    }
   }
   // All controllers activated, switching done
   switch_params_.do_switch = false;

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
@@ -87,6 +87,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>();
@@ -245,7 +251,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)
     {
@@ -258,14 +264,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));
       }
     }
@@ -297,14 +296,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 UpdateAllControllerAndCheck(
@@ -606,9 +603,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
@@ -171,6 +171,10 @@ if(BUILD_TESTING)
   target_link_libraries(test_resource_manager ${PROJECT_NAME})
   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 ${PROJECT_NAME})

diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp
@@ -325,7 +325,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,
@@ -338,6 +340,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.