[CM] Set chained controller interfaces 'available' for activated cont…

…rollers (#1098)

* control reference interface availability based on the controller state
* update the docs of the chainable controllers output

---------

Co-authored-by: Dr. Denis <denis@stoglrobotics.de>

controller_manager/doc/controller_chaining.rst

@@ -76,7 +76,9 @@ One can also think of it as an actual chain, you can not add a chain link or bre
 Debugging outputs
 ----------------------------
 
-Flag ``unavailable`` on reference interface does not provide much information about anything at the moment. So don't get confused by it. The reason we have it are internal implementation reasons irelevant for the usage.
+- The reference interfaces are ``unavailable`` and ``unclaimed``, when the controller exporting them is in inactive state
+- The reference interfaces are ``available`` and  ``unclaimed``, when the controller exporting them is in an active state but is not in chained mode with any other controller (The controllers gets its references from the subscriber)
+- The reference interfaces are ``available`` and  ``claimed``, when the controller exporting them is in active state and also in chained mode with other controllers (The controller gets its references from the controllers it is chained with)
 
 
 Closing remarks

controller_manager/src/controller_manager.cpp

@@ -1386,6 +1386,11 @@ void ControllerManager::deactivate_controllers(
     {
       const auto new_state = controller->get_node()->deactivate();
       controller->release_interfaces();
+      // if it is a chainable controller, make the reference interfaces unavailable on deactivation
+      if (controller->is_chainable())
+      {
+        resource_manager_->make_controller_reference_interfaces_unavailable(request);
+      }
       if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)
       {
         RCLCPP_ERROR(
@@ -1419,18 +1424,10 @@ void ControllerManager::switch_chained_mode(
     auto controller = found_it->c;
     if (!is_controller_active(*controller))
     {
-      if (controller->set_chained_mode(to_chained_mode))
-      {
-        if (to_chained_mode)
-        {
-          resource_manager_->make_controller_reference_interfaces_available(request);
-        }
-        else
-        {
-          resource_manager_->make_controller_reference_interfaces_unavailable(request);
-        }
-      }
-      else
+      // if it is a chainable controller, make the reference interfaces available on preactivation
+      // (This is needed when you activate a couple of chainable controller altogether)
+      resource_manager_->make_controller_reference_interfaces_available(request);
+      if (!controller->set_chained_mode(to_chained_mode))
       {
         RCLCPP_ERROR(
           get_logger(),
@@ -1567,6 +1564,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(request);
+    }
+
     if (controller->is_async())
     {
       async_controller_threads_.at(controller_name)->activate();

controller_manager/test/test_controller_manager_srvs.cpp

@@ -24,6 +24,7 @@
 #include "controller_manager/controller_manager.hpp"
 #include "controller_manager_msgs/srv/list_controller_types.hpp"
 #include "controller_manager_msgs/srv/list_controllers.hpp"
+#include "controller_manager_msgs/srv/list_hardware_interfaces.hpp"
 #include "controller_manager_msgs/srv/switch_controller.hpp"
 #include "lifecycle_msgs/msg/state.hpp"
 #include "test_chainable_controller/test_chainable_controller.hpp"
@@ -34,6 +35,7 @@ using ::testing::Return;
 using ::testing::UnorderedElementsAre;
 
 using ListControllers = controller_manager_msgs::srv::ListControllers;
+using ListHardwareInterfaces = controller_manager_msgs::srv::ListHardwareInterfaces;
 using TestController = test_controller::TestController;
 using TestChainableController = test_chainable_controller::TestChainableController;
 
@@ -1553,3 +1555,161 @@ TEST_F(TestControllerManagerSrvs, list_sorted_large_chained_controller_tree)
   }
   RCLCPP_INFO(srv_node->get_logger(), "Check successful!");
 }
+
+TEST_F(TestControllerManagerSrvs, list_hardware_interfaces_srv)
+{
+  // create server client and request
+  rclcpp::executors::SingleThreadedExecutor srv_executor;
+  rclcpp::Node::SharedPtr srv_node = std::make_shared<rclcpp::Node>("srv_client");
+  srv_executor.add_node(srv_node);
+  rclcpp::Client<ListHardwareInterfaces>::SharedPtr client =
+    srv_node->create_client<ListHardwareInterfaces>(
+      "test_controller_manager/list_hardware_interfaces");
+  auto request = std::make_shared<ListHardwareInterfaces::Request>();
+
+  // create chained controller
+  auto test_chained_controller = std::make_shared<TestChainableController>();
+  controller_interface::InterfaceConfiguration chained_cmd_cfg = {
+    controller_interface::interface_configuration_type::INDIVIDUAL, {"joint1/position"}};
+  controller_interface::InterfaceConfiguration chained_state_cfg = {
+    controller_interface::interface_configuration_type::INDIVIDUAL,
+    {"joint1/position", "joint1/velocity"}};
+  test_chained_controller->set_command_interface_configuration(chained_cmd_cfg);
+  test_chained_controller->set_state_interface_configuration(chained_state_cfg);
+  test_chained_controller->set_reference_interface_names({"joint1/position", "joint1/velocity"});
+  // create non-chained controller
+  auto test_controller = std::make_shared<TestController>();
+  controller_interface::InterfaceConfiguration cmd_cfg = {
+    controller_interface::interface_configuration_type::INDIVIDUAL,
+    {std::string(test_chainable_controller::TEST_CONTROLLER_NAME) + "/joint1/position",
+     std::string(test_chainable_controller::TEST_CONTROLLER_NAME) + "/joint1/velocity",
+     "joint2/velocity"}};
+  controller_interface::InterfaceConfiguration state_cfg = {
+    controller_interface::interface_configuration_type::INDIVIDUAL,
+    {"joint1/position", "joint1/velocity"}};
+  test_controller->set_command_interface_configuration(cmd_cfg);
+  test_controller->set_state_interface_configuration(state_cfg);
+
+  // add controllers
+  cm_->add_controller(
+    test_chained_controller, test_chainable_controller::TEST_CONTROLLER_NAME,
+    test_chainable_controller::TEST_CONTROLLER_CLASS_NAME);
+  cm_->add_controller(
+    test_controller, test_controller::TEST_CONTROLLER_NAME,
+    test_controller::TEST_CONTROLLER_CLASS_NAME);
+  // get hardware interface list before configure and loading
+  auto initial_result = call_service_and_wait(*client, request, srv_executor);
+  // As there is no controller, so all the interfaces should be available and unclaimed
+  for (const auto & cmd_itrf : initial_result->command_interfaces)
+  {
+    ASSERT_TRUE(cmd_itrf.is_available);
+    ASSERT_FALSE(cmd_itrf.is_claimed);
+  }
+
+  // configure controllers
+  cm_->configure_controller(test_chainable_controller::TEST_CONTROLLER_NAME);
+  cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME);
+
+  // The interfaces should be same available and unclaimed until we activate the controllers
+  auto result = call_service_and_wait(*client, request, srv_executor);
+
+  ASSERT_EQ(2u, result->command_interfaces.size() - initial_result->command_interfaces.size());
+  // There will be no increase in state interfaces
+  ASSERT_EQ(0u, result->state_interfaces.size() - initial_result->state_interfaces.size());
+  // As there is no controller, so all the interfaces should be available and unclaimed
+  for (const auto & cmd_itrf : result->command_interfaces)
+  {
+    // The controller command interface shouldn't be active until it's controller is active
+    if (
+      cmd_itrf.name ==
+        std::string(test_chainable_controller::TEST_CONTROLLER_NAME) + "/joint1/position" ||
+      cmd_itrf.name ==
+        std::string(test_chainable_controller::TEST_CONTROLLER_NAME) + "/joint1/velocity")
+    {
+      ASSERT_FALSE(cmd_itrf.is_available);
+    }
+    else
+    {
+      ASSERT_TRUE(cmd_itrf.is_available);
+    }
+    ASSERT_FALSE(cmd_itrf.is_claimed);
+  }
+  auto find_interface_in_list = [](const std::string & interface, auto & hw_interface_info)
+  {
+    return std::find_if(
+             hw_interface_info.begin(), hw_interface_info.end(),
+             [&](auto info) { return info.name == interface; }) != hw_interface_info.end();
+  };
+  ASSERT_TRUE(find_interface_in_list(
+    std::string(test_chainable_controller::TEST_CONTROLLER_NAME) + "/joint1/position",
+    result->command_interfaces));
+  ASSERT_TRUE(find_interface_in_list(
+    std::string(test_chainable_controller::TEST_CONTROLLER_NAME) + "/joint1/velocity",
+    result->command_interfaces));
+
+  // activate controllers
+  cm_->switch_controller(
+    {test_chainable_controller::TEST_CONTROLLER_NAME}, {},
+    controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0));
+
+  // On activating this chainable controller, we should see that there are 2 more command interfaces
+  // as this chainable controllers exports 2 reference interfaces
+  result = call_service_and_wait(*client, request, srv_executor);
+
+  // There will be no increase upon activation
+  ASSERT_EQ(2u, result->command_interfaces.size() - initial_result->command_interfaces.size());
+  ASSERT_EQ(0u, result->state_interfaces.size() - initial_result->state_interfaces.size());
+
+  for (const auto & cmd_itrf : result->command_interfaces)
+  {
+    ASSERT_TRUE(cmd_itrf.is_available);
+    // This interface is claimed by the chainable controller
+    if (cmd_itrf.name == "joint1/position")
+    {
+      EXPECT_TRUE(cmd_itrf.is_claimed);
+    }
+    else if (
+      cmd_itrf.name ==
+        std::string(test_chainable_controller::TEST_CONTROLLER_NAME) + "/joint1/position" ||
+      cmd_itrf.name ==
+        std::string(test_chainable_controller::TEST_CONTROLLER_NAME) + "/joint1/velocity")
+    {
+      // As these interfaces are exposed by the chainable controller and not yet claimed by other
+      // controllers
+      ASSERT_FALSE(cmd_itrf.is_claimed);
+    }
+    else
+    {
+      // Case for rest of the controllers
+      ASSERT_FALSE(cmd_itrf.is_claimed);
+    }
+  }
+
+  cm_->switch_controller(
+    {test_controller::TEST_CONTROLLER_NAME}, {},
+    controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0));
+
+  // Now as another controller using the interfaces of chainable controller is activated, those
+  // interfaces should reflect as claimed
+  result = call_service_and_wait(*client, request, srv_executor);
+
+  for (const auto & cmd_itrf : result->command_interfaces)
+  {
+    ASSERT_TRUE(cmd_itrf.is_available);
+    // This interface is claimed by the chainable controller
+    if (
+      cmd_itrf.name == "joint1/position" || cmd_itrf.name == "joint2/velocity" ||
+      cmd_itrf.name ==
+        std::string(test_chainable_controller::TEST_CONTROLLER_NAME) + "/joint1/position" ||
+      cmd_itrf.name ==
+        std::string(test_chainable_controller::TEST_CONTROLLER_NAME) + "/joint1/velocity")
+    {
+      ASSERT_TRUE(cmd_itrf.is_claimed);
+    }
+    else
+    {
+      // Case for rest of the controllers
+      ASSERT_FALSE(cmd_itrf.is_claimed);
+    }
+  }
+}

controller_manager/test/test_controllers_chaining_with_controller_manager.cpp

@@ -670,14 +670,14 @@ TEST_P(
   // PositionController is deactivated --> DiffDrive controller is not in chained mode anymore
   DeactivateAndCheckController(
     position_tracking_controller, POSITION_TRACKING_CONTROLLER,
-    POSITION_CONTROLLER_CLAIMED_INTERFACES, 4u);
+    POSITION_CONTROLLER_CLAIMED_INTERFACES, 4u, true);
   EXPECT_TRUE(pid_left_wheel_controller->is_in_chained_mode());
   EXPECT_TRUE(pid_right_wheel_controller->is_in_chained_mode());
   ASSERT_FALSE(diff_drive_controller->is_in_chained_mode());
 
   // DiffDrive (preceding) controller is activated --> PID controller in chained mode
   DeactivateAndCheckController(
-    diff_drive_controller, DIFF_DRIVE_CONTROLLER, DIFF_DRIVE_CLAIMED_INTERFACES, 8u);
+    diff_drive_controller, DIFF_DRIVE_CONTROLLER, DIFF_DRIVE_CLAIMED_INTERFACES, 8u, true);
   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());