From ce485952b9b1cc0b39224135cafb2954c2ef6e62 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 28 Nov 2024 18:57:20 +0100 Subject: [PATCH] Integrate pal_statistics for introspecting the ros2_control_demos --- .../controllers/src/passthrough_controller.cpp | 5 +++++ example_12/hardware/rrbot.cpp | 9 +++++++++ example_4/hardware/rrbot_system_with_sensor.cpp | 16 ++++++++++++++++ 3 files changed, 30 insertions(+) diff --git a/example_12/controllers/src/passthrough_controller.cpp b/example_12/controllers/src/passthrough_controller.cpp index 92660438a..0d44b74a5 100644 --- a/example_12/controllers/src/passthrough_controller.cpp +++ b/example_12/controllers/src/passthrough_controller.cpp @@ -85,6 +85,11 @@ controller_interface::CallbackReturn PassthroughController::on_configure( reference_interfaces_.resize( reference_interface_names_.size(), std::numeric_limits::quiet_NaN()); + for (size_t i = 0; i < reference_interface_names_.size(); i++) + { + REGISTER_DEFAULT_INTROSPECTION(reference_interface_names_[i], &reference_interfaces_[i]); + } + return controller_interface::CallbackReturn::SUCCESS; } diff --git a/example_12/hardware/rrbot.cpp b/example_12/hardware/rrbot.cpp index 53e20ab7c..750176fc7 100644 --- a/example_12/hardware/rrbot.cpp +++ b/example_12/hardware/rrbot.cpp @@ -83,6 +83,15 @@ hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_init( } } + REGISTER_DEFAULT_INTROSPECTION("hw_start_sec", &hw_start_sec_); + REGISTER_DEFAULT_INTROSPECTION("hw_stop_sec", &hw_stop_sec_); + REGISTER_DEFAULT_INTROSPECTION("hw_slowdown", &hw_slowdown_); + for (size_t i = 0; i < info_.joints.size(); ++i) + { + REGISTER_DEFAULT_INTROSPECTION(info_.joints[i].name + ".hw_state", &hw_states_[i]); + REGISTER_DEFAULT_INTROSPECTION(info_.joints[i].name + ".hw_command", &hw_commands_[i]); + } + return hardware_interface::CallbackReturn::SUCCESS; } diff --git a/example_4/hardware/rrbot_system_with_sensor.cpp b/example_4/hardware/rrbot_system_with_sensor.cpp index 751cbc512..6476c12de 100644 --- a/example_4/hardware/rrbot_system_with_sensor.cpp +++ b/example_4/hardware/rrbot_system_with_sensor.cpp @@ -90,6 +90,22 @@ hardware_interface::CallbackReturn RRBotSystemWithSensorHardware::on_init( } } + REGISTER_DEFAULT_INTROSPECTION("hw_start_sec", &hw_start_sec_); + REGISTER_DEFAULT_INTROSPECTION("hw_stop_sec", &hw_stop_sec_); + REGISTER_DEFAULT_INTROSPECTION("hw_slowdown", &hw_slowdown_); + REGISTER_DEFAULT_INTROSPECTION("hw_sensor_change", &hw_sensor_change_); + for (size_t i = 0; i < info_.joints.size(); ++i) + { + REGISTER_DEFAULT_INTROSPECTION(info_.joints[i].name + ".hw_state", &hw_joint_states_[i]); + REGISTER_DEFAULT_INTROSPECTION(info_.joints[i].name + ".hw_command", &hw_joint_commands_[i]); + } + for (size_t i = 0; i < info_.sensors[0].state_interfaces.size(); ++i) + { + REGISTER_DEFAULT_INTROSPECTION( + info_.sensors[0].name + "." + info_.sensors[0].state_interfaces[i].name, + &hw_sensor_states_[i]); + } + return hardware_interface::CallbackReturn::SUCCESS; }