Make reference interfaces of chainable controllers available in inact…

…ive state.
ros-controls · Dec 17, 2023 · 485b25f · 485b25f
1 parent f9766e2
commit 485b25f
Show file tree

Hide file tree

Showing 3 changed files with 20 additions and 14 deletions.
diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp
@@ -643,6 +643,11 @@ controller_interface::return_type ControllerManager::unload_controller(
   RCLCPP_DEBUG(get_logger(), "Cleanup controller");
   // TODO(destogl): remove reference interface if chainable; i.e., add a separate method for
   // cleaning-up controllers?
+  // Controller will not be removed so make its reference interface available
+  if (controller.c->is_chainable())
+  {
+    resource_manager_->make_controller_reference_interfaces_unavailable(controller_name);
+  }
   controller.c->get_node()->cleanup();
   executor_->remove_node(controller.c->get_node()->get_node_base_interface());
   to.erase(found_it);
@@ -799,6 +804,12 @@ controller_interface::return_type ControllerManager::configure_controller(
   // clear unused list
   rt_controllers_wrapper_.get_unused_list(guard).clear();
 
+  // Controller will not be in INACTIVE state so make its reference interface available
+  if (controller->is_chainable())
+  {
+    resource_manager_->make_controller_reference_interfaces_available(controller_name);
+  }
+
   return controller_interface::return_type::OK;
 }
 

diff --git a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp
@@ -211,7 +211,7 @@ class TestControllerChainingWithControllerManager
     for (const auto & interface : {"pid_left_wheel_controller/velocity"})
     {
       EXPECT_TRUE(cm_->resource_manager_->command_interface_exists(interface));
-      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));
     }
 
@@ -223,7 +223,7 @@ class TestControllerChainingWithControllerManager
     for (const auto & interface : {"pid_right_wheel_controller/velocity"})
     {
       EXPECT_TRUE(cm_->resource_manager_->command_interface_exists(interface));
-      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));
     }
 
@@ -236,7 +236,7 @@ class TestControllerChainingWithControllerManager
           "diff_drive_controller/rot_z"})
     {
       EXPECT_TRUE(cm_->resource_manager_->command_interface_exists(interface));
-      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));
     }
 
@@ -250,7 +250,7 @@ class TestControllerChainingWithControllerManager
           "diff_drive_controller/rot_z"})
     {
       EXPECT_TRUE(cm_->resource_manager_->command_interface_exists(interface));
-      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));
     }
 
@@ -290,25 +290,20 @@ class TestControllerChainingWithControllerManager
     size_t expected_internal_counter, const controller_interface::return_type expected_return,
     bool deactivated, bool claimed_interfaces_from_hw = false)
   {
+    (void)claimed_interfaces_from_hw;
     for (const auto & interface : claimed_command_itfs)
     {
       EXPECT_TRUE(cm_->resource_manager_->command_interface_exists(interface));
       // successful xor deactivated
       if ((expected_return == controller_interface::return_type::OK) != deactivated)
       {
         EXPECT_TRUE(cm_->resource_manager_->command_interface_exists(interface));
+        EXPECT_TRUE(cm_->resource_manager_->command_interface_is_available(interface));
         EXPECT_TRUE(cm_->resource_manager_->command_interface_is_claimed(interface));
       }
       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));
       }
     }

diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp
@@ -1108,7 +1108,7 @@ bool ResourceManager::prepare_command_mode_switch(
       if (!command_interface_exists(interface))
       {
         all_exist = false;
-        ss_not_existing << " " << interface << std::endl;
+        ss_not_existing << "  " << interface << std::endl;
       }
     }
     return all_exist;
@@ -1133,7 +1133,7 @@ bool ResourceManager::prepare_command_mode_switch(
       if (!command_interface_is_available(interface))
       {
         all_available = false;
-        ss_not_available << " " << interface << std::endl;
+        ss_not_available << "  " << interface << std::endl;
       }
     }
     return all_available;