From 3579381c81f323ab661b09b27f3e605ff3396553 Mon Sep 17 00:00:00 2001
From: Bence Magyar <bence.magyar.robotics@gmail.com>
Date: Sun, 14 Sep 2025 23:14:59 +0100
Subject: [PATCH] Add pal_statistics

---
 .../joint_state_topic_hardware_interface.hpp           |  2 ++
 .../src/joint_state_topic_hardware_interface.cpp       | 10 ++++++++++
 2 files changed, 12 insertions(+)

diff --git a/joint_state_topic_hardware_interface/include/joint_state_topic_hardware_interface/joint_state_topic_hardware_interface.hpp b/joint_state_topic_hardware_interface/include/joint_state_topic_hardware_interface/joint_state_topic_hardware_interface.hpp
index ab1bd00..554df4a 100644
--- a/joint_state_topic_hardware_interface/include/joint_state_topic_hardware_interface/joint_state_topic_hardware_interface.hpp
+++ b/joint_state_topic_hardware_interface/include/joint_state_topic_hardware_interface/joint_state_topic_hardware_interface.hpp
@@ -57,6 +57,8 @@ class JointStateTopicSystem : public hardware_interface::SystemInterface
 public:
   CallbackReturn on_init(const hardware_interface::HardwareComponentInterfaceParams& params) override;
 
+  CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override;
+
   std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
 
   std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;
diff --git a/joint_state_topic_hardware_interface/src/joint_state_topic_hardware_interface.cpp b/joint_state_topic_hardware_interface/src/joint_state_topic_hardware_interface.cpp
index 0d3440a..cf081e3 100644
--- a/joint_state_topic_hardware_interface/src/joint_state_topic_hardware_interface.cpp
+++ b/joint_state_topic_hardware_interface/src/joint_state_topic_hardware_interface.cpp
@@ -22,6 +22,7 @@
 #include <vector>
 
 #include <angles/angles.h>
+#include <hardware_interface/introspection.hpp>
 #include <joint_state_topic_hardware_interface/joint_state_topic_hardware_interface.hpp>
 #include <rclcpp/executors.hpp>
 
@@ -152,6 +153,15 @@ CallbackReturn JointStateTopicSystem::on_init(const hardware_interface::Hardware
   return CallbackReturn::SUCCESS;
 }
 
+CallbackReturn JointStateTopicSystem::on_configure(const rclcpp_lifecycle::State& /*previous_state*/)
+{
+  REGISTER_ROS2_CONTROL_INTROSPECTION("nonlimited_left_wheel_velocity", &nonlimited_velocity_commands_[0]);
+  REGISTER_ROS2_CONTROL_INTROSPECTION("nonlimited_right_wheel_velocity", &nonlimited_velocity_commands_[1]);
+  REGISTER_ROS2_CONTROL_INTROSPECTION("limited_left_wheel_velocity", &limited_velocity_commands_[0]);
+  REGISTER_ROS2_CONTROL_INTROSPECTION("limited_right_wheel_velocity", &limited_velocity_commands_[1]);
+  return CallbackReturn::SUCCESS;
+}
+
 std::vector<hardware_interface::StateInterface> JointStateTopicSystem::export_state_interfaces()
 {
   std::vector<hardware_interface::StateInterface> state_interfaces;