Integrate pal_statistics for introspection of controllers, hardware components and more

controller_interface/include/controller_interface/controller_interface_base.hpp

@@ -27,6 +27,7 @@
 #include "hardware_interface/loaned_command_interface.hpp"
 #include "hardware_interface/loaned_state_interface.hpp"
 
+#include "pal_statistics/pal_statistics_utils.hpp"
 #include "rclcpp/version.h"
 #include "rclcpp_lifecycle/lifecycle_node.hpp"
 
@@ -313,6 +314,15 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy
   CONTROLLER_INTERFACE_PUBLIC
   void wait_for_trigger_update_to_finish();
 
+  CONTROLLER_INTERFACE_PUBLIC
+  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_;
@@ -324,6 +334,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

@@ -69,6 +69,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
@@ -78,7 +79,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));
@@ -135,6 +140,8 @@ const rclcpp_lifecycle::State & ControllerInterfaceBase::configure()
       thread_priority);
     async_handler_->start_thread();
   }
+  REGISTER_DEFAULT_INTROSPECTION("total_triggers", &trigger_stats_.total_triggers);
+  REGISTER_DEFAULT_INTROSPECTION("failed_triggers", &trigger_stats_.failed_triggers);
   trigger_stats_.reset();
 
   return get_node()->configure();
@@ -213,4 +220,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/include/controller_manager/controller_manager.hpp

@@ -83,7 +83,7 @@ class ControllerManager : public rclcpp::Node
     const rclcpp::NodeOptions & options = get_cm_node_options());
 
   CONTROLLER_MANAGER_PUBLIC
-  virtual ~ControllerManager() = default;
+  virtual ~ControllerManager();
 
   CONTROLLER_MANAGER_PUBLIC
   void robot_description_callback(const std_msgs::msg::String & msg);

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"
@@ -279,6 +280,11 @@ ControllerManager::ControllerManager(
   init_controller_manager();
 }
 
+ControllerManager::~ControllerManager()
+{
+  STOP_PUBLISHER_THREAD(hardware_interface::DEFAULT_REGISTRY_KEY);
+}
+
 void ControllerManager::init_controller_manager()
 {
   // Get parameters needed for RT "update" loop to work
@@ -322,6 +328,10 @@ void ControllerManager::init_controller_manager()
   diagnostics_updater_.add(
     "Controller Manager Activity", this,
     &ControllerManager::controller_manager_diagnostic_callback);
+  INITIALIZE_REGISTRY(
+    this, hardware_interface::DEFAULT_INTROSPECTION_TOPIC,
+    hardware_interface::DEFAULT_REGISTRY_KEY);
+  START_PUBLISH_THREAD(hardware_interface::DEFAULT_REGISTRY_KEY);
 }
 
 void ControllerManager::robot_description_callback(const std_msgs::msg::String & robot_description)
@@ -2492,6 +2502,8 @@ controller_interface::return_type ControllerManager::update(
     manage_switch();
   }
 
+  PUBLISH_ASYNC_STATISTICS(hardware_interface::DEFAULT_REGISTRY_KEY);
+
   return ret;
 }
 

hardware_interface/CMakeLists.txt

@@ -17,6 +17,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

@@ -28,6 +28,7 @@
 #include "hardware_interface/types/hardware_interface_return_values.hpp"
 #include "hardware_interface/types/lifecycle_state_names.hpp"
 #include "lifecycle_msgs/msg/state.hpp"
+#include "pal_statistics/pal_statistics_utils.hpp"
 #include "rclcpp/duration.hpp"
 #include "rclcpp/logger.hpp"
 #include "rclcpp/node_interfaces/node_clock_interface.hpp"
@@ -409,6 +410,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
@@ -433,6 +450,9 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
   // interface names to Handle accessed through getters/setters
   std::unordered_map<std::string, StateInterface::SharedPtr> actuator_states_;
   std::unordered_map<std::string, CommandInterface::SharedPtr> actuator_commands_;
+
+protected:
+  pal_statistics::RegistrationsRAII stats_registrations_;
 };
 
 }  // namespace hardware_interface

hardware_interface/include/hardware_interface/handle.hpp

@@ -24,6 +24,7 @@
 #include <variant>
 
 #include "hardware_interface/hardware_info.hpp"
+#include "hardware_interface/introspection.hpp"
 #include "hardware_interface/macros.hpp"
 
 namespace hardware_interface
@@ -207,6 +208,23 @@ class StateInterface : public Handle
   {
   }
 
+  void registerIntrospection() const
+  {
+    if (std::holds_alternative<double>(value_))
+    {
+      std::function<double()> f = [this]() { return *value_ptr_; };
+      REGISTER_ENTITY(DEFAULT_REGISTRY_KEY, "state_interface." + get_name(), f);
+    }
+  }
+
+  void unregisterIntrospection() const
+  {
+    if (std::holds_alternative<double>(value_))
+    {
+      UNREGISTER_ENTITY(DEFAULT_REGISTRY_KEY, "state_interface." + get_name());
+    }
+  }
+
   StateInterface(const StateInterface & other) = default;
 
   StateInterface(StateInterface && other) = default;
@@ -234,6 +252,27 @@ class CommandInterface : public Handle
 
   CommandInterface(CommandInterface && other) = default;
 
+  void registerIntrospection() const
+  {
+    if (std::holds_alternative<double>(value_))
+    {
+      RCLCPP_INFO_STREAM(
+        rclcpp::get_logger("command_interface"), "Registering handle: " << get_name());
+      std::function<double()> f = [this]() { return *value_ptr_; };
+      REGISTER_ENTITY(DEFAULT_REGISTRY_KEY, "command_interface." + get_name(), f);
+    }
+  }
+
+  void unregisterIntrospection() const
+  {
+    if (std::holds_alternative<double>(value_))
+    {
+      RCLCPP_INFO_STREAM(
+        rclcpp::get_logger("command_interface"), "Unregistering handle: " << get_name());
+      UNREGISTER_ENTITY(DEFAULT_REGISTRY_KEY, "command_interface." + get_name());
+    }
+  }
+
   using Handle::Handle;
 
   using SharedPtr = std::shared_ptr<CommandInterface>;

hardware_interface/include/hardware_interface/introspection.hpp

@@ -0,0 +1,33 @@
+// Copyright 2024 PAL Robotics S.L.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/// \author Sai Kishor Kothakota
+
+#ifndef HARDWARE_INTERFACE__INTROSPECTION_HPP_
+#define HARDWARE_INTERFACE__INTROSPECTION_HPP_
+
+#include <pal_statistics/pal_statistics_macros.hpp>
+
+namespace hardware_interface
+{
+constexpr char DEFAULT_REGISTRY_KEY[] = "ros2_control";
+constexpr char DEFAULT_INTROSPECTION_TOPIC[] = "~/introspection_data";
+
+#define REGISTER_DEFAULT_INTROSPECTION(ID, ENTITY)                           \
+  REGISTER_ENTITY(                                                           \
+    hardware_interface::DEFAULT_REGISTRY_KEY, get_name() + "." + ID, ENTITY, \
+    &stats_registrations_, false)
+}  // namespace hardware_interface
+
+#endif  // HARDWARE_INTERFACE__INTROSPECTION_HPP_

hardware_interface/include/hardware_interface/sensor_interface.hpp

@@ -28,6 +28,7 @@
 #include "hardware_interface/types/hardware_interface_return_values.hpp"
 #include "hardware_interface/types/lifecycle_state_names.hpp"
 #include "lifecycle_msgs/msg/state.hpp"
+#include "pal_statistics/pal_statistics_utils.hpp"
 #include "rclcpp/duration.hpp"
 #include "rclcpp/logger.hpp"
 #include "rclcpp/node_interfaces/node_clock_interface.hpp"
@@ -271,6 +272,22 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
    */
   const HardwareInfo & get_hardware_info() const { return info_; }
 
+  /// Enable or disable introspection of the sensor 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
@@ -288,6 +305,9 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
   rclcpp::Logger sensor_logger_;
   // interface names to Handle accessed through getters/setters
   std::unordered_map<std::string, StateInterface::SharedPtr> sensor_states_map_;
+
+protected:
+  pal_statistics::RegistrationsRAII stats_registrations_;
 };
 
 }  // namespace hardware_interface

hardware_interface/include/hardware_interface/system_interface.hpp

@@ -29,6 +29,7 @@
 #include "hardware_interface/types/hardware_interface_type_values.hpp"
 #include "hardware_interface/types/lifecycle_state_names.hpp"
 #include "lifecycle_msgs/msg/state.hpp"
+#include "pal_statistics/pal_statistics_utils.hpp"
 #include "rclcpp/duration.hpp"
 #include "rclcpp/logger.hpp"
 #include "rclcpp/logging.hpp"
@@ -438,6 +439,22 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
    */
   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
@@ -472,6 +489,9 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
   // interface names to Handle accessed through getters/setters
   std::unordered_map<std::string, StateInterface::SharedPtr> system_states_;
   std::unordered_map<std::string, CommandInterface::SharedPtr> system_commands_;
+
+protected:
+  pal_statistics::RegistrationsRAII stats_registrations_;
 };
 
 }  // namespace hardware_interface

hardware_interface/package.xml

@@ -19,6 +19,7 @@
   <depend>joint_limits</depend>
   <depend>urdf</depend>
   <depend>sdformat_urdf</depend>
+  <depend>pal_statistics</depend>
 
   <build_depend>rcutils</build_depend>
   <exec_depend>rcutils</exec_depend>