controller_manager: Use command_interface_configuration for the claimed interfaces when calling list_controllers

controller_manager/src/controller_manager.cpp

@@ -861,6 +861,30 @@ void ControllerManager::list_controllers_srv_cb(
     cs.type = controllers[i].info.type;
     cs.claimed_interfaces = controllers[i].info.claimed_interfaces;
     cs.state = controllers[i].c->get_state().label();
+
+    // Get information about interfaces
+    auto command_interface_config = controllers[i].c->command_interface_configuration();
+    if (command_interface_config.type == controller_interface::interface_configuration_type::ALL)
+    {
+      cs.required_command_interfaces = resource_manager_->command_interface_keys();
+    }
+    else if (
+      command_interface_config.type ==
+      controller_interface::interface_configuration_type::INDIVIDUAL)
+    {
+      cs.required_command_interfaces = command_interface_config.names;
+    }
+
+    auto state_interface_config = controllers[i].c->state_interface_configuration();
+    if (state_interface_config.type == controller_interface::interface_configuration_type::ALL)
+    {
+      cs.required_state_interfaces = resource_manager_->state_interface_keys();
+    }
+    else if (
+      state_interface_config.type == controller_interface::interface_configuration_type::INDIVIDUAL)
+    {
+      cs.required_state_interfaces = state_interface_config.names;
+    }
   }
 
   RCLCPP_DEBUG(get_logger(), "list controller service finished");

controller_manager/test/test_controller/test_controller.cpp

@@ -66,6 +66,12 @@ void TestController::set_command_interface_configuration(
   cmd_iface_cfg_ = cfg;
 }
 
+void TestController::set_state_interface_configuration(
+  const controller_interface::InterfaceConfiguration & cfg)
+{
+  state_iface_cfg_ = cfg;
+}
+
 }  // namespace test_controller
 
 #include "pluginlib/class_list_macros.hpp"

controller_manager/test/test_controller/test_controller.hpp

@@ -44,8 +44,7 @@ class TestController : public controller_interface::ControllerInterface
 
   controller_interface::InterfaceConfiguration state_interface_configuration() const override
   {
-    return controller_interface::InterfaceConfiguration{
-      controller_interface::interface_configuration_type::NONE};
+    return state_iface_cfg_;
   }
 
   CONTROLLER_MANAGER_PUBLIC
@@ -67,12 +66,16 @@ class TestController : public controller_interface::ControllerInterface
   void set_command_interface_configuration(
     const controller_interface::InterfaceConfiguration & cfg);
 
+  CONTROLLER_MANAGER_PUBLIC
+  void set_state_interface_configuration(const controller_interface::InterfaceConfiguration & cfg);
+
   size_t internal_counter = 0;
   bool simulate_cleanup_failure = false;
   // Variable where we store when cleanup was called, pointer because the controller
   // is usually destroyed after cleanup
   size_t * cleanup_calls = nullptr;
   controller_interface::InterfaceConfiguration cmd_iface_cfg_;
+  controller_interface::InterfaceConfiguration state_iface_cfg_;
 };
 
 }  // namespace test_controller

controller_manager/test/test_controller_manager_srvs.cpp

@@ -119,10 +119,14 @@ TEST_F(TestControllerManagerSrvs, list_controllers_srv)
   ASSERT_EQ(0u, result->controller.size());
 
   auto test_controller = std::make_shared<test_controller::TestController>();
-  controller_interface::InterfaceConfiguration cfg = {
+  controller_interface::InterfaceConfiguration cmd_cfg = {
     controller_interface::interface_configuration_type::INDIVIDUAL,
     {"joint1/position", "joint2/velocity"}};
-  test_controller->set_command_interface_configuration(cfg);
+  controller_interface::InterfaceConfiguration state_cfg = {
+    controller_interface::interface_configuration_type::INDIVIDUAL,
+    {"joint1/position", "joint1/velocity", "joint2/position"}};
+  test_controller->set_command_interface_configuration(cmd_cfg);
+  test_controller->set_state_interface_configuration(state_cfg);
   auto abstract_test_controller = cm_->add_controller(
     test_controller, test_controller::TEST_CONTROLLER_NAME,
     test_controller::TEST_CONTROLLER_CLASS_NAME);
@@ -133,12 +137,24 @@ TEST_F(TestControllerManagerSrvs, list_controllers_srv)
   ASSERT_EQ(test_controller::TEST_CONTROLLER_CLASS_NAME, result->controller[0].type);
   ASSERT_EQ("unconfigured", result->controller[0].state);
   ASSERT_TRUE(result->controller[0].claimed_interfaces.empty());
+  ASSERT_THAT(
+    result->controller[0].required_command_interfaces,
+    testing::ElementsAre("joint1/position", "joint2/velocity"));
+  ASSERT_THAT(
+    result->controller[0].required_state_interfaces,
+    testing::ElementsAre("joint1/position", "joint1/velocity", "joint2/position"));
 
   cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME);
   result = call_service_and_wait(*client, request, srv_executor);
   ASSERT_EQ(1u, result->controller.size());
   ASSERT_EQ("inactive", result->controller[0].state);
   ASSERT_TRUE(result->controller[0].claimed_interfaces.empty());
+  ASSERT_THAT(
+    result->controller[0].required_command_interfaces,
+    testing::ElementsAre("joint1/position", "joint2/velocity"));
+  ASSERT_THAT(
+    result->controller[0].required_state_interfaces,
+    testing::ElementsAre("joint1/position", "joint1/velocity", "joint2/position"));
 
   cm_->switch_controller(
     {test_controller::TEST_CONTROLLER_NAME}, {},
@@ -150,6 +166,12 @@ TEST_F(TestControllerManagerSrvs, list_controllers_srv)
   ASSERT_THAT(
     result->controller[0].claimed_interfaces,
     testing::ElementsAre("joint1/position", "joint2/velocity"));
+  ASSERT_THAT(
+    result->controller[0].required_command_interfaces,
+    testing::ElementsAre("joint1/position", "joint2/velocity"));
+  ASSERT_THAT(
+    result->controller[0].required_state_interfaces,
+    testing::ElementsAre("joint1/position", "joint1/velocity", "joint2/position"));
 
   cm_->switch_controller(
     {}, {test_controller::TEST_CONTROLLER_NAME},
@@ -159,9 +181,17 @@ TEST_F(TestControllerManagerSrvs, list_controllers_srv)
   ASSERT_EQ(1u, result->controller.size());
   ASSERT_EQ("inactive", result->controller[0].state);
   ASSERT_TRUE(result->controller[0].claimed_interfaces.empty());
+  ASSERT_THAT(
+    result->controller[0].required_command_interfaces,
+    testing::ElementsAre("joint1/position", "joint2/velocity"));
+  ASSERT_THAT(
+    result->controller[0].required_state_interfaces,
+    testing::ElementsAre("joint1/position", "joint1/velocity", "joint2/position"));
 
-  cfg = {controller_interface::interface_configuration_type::ALL};
-  test_controller->set_command_interface_configuration(cfg);
+  cmd_cfg = {controller_interface::interface_configuration_type::ALL};
+  test_controller->set_command_interface_configuration(cmd_cfg);
+  state_cfg = {controller_interface::interface_configuration_type::ALL};
+  test_controller->set_state_interface_configuration(state_cfg);
   cm_->switch_controller(
     {test_controller::TEST_CONTROLLER_NAME}, {},
     controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0));
@@ -172,6 +202,15 @@ TEST_F(TestControllerManagerSrvs, list_controllers_srv)
   ASSERT_THAT(
     result->controller[0].claimed_interfaces,
     testing::ElementsAre("joint1/position", "joint2/velocity", "joint3/velocity"));
+  ASSERT_THAT(
+    result->controller[0].required_command_interfaces,
+    testing::ElementsAre("joint1/position", "joint2/velocity", "joint3/velocity"));
+  ASSERT_THAT(
+    result->controller[0].required_state_interfaces,
+    testing::ElementsAre(
+      "joint1/position", "joint1/some_unlisted_interface", "joint1/velocity", "joint2/acceleration",
+      "joint2/position", "joint2/velocity", "joint3/acceleration", "joint3/position",
+      "joint3/velocity", "sensor1/velocity"));
 
   cm_->switch_controller(
     {}, {test_controller::TEST_CONTROLLER_NAME},
@@ -181,6 +220,15 @@ TEST_F(TestControllerManagerSrvs, list_controllers_srv)
   ASSERT_EQ(1u, result->controller.size());
   ASSERT_EQ("inactive", result->controller[0].state);
   ASSERT_TRUE(result->controller[0].claimed_interfaces.empty());
+  ASSERT_THAT(
+    result->controller[0].required_command_interfaces,
+    testing::ElementsAre("joint1/position", "joint2/velocity", "joint3/velocity"));
+  ASSERT_THAT(
+    result->controller[0].required_state_interfaces,
+    testing::ElementsAre(
+      "joint1/position", "joint1/some_unlisted_interface", "joint1/velocity", "joint2/acceleration",
+      "joint2/position", "joint2/velocity", "joint3/acceleration", "joint3/position",
+      "joint3/velocity", "sensor1/velocity"));
 
   ASSERT_EQ(
     controller_interface::return_type::OK,

controller_manager_msgs/msg/ControllerState.msg

@@ -2,3 +2,5 @@ string name
 string state
 string type
 string[] claimed_interfaces
+string[] required_command_interfaces
+string[] required_state_interfaces

controller_manager_msgs/srv/ListControllers.srv

@@ -2,4 +2,4 @@
 # controllers that are loaded inside the controller_manager.
 
 ---
-ControllerState[] controller
+controller_manager_msgs/ControllerState[] controller

ros2controlcli/ros2controlcli/verb/list_controllers.py

@@ -26,12 +26,39 @@ class ListControllersVerb(VerbExtension):
 
     def add_arguments(self, parser, cli_name):
         add_arguments(parser)
+        parser.add_argument(
+            "--claimed-interfaces",
+            action="store_true",
+            help="List controller's claimed interfaces",
+        )
+        parser.add_argument(
+            "--required-state-interfaces",
+            action="store_true",
+            help="List controller's required state interfaces",
+        )
+        parser.add_argument(
+            "--required-command-interfaces",
+            action="store_true",
+            help="List controller's required command interfaces",
+        )
         add_controller_mgr_parsers(parser)
 
     def main(self, *, args):
         with NodeStrategy(args) as node:
             controllers = list_controllers(node, args.controller_manager).controller
             for c in controllers:
-                print(f'{c.name:20s}[{c.type:20s}] {c.state:10s}')
+                print(f"{c.name:20s}[{c.type:20s}] {c.state:10s}")
+                if args.claimed_interfaces:
+                    print("claimed interfaces:")
+                    for claimed_interface in c.claimed_interfaces:
+                        print(f"\t{claimed_interface}")
+                if args.required_command_interfaces:
+                    print("required command interfaces:")
+                    for required_command_interface in c.required_command_interfaces:
+                        print(f"\t{required_command_interface}")
+                if args.required_state_interfaces:
+                    print("required state interfaces:")
+                    for required_state_interface in c.required_state_interfaces:
+                        print(f"\t{required_state_interface}")
 
             return 0