Fixup the tests.

ros-controls · Jan 17, 2024 · 4fe9862 · 4fe9862
1 parent 4821105
commit 4fe9862
Show file tree

Hide file tree

Showing 3 changed files with 17 additions and 14 deletions.
diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp
@@ -1451,6 +1451,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/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp
@@ -288,6 +288,12 @@ TEST_F(TestControllerManagerSrvs, list_chained_controllers_srv)
     {test_chainable_controller::TEST_CONTROLLER_NAME}, {},
     controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0));
   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));

diff --git a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp
@@ -251,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)
     {
@@ -264,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));
       }
     }
@@ -303,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(
@@ -612,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());