Integrate pal_statistics for introspection of controllers, hardware c…

…omponents and more (#1918)

controller_interface/include/controller_interface/controller_interface_base.hpp

@@ -23,6 +23,7 @@
 #include "realtime_tools/async_function_handler.hpp"
 
 #include "hardware_interface/handle.hpp"
+#include "hardware_interface/introspection.hpp"
 #include "hardware_interface/loaned_command_interface.hpp"
 #include "hardware_interface/loaned_state_interface.hpp"
 
@@ -305,6 +306,14 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy
    */
   void wait_for_trigger_update_to_finish();
 
+  std::string get_name() const;
+
+  /// Enable or disable introspection of the controller.
+  /**
+   * \param[in] enable Enable introspection if true, disable otherwise.
+   */
+  void enable_introspection(bool enable);
+
 protected:
   std::vector<hardware_interface::LoanedCommandInterface> command_interfaces_;
   std::vector<hardware_interface::LoanedStateInterface> state_interfaces_;
@@ -316,6 +325,9 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy
   bool is_async_ = false;
   std::string urdf_ = "";
   ControllerUpdateStats trigger_stats_;
+
+protected:
+  pal_statistics::RegistrationsRAII stats_registrations_;
 };
 
 using ControllerInterfaceBaseSharedPtr = std::shared_ptr<ControllerInterfaceBase>;

controller_interface/src/controller_interface_base.cpp

@@ -18,6 +18,7 @@
 #include <string>
 #include <vector>
 
+#include "hardware_interface/introspection.hpp"
 #include "lifecycle_msgs/msg/state.hpp"
 
 namespace controller_interface
@@ -82,6 +83,9 @@ return_type ControllerInterfaceBase::init(
   node_->register_on_cleanup(
     [this](const rclcpp_lifecycle::State & previous_state) -> CallbackReturn
     {
+      // make sure introspection is disabled on controller cleanup as users may manually enable
+      // it in `on_configure` and `on_deactivate` - see the docs for details
+      enable_introspection(false);
       if (is_async() && async_handler_ && async_handler_->is_running())
       {
         async_handler_->stop_thread();
@@ -92,6 +96,7 @@ return_type ControllerInterfaceBase::init(
   node_->register_on_activate(
     [this](const rclcpp_lifecycle::State & previous_state) -> CallbackReturn
     {
+      enable_introspection(true);
       if (is_async() && async_handler_ && async_handler_->is_running())
       {
         // This is needed if it is disabled due to a thrown exception in the async callback thread
@@ -101,7 +106,11 @@ return_type ControllerInterfaceBase::init(
     });
 
   node_->register_on_deactivate(
-    std::bind(&ControllerInterfaceBase::on_deactivate, this, std::placeholders::_1));
+    [this](const rclcpp_lifecycle::State & previous_state) -> CallbackReturn
+    {
+      enable_introspection(false);
+      return on_deactivate(previous_state);
+    });
 
   node_->register_on_shutdown(
     std::bind(&ControllerInterfaceBase::on_shutdown, this, std::placeholders::_1));
@@ -158,6 +167,8 @@ const rclcpp_lifecycle::State & ControllerInterfaceBase::configure()
       thread_priority);
     async_handler_->start_thread();
   }
+  REGISTER_ROS2_CONTROL_INTROSPECTION("total_triggers", &trigger_stats_.total_triggers);
+  REGISTER_ROS2_CONTROL_INTROSPECTION("failed_triggers", &trigger_stats_.failed_triggers);
   trigger_stats_.reset();
 
   return get_node()->configure();
@@ -258,4 +269,19 @@ void ControllerInterfaceBase::wait_for_trigger_update_to_finish()
     async_handler_->wait_for_trigger_cycle_to_finish();
   }
 }
+
+std::string ControllerInterfaceBase::get_name() const { return get_node()->get_name(); }
+
+void ControllerInterfaceBase::enable_introspection(bool enable)
+{
+  if (enable)
+  {
+    stats_registrations_.enableAll();
+  }
+  else
+  {
+    stats_registrations_.disableAll();
+  }
+}
+
 }  // namespace controller_interface

controller_manager/src/controller_manager.cpp

@@ -22,6 +22,7 @@
 
 #include "controller_interface/controller_interface_base.hpp"
 #include "controller_manager_msgs/msg/hardware_component_state.hpp"
+#include "hardware_interface/introspection.hpp"
 #include "hardware_interface/types/lifecycle_state_names.hpp"
 #include "lifecycle_msgs/msg/state.hpp"
 #include "rcl/arguments.h"
@@ -331,6 +332,7 @@ ControllerManager::ControllerManager(
 
 ControllerManager::~ControllerManager()
 {
+  CLEAR_ALL_ROS2_CONTROL_INTROSPECTION_REGISTRIES();
   if (preshutdown_cb_handle_)
   {
     rclcpp::Context::SharedPtr context = this->get_node_base_interface()->get_context();
@@ -408,6 +410,11 @@ void ControllerManager::init_controller_manager()
     "Controller Manager Activity", this,
     &ControllerManager::controller_manager_diagnostic_callback);
 
+  INITIALIZE_ROS2_CONTROL_INTROSPECTION_REGISTRY(
+    this, hardware_interface::DEFAULT_INTROSPECTION_TOPIC,
+    hardware_interface::DEFAULT_REGISTRY_KEY);
+  START_ROS2_CONTROL_INTROSPECTION_PUBLISHER_THREAD(hardware_interface::DEFAULT_REGISTRY_KEY);
+
   // Add on_shutdown callback to stop the controller manager
   rclcpp::Context::SharedPtr context = this->get_node_base_interface()->get_context();
   preshutdown_cb_handle_ =
@@ -2721,6 +2728,8 @@ controller_interface::return_type ControllerManager::update(
     manage_switch();
   }
 
+  PUBLISH_ROS2_CONTROL_INTROSPECTION_DATA_ASYNC(hardware_interface::DEFAULT_REGISTRY_KEY);
+
   return ret;
 }
 

doc/index.rst

@@ -37,3 +37,4 @@ Guidelines and Best Practices
    :titlesonly:
 
    Debugging the Controller Manager and Plugins <debugging.rst>
+   Introspecting Controllers and Hardware Components <introspection.rst>

doc/introspection.rst

@@ -0,0 +1,81 @@
+
+Introspection of the ros2_control setup
+***************************************
+
+With the integration of the ``pal_statistics`` package, the ``controller_manager`` node publishes the registered variables within the same process to the ``~/introspection_data`` topics.
+By default, all ``State`` and ``Command`` interfaces in the ``controller_manager`` are registered when they are added, and are unregistered when they are removed from the ``ResourceManager``.
+The state of the all the registered entities are published at the end of every ``update`` cycle of the ``controller_manager``. For instance, In a complete synchronous ros2_control setup (with synchronous controllers and hardware components), this data in the ``Command`` interface is the command used by the hardware components to command the hardware.
+
+All the registered variables are published over 3 topics: ``~/introspection_data/full``, ``~/introspection_data/names``, and ``~/introspection_data/values``.
+- The ``~/introspection_data/full`` topic publishes the full introspection data along with names and values in a single message. This can be useful to track or view variables and information from command line.
+- The ``~/introspection_data/names`` topic publishes the names of the registered variables. This topic contains the names of the variables registered. This is only published every time a a variables is registered and unregistered.
+- The ``~/introspection_data/values`` topic publishes the values of the registered variables. This topic contains the values of the variables registered.
+
+The topics ``~/introspection_data/full`` and ``~/introspection_data/values`` are always published on every update cycle asynchronously, provided that there is at least one subscriber to these topics.
+
+The topic ``~/introspection_data/full`` can be used to integrate with your custom visualization tools or to track the variables from the command line. The topic ``~/introspection_data/names`` and ``~/introspection_data/values`` are to be used for visualization tools like `PlotJuggler <https://plotjuggler.io/>`_ or `RQT plot <http://wiki.ros.org/rqt_plot>`_ to visualize the data.
+
+.. note::
+  If you have a high frequency of data, it is recommended to use the ``~/introspection_data/names`` and ``~/introspection_data/values`` topic. So, that the data transferred and stored is minimized.
+
+How to introspect internal variables of controllers and hardware components
+============================================================================
+
+Any member variable of a controller or hardware component can be registered for the introspection. It is very important that the lifetime of this variable exists as long as the controller or hardware component is available.
+
+.. note::
+  If a variable's lifetime is not properly managed, it may be attempted to read, which in the worst case scenario will cause a segmentation fault.
+
+How to register a variable for introspection
+---------------------------------------------
+
+1. Include the necessary headers in the controller or hardware component header file.
+
+   .. code-block:: cpp
+
+     #include <hardware_interface/introspection.hpp>
+
+2. Register the variable in the configure method of the controller or hardware component.
+
+   .. code-block:: cpp
+
+     void MyController::on_configure()
+     {
+       ...
+       // Register the variable for introspection (disabled by default)
+       // The variable is introspected only when the controller is active and
+       // then deactivated when the controller is deactivated.
+       REGISTER_ROS2_CONTROL_INTROSPECTION("my_variable_name", &my_variable_);
+       ...
+     }
+
+3. By default, the introspection of all the registered variables of the controllers and the hardware components is only activated, when they are active and it is deactivated when the controller or hardware component is deactivated.
+
+   .. note::
+      If you want to keep the introspection active even when the controller or hardware component is not active, you can do that by calling ``this->enable_introspection(true)`` in the ``on_configure`` and ``on_deactivate`` method of the controller or hardware component after registering the variables.
+
+Types of entities that can be introspected
+-------------------------------------------
+
+- Any variable that can be cast to a double is suitable for registration.
+- A function that returns a value that can be cast to a double is also suitable for registration.
+- Variables of complex structures can be registered by having defined introspection for their every internal variable.
+- Introspection of custom types can be done by defining a `custom introspection function <https://github.com/pal-robotics/pal_statistics/blob/humble-devel/pal_statistics/include/pal_statistics/registration_utils.hpp>`_.
+
+  .. note::
+    Registering the variables for introspection is not real-time safe. It is recommended to register the variables in the ``on_configure`` method only.
+
+Data Visualization
+*******************
+
+Data can be visualized with any tools that display ROS topics, but we recommend `PlotJuggler <https://plotjuggler.io/>`_ for viewing high resolution live data, or data in bags.
+
+1. Open ``PlotJuggler`` running ``ros2 run plotjuggler plotjuggler``.
+   .. image:: images/plotjuggler.png
+2. Visualize the data:
+   - Importing from the ros2bag
+   - Subscribing to the ROS2 topics live with the ``ROS2 Topic Subscriber`` option under ``Streaming`` header.
+3. Choose the topics ``~/introspection_data/names`` and ``~/introspection_data/values`` from the popup window.
+   .. image:: images/plotjuggler_select_topics.png
+4. Now, select the variables that are of your interest and drag them to the plot.
+   .. image:: images/plotjuggler_visualizing_data.png

doc/release_notes.rst

@@ -27,6 +27,7 @@ For details see the controller_manager section.
 * The ``assign_interfaces`` and ``release_interfaces`` methods are now virtual, so that the user can override them to store the interfaces into custom variable types, so that the user can have the flexibility to take the ownership of the loaned interfaces to the controller (`#1743 <https://github.com/ros-controls/ros2_control/pull/1743>`_)
 * The new ``PoseSensor`` semantic component provides a standard interface for hardware providing cartesian poses (`#1775 <https://github.com/ros-controls/ros2_control/pull/1775>`_)
 * The controllers now support the fallback controllers (a list of controllers that will be activated, when the spawned controllers fails by throwing an exception or returning ``return_type::ERROR`` during the ``update`` cycle) (`#1789 <https://github.com/ros-controls/ros2_control/pull/1789>`_)
+* The controllers can be easily introspect the internal member variables using the macro ``REGISTER_ROS2_CONTROL_INTROSPECTION`` (`#1918 <https://github.com/ros-controls/ros2_control/pull/1918>`_)
 * A new ``SemanticComponentCommandInterface`` semantic component provides capabilities analogous to the ``SemanticComponentInterface``, but for write-only devices (`#1945 <https://github.com/ros-controls/ros2_control/pull/1945>`_)
 * The new semantic command interface ``LedRgbDevice`` provides standard (command) interfaces for 3-channel LED devices (`#1945 <https://github.com/ros-controls/ros2_control/pull/1945>`_)
 
@@ -87,6 +88,7 @@ controller_manager
 * The ``ros2_control_node`` node has a new ``cpu_affinity`` parameter to bind the process to a specific CPU core. By default, this is not enabled. (`#1852 <https://github.com/ros-controls/ros2_control/pull/1852>`_).
 * The ``--service-call-timeout`` was added as parameter to the helper scripts ``spawner.py``. Useful when the CPU load is high at startup and the service call does not return immediately (`#1808 <https://github.com/ros-controls/ros2_control/pull/1808>`_).
 * 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>`_).
 
 hardware_interface
@@ -161,6 +163,7 @@ hardware_interface
 * With (`#1421 <https://github.com/ros-controls/ros2_control/pull/1421>`_) a key-value storage is added to InterfaceInfo. This allows to define extra params with per Command-/StateInterface in the ``.ros2_control.xacro`` file.
 * With (`#1763 <https://github.com/ros-controls/ros2_control/pull/1763>`_) parsing for SDF published to ``robot_description`` topic is now also supported.
 * With (`#1567 <https://github.com/ros-controls/ros2_control/pull/1567>`_) all the Hardware components now have a fully functional asynchronous functionality, by simply adding ``is_async`` tag to the ros2_control tag in the URDF. This will allow the hardware components to run in a separate thread, and the controller manager will be able to run the controllers in parallel with the hardware components.
+* The hardware components can be easily introspect the internal member variables using the macro ``REGISTER_ROS2_CONTROL_INTROSPECTION`` (`#1918 <https://github.com/ros-controls/ros2_control/pull/1918>`_)
 
 joint_limits
 ************

hardware_interface/CMakeLists.txt

@@ -22,6 +22,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
   tinyxml2_vendor
   joint_limits
   urdf
+  pal_statistics
 )
 
 find_package(ament_cmake REQUIRED)

hardware_interface/include/hardware_interface/actuator_interface.hpp

@@ -25,6 +25,7 @@
 #include "hardware_interface/component_parser.hpp"
 #include "hardware_interface/handle.hpp"
 #include "hardware_interface/hardware_info.hpp"
+#include "hardware_interface/introspection.hpp"
 #include "hardware_interface/types/hardware_interface_return_values.hpp"
 #include "hardware_interface/types/lifecycle_state_names.hpp"
 #include "hardware_interface/types/trigger_type.hpp"
@@ -93,7 +94,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
    */
   ActuatorInterface(const ActuatorInterface & other) = delete;
 
-  ActuatorInterface(ActuatorInterface && other) = default;
+  ActuatorInterface(ActuatorInterface && other) = delete;
 
   virtual ~ActuatorInterface() = default;
 
@@ -522,6 +523,22 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
    */
   const HardwareInfo & get_hardware_info() const { return info_; }
 
+  /// Enable or disable introspection of the hardware.
+  /**
+   * \param[in] enable Enable introspection if true, disable otherwise.
+   */
+  void enable_introspection(bool enable)
+  {
+    if (enable)
+    {
+      stats_registrations_.enableAll();
+    }
+    else
+    {
+      stats_registrations_.disableAll();
+    }
+  }
+
 protected:
   HardwareInfo info_;
   // interface names to InterfaceDescription
@@ -548,6 +565,9 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
   std::unordered_map<std::string, StateInterface::SharedPtr> actuator_states_;
   std::unordered_map<std::string, CommandInterface::SharedPtr> actuator_commands_;
   std::atomic<TriggerType> next_trigger_ = TriggerType::READ;
+
+protected:
+  pal_statistics::RegistrationsRAII stats_registrations_;
 };
 
 }  // namespace hardware_interface

hardware_interface/include/hardware_interface/handle.hpp

@@ -25,6 +25,7 @@
 #include <variant>
 
 #include "hardware_interface/hardware_info.hpp"
+#include "hardware_interface/introspection.hpp"
 #include "hardware_interface/macros.hpp"
 
 namespace hardware_interface
@@ -203,6 +204,24 @@ class StateInterface : public Handle
   {
   }
 
+  void registerIntrospection() const
+  {
+    if (std::holds_alternative<double>(value_))
+    {
+      std::function<double()> f = [this]()
+      { return value_ptr_ ? *value_ptr_ : std::numeric_limits<double>::quiet_NaN(); };
+      DEFAULT_REGISTER_ROS2_CONTROL_INTROSPECTION("state_interface." + get_name(), f);
+    }
+  }
+
+  void unregisterIntrospection() const
+  {
+    if (std::holds_alternative<double>(value_))
+    {
+      DEFAULT_UNREGISTER_ROS2_CONTROL_INTROSPECTION("state_interface." + get_name());
+    }
+  }
+
   StateInterface(const StateInterface & other) = default;
 
   StateInterface(StateInterface && other) = default;
@@ -230,6 +249,24 @@ class CommandInterface : public Handle
 
   CommandInterface(CommandInterface && other) = default;
 
+  void registerIntrospection() const
+  {
+    if (std::holds_alternative<double>(value_))
+    {
+      std::function<double()> f = [this]()
+      { return value_ptr_ ? *value_ptr_ : std::numeric_limits<double>::quiet_NaN(); };
+      DEFAULT_REGISTER_ROS2_CONTROL_INTROSPECTION("command_interface." + get_name(), f);
+    }
+  }
+
+  void unregisterIntrospection() const
+  {
+    if (std::holds_alternative<double>(value_))
+    {
+      DEFAULT_UNREGISTER_ROS2_CONTROL_INTROSPECTION("command_interface." + get_name());
+    }
+  }
+
   using Handle::Handle;
 
   using SharedPtr = std::shared_ptr<CommandInterface>;