From 08fa8cc482ca14b3a4e6b40b8846113042bcb880 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 17 Nov 2024 23:07:28 +0100 Subject: [PATCH 1/5] add first integration of pal_statistics --- .../controller_manager/controller_manager.hpp | 2 +- controller_manager/src/controller_manager.cpp | 12 ++++++ hardware_interface/CMakeLists.txt | 1 + .../include/hardware_interface/handle.hpp | 39 +++++++++++++++++++ .../hardware_interface/introspection.hpp | 28 +++++++++++++ hardware_interface/package.xml | 1 + hardware_interface/src/resource_manager.cpp | 4 ++ 7 files changed, 86 insertions(+), 1 deletion(-) create mode 100644 hardware_interface/include/hardware_interface/introspection.hpp diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 068eefc1f9..fcaa28e2d8 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/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); diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 82546aaeda..8069b7a0db 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/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) @@ -2504,6 +2514,8 @@ controller_interface::return_type ControllerManager::update( manage_switch(); } + PUBLISH_ASYNC_STATISTICS(hardware_interface::DEFAULT_REGISTRY_KEY); + return ret; } diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index 385ae96fb1..ffcd6fbba1 100644 --- a/hardware_interface/CMakeLists.txt +++ b/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) diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index 1dfd499c2c..537269f382 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -24,6 +24,7 @@ #include #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(value_)) + { + std::function f = [this]() { return *value_ptr_; }; + REGISTER_ENTITY(DEFAULT_REGISTRY_KEY, "state_interface." + get_name(), f); + } + } + + void unregisterIntrospection() const + { + if (std::holds_alternative(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(value_)) + { + RCLCPP_INFO_STREAM( + rclcpp::get_logger("command_interface"), "Registering handle: " << get_name()); + std::function f = [this]() { return *value_ptr_; }; + REGISTER_ENTITY(DEFAULT_REGISTRY_KEY, "command_interface." + get_name(), f); + } + } + + void unregisterIntrospection() const + { + if (std::holds_alternative(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; diff --git a/hardware_interface/include/hardware_interface/introspection.hpp b/hardware_interface/include/hardware_interface/introspection.hpp new file mode 100644 index 0000000000..a24ba03c3f --- /dev/null +++ b/hardware_interface/include/hardware_interface/introspection.hpp @@ -0,0 +1,28 @@ +// 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 + +namespace hardware_interface +{ +constexpr char DEFAULT_REGISTRY_KEY[] = "ros2_control"; +constexpr char DEFAULT_INTROSPECTION_TOPIC[] = "introspection_data"; +} // namespace hardware_interface + +#endif // HARDWARE_INTERFACE__INTROSPECTION_HPP_ diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index dbe9ad5750..5174175429 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -19,6 +19,7 @@ joint_limits urdf sdformat_urdf + pal_statistics rcutils rcutils diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index e5445491c8..72aaa1893e 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -632,6 +632,7 @@ class ResourceStorage command_interface->get_name() + "]"); throw std::runtime_error(msg); } + command_interface->registerIntrospection(); } // BEGIN (Handle export change): for backward compatibility, can be removed if @@ -689,6 +690,7 @@ class ResourceStorage interface->get_name() + "]"); throw std::runtime_error(msg); } + interface->registerIntrospection(); return interface_name; } /// Adds exported state interfaces into internal storage. @@ -736,6 +738,7 @@ class ResourceStorage { for (const auto & interface : interface_names) { + state_interface_map_[interface]->unregisterIntrospection(); state_interface_map_.erase(interface); } } @@ -796,6 +799,7 @@ class ResourceStorage { for (const auto & interface : interface_names) { + command_interface_map_[interface]->unregisterIntrospection(); command_interface_map_.erase(interface); claimed_command_interface_map_.erase(interface); } From 1a27299ba810734888963894d9c45c08e8f25b9f Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 18 Nov 2024 19:51:13 +0100 Subject: [PATCH 2/5] namespace the introspection_data to the controller_manager node naming --- hardware_interface/include/hardware_interface/introspection.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/hardware_interface/include/hardware_interface/introspection.hpp b/hardware_interface/include/hardware_interface/introspection.hpp index a24ba03c3f..9fbfb405c3 100644 --- a/hardware_interface/include/hardware_interface/introspection.hpp +++ b/hardware_interface/include/hardware_interface/introspection.hpp @@ -22,7 +22,7 @@ namespace hardware_interface { constexpr char DEFAULT_REGISTRY_KEY[] = "ros2_control"; -constexpr char DEFAULT_INTROSPECTION_TOPIC[] = "introspection_data"; +constexpr char DEFAULT_INTROSPECTION_TOPIC[] = "~/introspection_data"; } // namespace hardware_interface #endif // HARDWARE_INTERFACE__INTROSPECTION_HPP_ From 8a687aa6565655c91ac4f0b987557e85a46a2683 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 19 Nov 2024 18:53:38 +0100 Subject: [PATCH 3/5] Add integration into the controller and hardware components base classes and add helper macros --- .../controller_interface_base.hpp | 13 +++++++++ .../src/controller_interface_base.cpp | 27 ++++++++++++++++++- .../hardware_interface/actuator_interface.hpp | 20 ++++++++++++++ .../hardware_interface/introspection.hpp | 5 ++++ .../hardware_interface/sensor_interface.hpp | 20 ++++++++++++++ .../hardware_interface/system_interface.hpp | 20 ++++++++++++++ hardware_interface/src/actuator.cpp | 5 ++++ hardware_interface/src/sensor.cpp | 5 ++++ hardware_interface/src/system.cpp | 5 ++++ 9 files changed, 119 insertions(+), 1 deletion(-) diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index ae3804919c..e9641a177e 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/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 command_interfaces_; std::vector 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; diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index 0713ec3c25..845b10766f 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/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)); @@ -134,7 +139,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([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(); @@ -213,4 +223,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 diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index 8aa214e728..4b95fd07ca 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/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 actuator_states_; std::unordered_map actuator_commands_; + +protected: + pal_statistics::RegistrationsRAII stats_registrations_; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/introspection.hpp b/hardware_interface/include/hardware_interface/introspection.hpp index 9fbfb405c3..2aea8d2a20 100644 --- a/hardware_interface/include/hardware_interface/introspection.hpp +++ b/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_ diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index 74c1d04bd7..f7f2f31aa5 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/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 sensor_states_map_; + +protected: + pal_statistics::RegistrationsRAII stats_registrations_; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index 32d0b8a48a..75a7ae55a2 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/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 system_states_; std::unordered_map system_commands_; + +protected: + pal_statistics::RegistrationsRAII stats_registrations_; }; } // namespace hardware_interface diff --git a/hardware_interface/src/actuator.cpp b/hardware_interface/src/actuator.cpp index b0ed1f7c80..5e87f65a88 100644 --- a/hardware_interface/src/actuator.cpp +++ b/hardware_interface/src/actuator.cpp @@ -94,6 +94,7 @@ const rclcpp_lifecycle::State & Actuator::configure() const rclcpp_lifecycle::State & Actuator::cleanup() { std::unique_lock 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())) @@ -115,6 +116,7 @@ const rclcpp_lifecycle::State & Actuator::cleanup() const rclcpp_lifecycle::State & Actuator::shutdown() { std::unique_lock 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) @@ -142,6 +144,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; @@ -160,6 +163,7 @@ const rclcpp_lifecycle::State & Actuator::activate() const rclcpp_lifecycle::State & Actuator::deactivate() { std::unique_lock 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())) @@ -183,6 +187,7 @@ const rclcpp_lifecycle::State & Actuator::deactivate() const rclcpp_lifecycle::State & Actuator::error() { std::unique_lock 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) diff --git a/hardware_interface/src/sensor.cpp b/hardware_interface/src/sensor.cpp index 5da01368c9..95f1ec05f3 100644 --- a/hardware_interface/src/sensor.cpp +++ b/hardware_interface/src/sensor.cpp @@ -93,6 +93,7 @@ const rclcpp_lifecycle::State & Sensor::configure() const rclcpp_lifecycle::State & Sensor::cleanup() { std::unique_lock 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())) @@ -114,6 +115,7 @@ const rclcpp_lifecycle::State & Sensor::cleanup() const rclcpp_lifecycle::State & Sensor::shutdown() { std::unique_lock 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) @@ -141,6 +143,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; @@ -159,6 +162,7 @@ const rclcpp_lifecycle::State & Sensor::activate() const rclcpp_lifecycle::State & Sensor::deactivate() { std::unique_lock 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())) @@ -182,6 +186,7 @@ const rclcpp_lifecycle::State & Sensor::deactivate() const rclcpp_lifecycle::State & Sensor::error() { std::unique_lock 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) diff --git a/hardware_interface/src/system.cpp b/hardware_interface/src/system.cpp index abae924dfc..0e57a2ba17 100644 --- a/hardware_interface/src/system.cpp +++ b/hardware_interface/src/system.cpp @@ -92,6 +92,7 @@ const rclcpp_lifecycle::State & System::configure() const rclcpp_lifecycle::State & System::cleanup() { std::unique_lock 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())) @@ -113,6 +114,7 @@ const rclcpp_lifecycle::State & System::cleanup() const rclcpp_lifecycle::State & System::shutdown() { std::unique_lock 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) @@ -140,6 +142,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; @@ -158,6 +161,7 @@ const rclcpp_lifecycle::State & System::activate() const rclcpp_lifecycle::State & System::deactivate() { std::unique_lock 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())) @@ -181,6 +185,7 @@ const rclcpp_lifecycle::State & System::deactivate() const rclcpp_lifecycle::State & System::error() { std::unique_lock 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) From 52310fd5ed2628e293131f5e8894f74286fcd35f Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 28 Nov 2024 18:26:02 +0100 Subject: [PATCH 4/5] remove execution time introspection --- controller_interface/src/controller_interface_base.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index 845b10766f..08468dd4b3 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -139,9 +139,6 @@ 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([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); From c312a9b13034aa930204be37622e07907d602b20 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 29 Nov 2024 00:09:57 +0100 Subject: [PATCH 5/5] handle cases when the value_ptr_ is invalid --- hardware_interface/include/hardware_interface/handle.hpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index 537269f382..015cca3ebb 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -212,7 +212,8 @@ class StateInterface : public Handle { if (std::holds_alternative(value_)) { - std::function f = [this]() { return *value_ptr_; }; + std::function f = [this]() + { return value_ptr_ ? *value_ptr_ : std::numeric_limits::quiet_NaN(); }; REGISTER_ENTITY(DEFAULT_REGISTRY_KEY, "state_interface." + get_name(), f); } } @@ -258,7 +259,8 @@ class CommandInterface : public Handle { RCLCPP_INFO_STREAM( rclcpp::get_logger("command_interface"), "Registering handle: " << get_name()); - std::function f = [this]() { return *value_ptr_; }; + std::function f = [this]() + { return value_ptr_ ? *value_ptr_ : std::numeric_limits::quiet_NaN(); }; REGISTER_ENTITY(DEFAULT_REGISTRY_KEY, "command_interface." + get_name(), f); } }