Add integration into the controller and hardware components base clas…

…ses and add helper macros

controller_interface/include/controller_interface/controller_interface_base.hpp

@@ -26,6 +26,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"
 
@@ -305,6 +306,15 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy
    */
   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_;
@@ -316,6 +326,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

@@ -71,6 +71,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
@@ -80,7 +81,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));
@@ -136,7 +141,12 @@ const rclcpp_lifecycle::State & ControllerInterfaceBase::configure()
         &ControllerInterfaceBase::update, this, std::placeholders::_1, std::placeholders::_2),
       thread_priority);
     async_handler_->start_thread();
+    REGISTER_DEFAULT_INTROSPECTION(
+      "execution_time",
+      std::function<int()>([this]() { return async_handler_->get_last_execution_time().count(); }));
   }
+  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();
@@ -233,4 +243,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

hardware_interface/include/hardware_interface/actuator_interface.hpp

@@ -29,6 +29,7 @@
 #include "hardware_interface/types/lifecycle_state_names.hpp"
 #include "hardware_interface/types/trigger_type.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"
@@ -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/introspection.hpp

@@ -23,6 +23,11 @@ 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"
@@ -326,6 +327,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
@@ -346,6 +363,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

@@ -30,6 +30,7 @@
 #include "hardware_interface/types/lifecycle_state_names.hpp"
 #include "hardware_interface/types/trigger_type.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"
@@ -551,6 +552,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
@@ -587,6 +604,9 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
   std::unordered_map<std::string, StateInterface::SharedPtr> system_states_;
   std::unordered_map<std::string, CommandInterface::SharedPtr> system_commands_;
   std::atomic<TriggerType> next_trigger_ = TriggerType::READ;
+
+protected:
+  pal_statistics::RegistrationsRAII stats_registrations_;
 };
 
 }  // namespace hardware_interface

hardware_interface/src/actuator.cpp

@@ -98,6 +98,7 @@ const rclcpp_lifecycle::State & Actuator::configure()
 const rclcpp_lifecycle::State & Actuator::cleanup()
 {
   std::unique_lock<std::recursive_mutex> lock(actuators_mutex_);
+  impl_->enable_introspection(false);
   if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)
   {
     switch (impl_->on_cleanup(impl_->get_lifecycle_state()))
@@ -119,6 +120,7 @@ const rclcpp_lifecycle::State & Actuator::cleanup()
 const rclcpp_lifecycle::State & Actuator::shutdown()
 {
   std::unique_lock<std::recursive_mutex> lock(actuators_mutex_);
+  impl_->enable_introspection(false);
   if (
     impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN &&
     impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED)
@@ -146,6 +148,7 @@ const rclcpp_lifecycle::State & Actuator::activate()
     switch (impl_->on_activate(impl_->get_lifecycle_state()))
     {
       case CallbackReturn::SUCCESS:
+        impl_->enable_introspection(true);
         impl_->set_lifecycle_state(rclcpp_lifecycle::State(
           lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE));
         break;
@@ -164,6 +167,7 @@ const rclcpp_lifecycle::State & Actuator::activate()
 const rclcpp_lifecycle::State & Actuator::deactivate()
 {
   std::unique_lock<std::recursive_mutex> lock(actuators_mutex_);
+  impl_->enable_introspection(false);
   if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
   {
     switch (impl_->on_deactivate(impl_->get_lifecycle_state()))
@@ -187,6 +191,7 @@ const rclcpp_lifecycle::State & Actuator::deactivate()
 const rclcpp_lifecycle::State & Actuator::error()
 {
   std::unique_lock<std::recursive_mutex> lock(actuators_mutex_);
+  impl_->enable_introspection(false);
   if (
     impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN &&
     impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED)

hardware_interface/src/sensor.cpp

@@ -95,6 +95,7 @@ const rclcpp_lifecycle::State & Sensor::configure()
 const rclcpp_lifecycle::State & Sensor::cleanup()
 {
   std::unique_lock<std::recursive_mutex> lock(sensors_mutex_);
+  impl_->enable_introspection(false);
   if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)
   {
     switch (impl_->on_cleanup(impl_->get_lifecycle_state()))
@@ -116,6 +117,7 @@ const rclcpp_lifecycle::State & Sensor::cleanup()
 const rclcpp_lifecycle::State & Sensor::shutdown()
 {
   std::unique_lock<std::recursive_mutex> lock(sensors_mutex_);
+  impl_->enable_introspection(false);
   if (
     impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN &&
     impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED)
@@ -143,6 +145,7 @@ const rclcpp_lifecycle::State & Sensor::activate()
     switch (impl_->on_activate(impl_->get_lifecycle_state()))
     {
       case CallbackReturn::SUCCESS:
+        impl_->enable_introspection(true);
         impl_->set_lifecycle_state(rclcpp_lifecycle::State(
           lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE));
         break;
@@ -161,6 +164,7 @@ const rclcpp_lifecycle::State & Sensor::activate()
 const rclcpp_lifecycle::State & Sensor::deactivate()
 {
   std::unique_lock<std::recursive_mutex> lock(sensors_mutex_);
+  impl_->enable_introspection(false);
   if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
   {
     switch (impl_->on_deactivate(impl_->get_lifecycle_state()))
@@ -184,6 +188,7 @@ const rclcpp_lifecycle::State & Sensor::deactivate()
 const rclcpp_lifecycle::State & Sensor::error()
 {
   std::unique_lock<std::recursive_mutex> lock(sensors_mutex_);
+  impl_->enable_introspection(false);
   if (
     impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN &&
     impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED)

hardware_interface/src/system.cpp

@@ -96,6 +96,7 @@ const rclcpp_lifecycle::State & System::configure()
 const rclcpp_lifecycle::State & System::cleanup()
 {
   std::unique_lock<std::recursive_mutex> lock(system_mutex_);
+  impl_->enable_introspection(false);
   if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)
   {
     switch (impl_->on_cleanup(impl_->get_lifecycle_state()))
@@ -117,6 +118,7 @@ const rclcpp_lifecycle::State & System::cleanup()
 const rclcpp_lifecycle::State & System::shutdown()
 {
   std::unique_lock<std::recursive_mutex> lock(system_mutex_);
+  impl_->enable_introspection(false);
   if (
     impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN &&
     impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED)
@@ -144,6 +146,7 @@ const rclcpp_lifecycle::State & System::activate()
     switch (impl_->on_activate(impl_->get_lifecycle_state()))
     {
       case CallbackReturn::SUCCESS:
+        impl_->enable_introspection(true);
         impl_->set_lifecycle_state(rclcpp_lifecycle::State(
           lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE));
         break;
@@ -162,6 +165,7 @@ const rclcpp_lifecycle::State & System::activate()
 const rclcpp_lifecycle::State & System::deactivate()
 {
   std::unique_lock<std::recursive_mutex> lock(system_mutex_);
+  impl_->enable_introspection(false);
   if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
   {
     switch (impl_->on_deactivate(impl_->get_lifecycle_state()))
@@ -185,6 +189,7 @@ const rclcpp_lifecycle::State & System::deactivate()
 const rclcpp_lifecycle::State & System::error()
 {
   std::unique_lock<std::recursive_mutex> lock(system_mutex_);
+  impl_->enable_introspection(false);
   if (
     impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN &&
     impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED)