[CM] Add controller_manager activity topic (#2006)

* Add LifecycleInfo and ControllerManagerStatus message types

* Add latched publisher of the controller manager activity

* Rename the LifecycleInfo to NamedLifecycleState message type

* Add `set_on_switch_callback` method to the RTControllersWrapper

* Add `set_on_component_state_switch_callback` method to ResourceManager

* Add test for the controller activity message

* update release_notes.rst

* Change the message from ControllerManagerStatus to ControllerManagerActivity

* Use on_* in the variable names

* Add documentation of the new publisher

* Add comments to the newly added variables

* Make reliable explicit on the QoS

* Use `set_component_state` instead of directly deactivating it

* Publish the activity of the fallback controllers also

* Fix pre-commit

Author: saikishor

controller_manager/doc/userdoc.rst

@@ -45,6 +45,14 @@ Alternatives to the standard kernel include
 Though installing a realtime-kernel will definitely get the best results when it comes to low
 jitter, using a lowlatency kernel can improve things a lot with being really easy to install.
 
+Publishers
+-----------
+
+~/activity [controller_manager_msgs::msg::ControllerManagerActivity]
+  A topic that is published every time there is a change of state of the controllers or hardware components managed by the controller manager.
+  The message contains the list of the controllers and the hardware components that are managed by the controller manager along with their lifecycle states.
+  The topic is published using the "transient local" quality of service, so subscribers should also be "transient local".
+
 Subscribers
 -----------
 

controller_manager/include/controller_manager/controller_manager.hpp

@@ -27,6 +27,7 @@
 #include "controller_interface/controller_interface_base.hpp"
 
 #include "controller_manager/controller_spec.hpp"
+#include "controller_manager_msgs/msg/controller_manager_activity.hpp"
 #include "controller_manager_msgs/srv/configure_controller.hpp"
 #include "controller_manager_msgs/srv/list_controller_types.hpp"
 #include "controller_manager_msgs/srv/list_controllers.hpp"
@@ -430,6 +431,13 @@ class ControllerManager : public rclcpp::Node
     const std::string & ctrl_name, std::vector<std::string>::iterator controller_iterator,
     bool append_to_controller);
 
+  /**
+   * @brief Method to publish the state of the controller manager.
+   * The state includes the list of controllers and the list of hardware interfaces along with
+   * their states.
+   */
+  void publish_activity();
+
   void controller_activity_diagnostic_callback(diagnostic_updater::DiagnosticStatusWrapper & stat);
 
   void hardware_components_diagnostic_callback(diagnostic_updater::DiagnosticStatusWrapper & stat);
@@ -521,6 +529,12 @@ class ControllerManager : public rclcpp::Node
      */
     void switch_updated_list(const std::lock_guard<std::recursive_mutex> & guard);
 
+    /// A method to register a callback to be called when the list is switched
+    /**
+     * \param[in] callback Callback to be called when the list is switched
+     */
+    void set_on_switch_callback(std::function<void()> callback);
+
     // Mutex protecting the controllers list
     // must be acquired before using any list other than the "used by rt"
     mutable std::recursive_mutex controllers_lock_;
@@ -542,6 +556,8 @@ class ControllerManager : public rclcpp::Node
     int updated_controllers_index_ = 0;
     /// The index of the controllers list being used in the real-time thread.
     int used_by_realtime_controllers_index_ = -1;
+    /// The callback to be called when the list is switched
+    std::function<void()> on_switch_callback_ = nullptr;
   };
 
   std::unique_ptr<rclcpp::PreShutdownCallbackHandle> preshutdown_cb_handle_{nullptr};
@@ -551,6 +567,8 @@ class ControllerManager : public rclcpp::Node
   /// mutex copied from ROS1 Control, protects service callbacks
   /// not needed if we're guaranteed that the callbacks don't come from multiple threads
   std::mutex services_lock_;
+  rclcpp::Publisher<controller_manager_msgs::msg::ControllerManagerActivity>::SharedPtr
+    controller_manager_activity_publisher_;
   rclcpp::Service<controller_manager_msgs::srv::ListControllers>::SharedPtr
     list_controllers_service_;
   rclcpp::Service<controller_manager_msgs::srv::ListControllerTypes>::SharedPtr

controller_manager/src/controller_manager.cpp

@@ -373,6 +373,14 @@ bool ControllerManager::shutdown_controllers()
 
 void ControllerManager::init_controller_manager()
 {
+  controller_manager_activity_publisher_ =
+    create_publisher<controller_manager_msgs::msg::ControllerManagerActivity>(
+      "~/activity", rclcpp::QoS(1).reliable().transient_local());
+  rt_controllers_wrapper_.set_on_switch_callback(
+    std::bind(&ControllerManager::publish_activity, this));
+  resource_manager_->set_on_component_state_switch_callback(
+    std::bind(&ControllerManager::publish_activity, this));
+
   // Get parameters needed for RT "update" loop to work
   if (is_resource_manager_initialized())
   {
@@ -2695,6 +2703,8 @@ controller_interface::return_type ControllerManager::update(
     {
       activate_controllers(rt_controller_list, rt_buffer_.fallback_controllers_list);
     }
+    // To publish the activity of the failing controllers and the fallback controllers
+    publish_activity();
   }
 
   // there are controllers to (de)activate
@@ -2784,6 +2794,17 @@ void ControllerManager::RTControllerListWrapper::switch_updated_list(
   int former_current_controllers_list_ = updated_controllers_index_;
   updated_controllers_index_ = get_other_list(former_current_controllers_list_);
   wait_until_rt_not_using(former_current_controllers_list_);
+  if (on_switch_callback_)
+  {
+    on_switch_callback_();
+  }
+}
+
+void ControllerManager::RTControllerListWrapper::set_on_switch_callback(
+  std::function<void()> callback)
+{
+  std::lock_guard<std::recursive_mutex> guard(controllers_lock_);
+  on_switch_callback_ = callback;
 }
 
 int ControllerManager::RTControllerListWrapper::get_other_list(int index) const
@@ -3231,6 +3252,38 @@ ControllerManager::check_fallback_controllers_state_pre_activation(
   return controller_interface::return_type::OK;
 }
 
+void ControllerManager::publish_activity()
+{
+  controller_manager_msgs::msg::ControllerManagerActivity status_msg;
+  status_msg.header.stamp = get_clock()->now();
+  {
+    // lock controllers
+    std::lock_guard<std::recursive_mutex> guard(rt_controllers_wrapper_.controllers_lock_);
+    const std::vector<ControllerSpec> & controllers =
+      rt_controllers_wrapper_.get_updated_list(guard);
+    for (const auto & controller : controllers)
+    {
+      controller_manager_msgs::msg::NamedLifecycleState lifecycle_info;
+      lifecycle_info.name = controller.info.name;
+      lifecycle_info.state.id = controller.c->get_lifecycle_state().id();
+      lifecycle_info.state.label = controller.c->get_lifecycle_state().label();
+      status_msg.controllers.push_back(lifecycle_info);
+    }
+  }
+  {
+    const auto hw_components_info = resource_manager_->get_components_status();
+    for (const auto & [component_name, component_info] : hw_components_info)
+    {
+      controller_manager_msgs::msg::NamedLifecycleState lifecycle_info;
+      lifecycle_info.name = component_name;
+      lifecycle_info.state.id = component_info.state.id();
+      lifecycle_info.state.label = component_info.state.label();
+      status_msg.hardware_components.push_back(lifecycle_info);
+    }
+  }
+  controller_manager_activity_publisher_->publish(status_msg);
+}
+
 void ControllerManager::controller_activity_diagnostic_callback(
   diagnostic_updater::DiagnosticStatusWrapper & stat)
 {

controller_manager/test/test_controller_manager.cpp

@@ -18,8 +18,10 @@
 #include <vector>
 
 #include "controller_manager/controller_manager.hpp"
+#include "controller_manager_msgs/msg/controller_manager_activity.hpp"
 #include "controller_manager_test_common.hpp"
 #include "lifecycle_msgs/msg/state.hpp"
+#include "rclcpp/executor.hpp"
 #include "test_chainable_controller/test_chainable_controller.hpp"
 #include "test_controller/test_controller.hpp"
 
@@ -30,6 +32,46 @@ class TestControllerManagerWithStrictness
 : public ControllerManagerFixture<controller_manager::ControllerManager>,
   public testing::WithParamInterface<Strictness>
 {
+public:
+  void get_cm_status_message(
+    const std::string & topic, controller_manager_msgs::msg::ControllerManagerActivity & cm_msg)
+  {
+    controller_manager_msgs::msg::ControllerManagerActivity::SharedPtr received_msg;
+    rclcpp::Node test_node("test_node");
+    auto subs_callback =
+      [&](const controller_manager_msgs::msg::ControllerManagerActivity::SharedPtr msg)
+    { received_msg = msg; };
+    auto subscription =
+      test_node.create_subscription<controller_manager_msgs::msg::ControllerManagerActivity>(
+        topic, rclcpp::QoS(1).reliable().transient_local(), subs_callback);
+
+    rclcpp::executors::SingleThreadedExecutor executor;
+    executor.add_node(test_node.get_node_base_interface());
+    std::this_thread::sleep_for(std::chrono::milliseconds(200));
+
+    // call update to publish the test value
+    // since update doesn't guarantee a published message, republish until received
+    int max_sub_check_loop_count = 5;  // max number of tries for pub/sub loop
+    while (max_sub_check_loop_count--)
+    {
+      const auto timeout = std::chrono::milliseconds{50};
+      const auto until = test_node.get_clock()->now() + timeout;
+      while (!received_msg && test_node.get_clock()->now() < until)
+      {
+        executor.spin_some();
+        std::this_thread::sleep_for(std::chrono::microseconds(10));
+      }
+      // check if message has been received
+      if (received_msg.get())
+      {
+        break;
+      }
+    }
+    ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through "
+                                              "controller manager activity";
+    ASSERT_TRUE(received_msg);
+    cm_msg = *received_msg;
+  }
 };
 
 class TestControllerManagerRobotDescription
@@ -65,6 +107,15 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle)
   const auto test_param = GetParam();
   auto test_controller = std::make_shared<test_controller::TestController>();
   auto test_controller2 = std::make_shared<test_controller::TestController>();
+
+  // Check for the hardware component and no controllers
+  controller_manager_msgs::msg::ControllerManagerActivity cm_msg;
+  const std::string cm_activity_topic =
+    std::string("/") + std::string(TEST_CM_NAME) + std::string("/activity");
+  get_cm_status_message(cm_activity_topic, cm_msg);
+  ASSERT_EQ(cm_msg.hardware_components.size(), 3u);
+  ASSERT_EQ(cm_msg.controllers.size(), 0u);
+
   constexpr char TEST_CONTROLLER2_NAME[] = "test_controller2_name";
   cm_->add_controller(
     test_controller, test_controller::TEST_CONTROLLER_NAME,
@@ -74,6 +125,14 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle)
   EXPECT_EQ(2u, cm_->get_loaded_controllers().size());
   EXPECT_EQ(2, test_controller.use_count());
 
+  get_cm_status_message(cm_activity_topic, cm_msg);
+  ASSERT_EQ(cm_msg.hardware_components.size(), 3u);
+  ASSERT_EQ(cm_msg.controllers.size(), 2u);
+  ASSERT_EQ(cm_msg.controllers[0].name, test_controller::TEST_CONTROLLER_NAME);
+  ASSERT_EQ(cm_msg.controllers[0].state.id, lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
+  ASSERT_EQ(cm_msg.controllers[1].name, TEST_CONTROLLER2_NAME);
+  ASSERT_EQ(cm_msg.controllers[1].state.id, lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
+
   // setup interface to claim from controllers
   controller_interface::InterfaceConfiguration cmd_itfs_cfg;
   cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL;
@@ -137,6 +196,14 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle)
     cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME);
     cm_->configure_controller(TEST_CONTROLLER2_NAME);
   }
+  get_cm_status_message(cm_activity_topic, cm_msg);
+  ASSERT_EQ(cm_msg.hardware_components.size(), 3u);
+  ASSERT_EQ(cm_msg.controllers.size(), 2u);
+  ASSERT_EQ(cm_msg.controllers[0].name, TEST_CONTROLLER2_NAME);
+  ASSERT_EQ(cm_msg.controllers[0].state.id, lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE);
+  ASSERT_EQ(cm_msg.controllers[1].name, test_controller::TEST_CONTROLLER_NAME);
+  ASSERT_EQ(cm_msg.controllers[1].state.id, lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE);
+
   EXPECT_EQ(
     controller_interface::return_type::OK,
     cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
@@ -162,6 +229,16 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle)
     ControllerManagerRunner cm_runner(this);
     EXPECT_EQ(test_param.expected_return, switch_future.get());
   }
+  auto expected_ctrl2_state = test_param.strictness == 1
+                                ? lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE
+                                : lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE;
+  get_cm_status_message(cm_activity_topic, cm_msg);
+  ASSERT_EQ(cm_msg.hardware_components.size(), 3u);
+  ASSERT_EQ(cm_msg.controllers.size(), 2u);
+  ASSERT_EQ(cm_msg.controllers[0].name, TEST_CONTROLLER2_NAME);
+  ASSERT_EQ(cm_msg.controllers[0].state.id, expected_ctrl2_state);
+  ASSERT_EQ(cm_msg.controllers[1].name, test_controller::TEST_CONTROLLER_NAME);
+  ASSERT_EQ(cm_msg.controllers[1].state.id, lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE);
 
   EXPECT_EQ(
     controller_interface::return_type::OK,
@@ -189,6 +266,14 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle)
   EXPECT_EQ(
     lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_lifecycle_state().id());
 
+  get_cm_status_message(cm_activity_topic, cm_msg);
+  ASSERT_EQ(cm_msg.hardware_components.size(), 3u);
+  ASSERT_EQ(cm_msg.controllers.size(), 2u);
+  ASSERT_EQ(cm_msg.controllers[0].name, TEST_CONTROLLER2_NAME);
+  ASSERT_EQ(cm_msg.controllers[0].state.id, expected_ctrl2_state);
+  ASSERT_EQ(cm_msg.controllers[1].name, test_controller::TEST_CONTROLLER_NAME);
+  ASSERT_EQ(cm_msg.controllers[1].state.id, lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
+
   EXPECT_EQ(
     controller_interface::return_type::OK,
     cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
@@ -205,6 +290,14 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle)
   ASSERT_EQ(std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100)))
     << "switch_controller should be blocking until next update cycle";
 
+  get_cm_status_message(cm_activity_topic, cm_msg);
+  ASSERT_EQ(cm_msg.hardware_components.size(), 3u);
+  ASSERT_EQ(cm_msg.controllers.size(), 2u);
+  ASSERT_EQ(cm_msg.controllers[0].name, TEST_CONTROLLER2_NAME);
+  ASSERT_EQ(cm_msg.controllers[0].state.id, expected_ctrl2_state);
+  ASSERT_EQ(cm_msg.controllers[1].name, test_controller::TEST_CONTROLLER_NAME);
+  ASSERT_EQ(cm_msg.controllers[1].state.id, lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
+
   EXPECT_EQ(
     controller_interface::return_type::OK,
     cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
@@ -227,6 +320,12 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle)
   ControllerManagerRunner cm_runner(this);
   EXPECT_EQ(controller_interface::return_type::OK, unload_future.get());
 
+  get_cm_status_message(cm_activity_topic, cm_msg);
+  ASSERT_EQ(cm_msg.hardware_components.size(), 3u);
+  ASSERT_EQ(cm_msg.controllers.size(), 1u);
+  ASSERT_EQ(cm_msg.controllers[0].name, TEST_CONTROLLER2_NAME);
+  ASSERT_EQ(cm_msg.controllers[0].state.id, expected_ctrl2_state);
+
   EXPECT_EQ(
     lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED,
     test_controller->get_lifecycle_state().id());

controller_manager_msgs/CMakeLists.txt

@@ -4,13 +4,16 @@ project(controller_manager_msgs)
 find_package(ament_cmake REQUIRED)
 find_package(builtin_interfaces REQUIRED)
 find_package(lifecycle_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
 
 set(msg_files
   msg/ControllerState.msg
   msg/ChainConnection.msg
   msg/HardwareComponentState.msg
   msg/HardwareInterface.msg
+  msg/NamedLifecycleState.msg
+  msg/ControllerManagerActivity.msg
 )
 set(srv_files
   srv/ConfigureController.srv
@@ -28,7 +31,7 @@ set(srv_files
 rosidl_generate_interfaces(${PROJECT_NAME}
   ${msg_files}
   ${srv_files}
-  DEPENDENCIES builtin_interfaces lifecycle_msgs
+  DEPENDENCIES builtin_interfaces lifecycle_msgs std_msgs
   ADD_LINTER_TESTS
 )
 ament_export_dependencies(rosidl_default_runtime)

controller_manager_msgs/msg/ControllerManagerActivity.msg

@@ -0,0 +1,10 @@
+# This message is used to provide the activity within the controller manager regarding the change in state of controllers and hardware interfaces
+
+# The header is used to provide timestamp information
+std_msgs/Header header
+
+# The current state of the controllers
+NamedLifecycleState[] controllers
+
+# The current state of the hardware components
+NamedLifecycleState[] hardware_components

controller_manager_msgs/msg/NamedLifecycleState.msg

@@ -0,0 +1,7 @@
+# This message is used to provide information about the lifecycle state of the controller or the hardware components
+
+# The name of the controller or hardware interface
+string name
+
+# The current lifecycle state of the controller or hardware components
+lifecycle_msgs/State state

controller_manager_msgs/package.xml

@@ -15,9 +15,11 @@
 
   <build_depend>builtin_interfaces</build_depend>
   <build_depend>lifecycle_msgs</build_depend>
+  <build_depend>std_msgs</build_depend>
 
   <exec_depend>builtin_interfaces</exec_depend>
   <exec_depend>lifecycle_msgs</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
   <exec_depend>rosidl_default_runtime</exec_depend>
 
   <test_depend>ament_lint_common</test_depend>

doc/release_notes.rst

@@ -90,6 +90,7 @@ controller_manager
 * The ``cpu_affinity`` parameter can now accept of types ``int`` or ``int_array`` to bind the process to a specific CPU core or multiple CPU cores. (`#1915 <https://github.com/ros-controls/ros2_control/pull/1915>`_).
 * The ``pal_statistics`` is now integrated into the controller_manager, so that the controllers, hardware components and the controller_manager can be easily introspected and monitored using the topics ``~/introspection_data/names`` and ``~/introspection_data/values`` (`#1918 <https://github.com/ros-controls/ros2_control/pull/1918>`_).
 * A python module ``test_utils`` was added to the ``controller_manager`` package to help with integration testing (`#1955 <https://github.com/ros-controls/ros2_control/pull/1955>`_).
+* A latched topic ``~/activity`` has been added to the controller_manager to publish the activity of the controller_manager, where the change in states of the controllers and the hardware components are published. (`#2006 <https://github.com/ros-controls/ros2_control/pull/2006>`_).
 
 hardware_interface
 ******************

hardware_interface/include/hardware_interface/resource_manager.hpp

@@ -464,6 +464,12 @@ class ResourceManager
    */
   bool state_interface_exists(const std::string & key) const;
 
+  /// A method to register a callback to be called when the component state changes.
+  /**
+   * \param[in] callback function to be called when the component state changes.
+   */
+  void set_on_component_state_switch_callback(std::function<void()> callback);
+
 protected:
   /// Gets the logger for the resource manager
   /**