diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index a548cd5856..0f76713eae 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -23,6 +23,7 @@ #include "realtime_tools/async_function_handler.hpp" #include "hardware_interface/handle.hpp" +#include "hardware_interface/introspection.hpp" #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/loaned_state_interface.hpp" @@ -305,6 +306,14 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy */ void wait_for_trigger_update_to_finish(); + 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_; @@ -316,6 +325,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 b8dd9f770d..a86339bed2 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -18,6 +18,7 @@ #include #include +#include "hardware_interface/introspection.hpp" #include "lifecycle_msgs/msg/state.hpp" namespace controller_interface @@ -82,6 +83,9 @@ return_type ControllerInterfaceBase::init( node_->register_on_cleanup( [this](const rclcpp_lifecycle::State & previous_state) -> CallbackReturn { + // make sure introspection is disabled on controller cleanup as users may manually enable + // it in `on_configure` and `on_deactivate` - see the docs for details + enable_introspection(false); if (is_async() && async_handler_ && async_handler_->is_running()) { async_handler_->stop_thread(); @@ -92,6 +96,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 @@ -101,7 +106,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)); @@ -158,6 +167,8 @@ const rclcpp_lifecycle::State & ControllerInterfaceBase::configure() thread_priority); async_handler_->start_thread(); } + REGISTER_ROS2_CONTROL_INTROSPECTION("total_triggers", &trigger_stats_.total_triggers); + REGISTER_ROS2_CONTROL_INTROSPECTION("failed_triggers", &trigger_stats_.failed_triggers); trigger_stats_.reset(); return get_node()->configure(); @@ -258,4 +269,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/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index bef2985c0b..a4ab75fb4e 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" @@ -293,6 +294,7 @@ ControllerManager::ControllerManager( ControllerManager::~ControllerManager() { + CLEAR_ALL_ROS2_CONTROL_INTROSPECTION_REGISTRIES(); if (preshutdown_cb_handle_) { rclcpp::Context::SharedPtr context = this->get_node_base_interface()->get_context(); @@ -370,6 +372,11 @@ void ControllerManager::init_controller_manager() "Controller Manager Activity", this, &ControllerManager::controller_manager_diagnostic_callback); + INITIALIZE_ROS2_CONTROL_INTROSPECTION_REGISTRY( + this, hardware_interface::DEFAULT_INTROSPECTION_TOPIC, + hardware_interface::DEFAULT_REGISTRY_KEY); + START_ROS2_CONTROL_INTROSPECTION_PUBLISHER_THREAD(hardware_interface::DEFAULT_REGISTRY_KEY); + // Add on_shutdown callback to stop the controller manager rclcpp::Context::SharedPtr context = this->get_node_base_interface()->get_context(); preshutdown_cb_handle_ = @@ -2681,6 +2688,8 @@ controller_interface::return_type ControllerManager::update( manage_switch(); } + PUBLISH_ROS2_CONTROL_INTROSPECTION_DATA_ASYNC(hardware_interface::DEFAULT_REGISTRY_KEY); + return ret; } diff --git a/doc/images/plotjuggler.png b/doc/images/plotjuggler.png new file mode 100644 index 0000000000..708a476cb1 Binary files /dev/null and b/doc/images/plotjuggler.png differ diff --git a/doc/images/plotjuggler_select_topics.png b/doc/images/plotjuggler_select_topics.png new file mode 100644 index 0000000000..7f18ae4da7 Binary files /dev/null and b/doc/images/plotjuggler_select_topics.png differ diff --git a/doc/images/plotjuggler_visualizing_data.png b/doc/images/plotjuggler_visualizing_data.png new file mode 100644 index 0000000000..a52732c442 Binary files /dev/null and b/doc/images/plotjuggler_visualizing_data.png differ diff --git a/doc/index.rst b/doc/index.rst index 09a2ddf745..5aea45d713 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -37,3 +37,4 @@ Guidelines and Best Practices :titlesonly: Debugging the Controller Manager and Plugins + Introspecting Controllers and Hardware Components diff --git a/doc/introspection.rst b/doc/introspection.rst new file mode 100644 index 0000000000..d3cccf7446 --- /dev/null +++ b/doc/introspection.rst @@ -0,0 +1,81 @@ + +Introspection of the ros2_control setup +*************************************** + +With the integration of the ``pal_statistics`` package, the ``controller_manager`` node publishes the registered variables within the same process to the ``~/introspection_data`` topics. +By default, all ``State`` and ``Command`` interfaces in the ``controller_manager`` are registered when they are added, and are unregistered when they are removed from the ``ResourceManager``. +The state of the all the registered entities are published at the end of every ``update`` cycle of the ``controller_manager``. For instance, In a complete synchronous ros2_control setup (with synchronous controllers and hardware components), this data in the ``Command`` interface is the command used by the hardware components to command the hardware. + +All the registered variables are published over 3 topics: ``~/introspection_data/full``, ``~/introspection_data/names``, and ``~/introspection_data/values``. +- The ``~/introspection_data/full`` topic publishes the full introspection data along with names and values in a single message. This can be useful to track or view variables and information from command line. +- The ``~/introspection_data/names`` topic publishes the names of the registered variables. This topic contains the names of the variables registered. This is only published every time a a variables is registered and unregistered. +- The ``~/introspection_data/values`` topic publishes the values of the registered variables. This topic contains the values of the variables registered. + +The topics ``~/introspection_data/full`` and ``~/introspection_data/values`` are always published on every update cycle asynchronously, provided that there is at least one subscriber to these topics. + +The topic ``~/introspection_data/full`` can be used to integrate with your custom visualization tools or to track the variables from the command line. The topic ``~/introspection_data/names`` and ``~/introspection_data/values`` are to be used for visualization tools like `PlotJuggler `_ or `RQT plot `_ to visualize the data. + +.. note:: + If you have a high frequency of data, it is recommended to use the ``~/introspection_data/names`` and ``~/introspection_data/values`` topic. So, that the data transferred and stored is minimized. + +How to introspect internal variables of controllers and hardware components +============================================================================ + +Any member variable of a controller or hardware component can be registered for the introspection. It is very important that the lifetime of this variable exists as long as the controller or hardware component is available. + +.. note:: + If a variable's lifetime is not properly managed, it may be attempted to read, which in the worst case scenario will cause a segmentation fault. + +How to register a variable for introspection +--------------------------------------------- + +1. Include the necessary headers in the controller or hardware component header file. + + .. code-block:: cpp + + #include + +2. Register the variable in the configure method of the controller or hardware component. + + .. code-block:: cpp + + void MyController::on_configure() + { + ... + // Register the variable for introspection (disabled by default) + // The variable is introspected only when the controller is active and + // then deactivated when the controller is deactivated. + REGISTER_ROS2_CONTROL_INTROSPECTION("my_variable_name", &my_variable_); + ... + } + +3. By default, the introspection of all the registered variables of the controllers and the hardware components is only activated, when they are active and it is deactivated when the controller or hardware component is deactivated. + + .. note:: + If you want to keep the introspection active even when the controller or hardware component is not active, you can do that by calling ``this->enable_introspection(true)`` in the ``on_configure`` and ``on_deactivate`` method of the controller or hardware component after registering the variables. + +Types of entities that can be introspected +------------------------------------------- + +- Any variable that can be cast to a double is suitable for registration. +- A function that returns a value that can be cast to a double is also suitable for registration. +- Variables of complex structures can be registered by having defined introspection for their every internal variable. +- Introspection of custom types can be done by defining a `custom introspection function `_. + + .. note:: + Registering the variables for introspection is not real-time safe. It is recommended to register the variables in the ``on_configure`` method only. + +Data Visualization +******************* + +Data can be visualized with any tools that display ROS topics, but we recommend `PlotJuggler `_ for viewing high resolution live data, or data in bags. + +1. Open ``PlotJuggler`` running ``ros2 run plotjuggler plotjuggler``. + .. image:: images/plotjuggler.png +2. Visualize the data: + - Importing from the ros2bag + - Subscribing to the ROS2 topics live with the ``ROS2 Topic Subscriber`` option under ``Streaming`` header. +3. Choose the topics ``~/introspection_data/names`` and ``~/introspection_data/values`` from the popup window. + .. image:: images/plotjuggler_select_topics.png +4. Now, select the variables that are of your interest and drag them to the plot. + .. image:: images/plotjuggler_visualizing_data.png diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 3dec704f23..efe81d0567 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -27,6 +27,7 @@ For details see the controller_manager section. * The ``assign_interfaces`` and ``release_interfaces`` methods are now virtual, so that the user can override them to store the interfaces into custom variable types, so that the user can have the flexibility to take the ownership of the loaned interfaces to the controller (`#1743 `_) * The new ``PoseSensor`` semantic component provides a standard interface for hardware providing cartesian poses (`#1775 `_) * The controllers now support the fallback controllers (a list of controllers that will be activated, when the spawned controllers fails by throwing an exception or returning ``return_type::ERROR`` during the ``update`` cycle) (`#1789 `_) +* The controllers can be easily introspect the internal member variables using the macro ``REGISTER_ROS2_CONTROL_INTROSPECTION`` (`#1918 `_) * A new ``SemanticComponentCommandInterface`` semantic component provides capabilities analogous to the ``SemanticComponentInterface``, but for write-only devices (`#1945 `_) * The new semantic command interface ``LedRgbDevice`` provides standard (command) interfaces for 3-channel LED devices (`#1945 `_) @@ -87,6 +88,7 @@ controller_manager * The ``ros2_control_node`` node has a new ``cpu_affinity`` parameter to bind the process to a specific CPU core. By default, this is not enabled. (`#1852 `_). * The ``--service-call-timeout`` was added as parameter to the helper scripts ``spawner.py``. Useful when the CPU load is high at startup and the service call does not return immediately (`#1808 `_). * The ``cpu_affinity`` parameter can now accept of types ``int`` or ``int_array`` to bind the process to a specific CPU core or multiple CPU cores. (`#1915 `_). +* The ``pal_statistics`` is now integrated into the controller_manager, so that the controllers, hardware components and the controller_manager can be easily introspected and monitored using the topics ``~/introspection_data/names`` and ``~/introspection_data/values`` (`#1918 `_). * A python module ``test_utils`` was added to the ``controller_manager`` package to help with integration testing (`#1955 `_). hardware_interface @@ -161,6 +163,7 @@ hardware_interface * With (`#1421 `_) a key-value storage is added to InterfaceInfo. This allows to define extra params with per Command-/StateInterface in the ``.ros2_control.xacro`` file. * With (`#1763 `_) parsing for SDF published to ``robot_description`` topic is now also supported. * With (`#1567 `_) all the Hardware components now have a fully functional asynchronous functionality, by simply adding ``is_async`` tag to the ros2_control tag in the URDF. This will allow the hardware components to run in a separate thread, and the controller manager will be able to run the controllers in parallel with the hardware components. +* The hardware components can be easily introspect the internal member variables using the macro ``REGISTER_ROS2_CONTROL_INTROSPECTION`` (`#1918 `_) joint_limits ************ diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index 673704c868..e3cf55df6d 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -22,6 +22,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/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index 87b5202734..1f693d1d34 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -25,6 +25,7 @@ #include "hardware_interface/component_parser.hpp" #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/introspection.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/lifecycle_state_names.hpp" #include "hardware_interface/types/trigger_type.hpp" @@ -93,7 +94,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod */ ActuatorInterface(const ActuatorInterface & other) = delete; - ActuatorInterface(ActuatorInterface && other) = default; + ActuatorInterface(ActuatorInterface && other) = delete; virtual ~ActuatorInterface() = default; @@ -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 actuator_states_; std::unordered_map actuator_commands_; std::atomic next_trigger_ = TriggerType::READ; + +protected: + pal_statistics::RegistrationsRAII stats_registrations_; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index 852d67b98f..2fa9648d2e 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -25,6 +25,7 @@ #include #include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/introspection.hpp" #include "hardware_interface/macros.hpp" namespace hardware_interface @@ -205,6 +206,24 @@ class StateInterface : public Handle { } + void registerIntrospection() const + { + if (std::holds_alternative(value_)) + { + std::function f = [this]() + { return value_ptr_ ? *value_ptr_ : std::numeric_limits::quiet_NaN(); }; + DEFAULT_REGISTER_ROS2_CONTROL_INTROSPECTION("state_interface." + get_name(), f); + } + } + + void unregisterIntrospection() const + { + if (std::holds_alternative(value_)) + { + DEFAULT_UNREGISTER_ROS2_CONTROL_INTROSPECTION("state_interface." + get_name()); + } + } + StateInterface(const StateInterface & other) = default; StateInterface(StateInterface && other) = default; @@ -232,6 +251,24 @@ class CommandInterface : public Handle CommandInterface(CommandInterface && other) = default; + void registerIntrospection() const + { + if (std::holds_alternative(value_)) + { + std::function f = [this]() + { return value_ptr_ ? *value_ptr_ : std::numeric_limits::quiet_NaN(); }; + DEFAULT_REGISTER_ROS2_CONTROL_INTROSPECTION("command_interface." + get_name(), f); + } + } + + void unregisterIntrospection() const + { + if (std::holds_alternative(value_)) + { + DEFAULT_UNREGISTER_ROS2_CONTROL_INTROSPECTION("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..d25b71c2d8 --- /dev/null +++ b/hardware_interface/include/hardware_interface/introspection.hpp @@ -0,0 +1,54 @@ +// 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" +#include "pal_statistics/pal_statistics_utils.hpp" + +namespace hardware_interface +{ +constexpr char DEFAULT_REGISTRY_KEY[] = "ros2_control"; +constexpr char DEFAULT_INTROSPECTION_TOPIC[] = "~/introspection_data"; + +#define REGISTER_ROS2_CONTROL_INTROSPECTION(ID, ENTITY) \ + REGISTER_ENTITY( \ + hardware_interface::DEFAULT_REGISTRY_KEY, get_name() + "." + ID, ENTITY, \ + &stats_registrations_, false); + +#define UNREGISTER_ROS2_CONTROL_INTROSPECTION(ID) \ + UNREGISTER_ENTITY(DEFAULT_REGISTRY_KEY, get_name() + "." + ID); + +#define CLEAR_ALL_ROS2_CONTROL_INTROSPECTION_REGISTRIES() CLEAR_ALL_REGISTRIES(); + +#define INITIALIZE_ROS2_CONTROL_INTROSPECTION_REGISTRY(node, topic, registry_key) \ + INITIALIZE_REGISTRY(node, topic, registry_key); + +#define START_ROS2_CONTROL_INTROSPECTION_PUBLISHER_THREAD(registry_key) \ + START_PUBLISH_THREAD(registry_key); + +#define PUBLISH_ROS2_CONTROL_INTROSPECTION_DATA_ASYNC(registry_key) \ + PUBLISH_ASYNC_STATISTICS(registry_key); + +#define DEFAULT_REGISTER_ROS2_CONTROL_INTROSPECTION(ID, ENTITY) \ + REGISTER_ENTITY(DEFAULT_REGISTRY_KEY, ID, ENTITY); + +#define DEFAULT_UNREGISTER_ROS2_CONTROL_INTROSPECTION(ID) \ + UNREGISTER_ENTITY(DEFAULT_REGISTRY_KEY, ID); +} // 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 c99138fc11..58a8b4790a 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -25,6 +25,7 @@ #include "hardware_interface/component_parser.hpp" #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/introspection.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/lifecycle_state_names.hpp" #include "lifecycle_msgs/msg/state.hpp" @@ -92,7 +93,7 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI */ SensorInterface(const SensorInterface & other) = delete; - SensorInterface(SensorInterface && other) = default; + SensorInterface(SensorInterface && other) = delete; virtual ~SensorInterface() = default; @@ -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 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 4337f5fd19..7577d0ebdc 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -25,6 +25,7 @@ #include "hardware_interface/component_parser.hpp" #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/introspection.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "hardware_interface/types/lifecycle_state_names.hpp" @@ -96,7 +97,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI */ SystemInterface(const SystemInterface & other) = delete; - SystemInterface(SystemInterface && other) = default; + SystemInterface(SystemInterface && other) = delete; virtual ~SystemInterface() = default; @@ -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 system_states_; std::unordered_map system_commands_; std::atomic next_trigger_ = TriggerType::READ; + +protected: + pal_statistics::RegistrationsRAII stats_registrations_; }; } // namespace hardware_interface diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index f890b1bfe9..2e68a60957 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -12,15 +12,16 @@ backward_ros control_msgs + joint_limits lifecycle_msgs + pal_statistics pluginlib rclcpp_lifecycle rcpputils realtime_tools + sdformat_urdf tinyxml2_vendor - joint_limits urdf - sdformat_urdf rcutils rcutils diff --git a/hardware_interface/src/actuator.cpp b/hardware_interface/src/actuator.cpp index 790898cfed..65b337aa62 100644 --- a/hardware_interface/src/actuator.cpp +++ b/hardware_interface/src/actuator.cpp @@ -98,6 +98,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())) @@ -119,6 +120,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) @@ -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 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 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/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index a99ae3db3b..e43c650d5d 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -611,6 +611,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 @@ -668,6 +669,7 @@ class ResourceStorage interface->get_name() + "]"); throw std::runtime_error(msg); } + interface->registerIntrospection(); return interface_name; } /// Adds exported state interfaces into internal storage. @@ -715,6 +717,7 @@ class ResourceStorage { for (const auto & interface : interface_names) { + state_interface_map_[interface]->unregisterIntrospection(); state_interface_map_.erase(interface); } } @@ -775,6 +778,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); } diff --git a/hardware_interface/src/sensor.cpp b/hardware_interface/src/sensor.cpp index 0ef4da36d4..87ea42790e 100644 --- a/hardware_interface/src/sensor.cpp +++ b/hardware_interface/src/sensor.cpp @@ -95,6 +95,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())) @@ -116,6 +117,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) @@ -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 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 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 694e45f2f3..8f626f7c8d 100644 --- a/hardware_interface/src/system.cpp +++ b/hardware_interface/src/system.cpp @@ -96,6 +96,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())) @@ -117,6 +118,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) @@ -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 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 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)