diff --git a/.github/workflows/jazzy-semi-binary-build.yml b/.github/workflows/jazzy-semi-binary-build.yml index d5e3a96835..ee86f3003a 100644 --- a/.github/workflows/jazzy-semi-binary-build.yml +++ b/.github/workflows/jazzy-semi-binary-build.yml @@ -51,7 +51,7 @@ jobs: upstream_workspace: ros2_control.${{ matrix.ROS_DISTRO }}.repos ref_for_scheduled_build: master semi-binary-clang: - uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@exclude_system_header with: ros_distro: jazzy ros_repo: testing diff --git a/.github/workflows/rolling-semi-binary-build.yml b/.github/workflows/rolling-semi-binary-build.yml index 6431b9f4fa..77f013fbee 100644 --- a/.github/workflows/rolling-semi-binary-build.yml +++ b/.github/workflows/rolling-semi-binary-build.yml @@ -51,7 +51,7 @@ jobs: upstream_workspace: ros2_control.${{ matrix.ROS_DISTRO }}.repos ref_for_scheduled_build: master semi-binary-clang: - uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@exclude_system_header with: # job for building only, no tests -> one distro is enough ros_distro: rolling diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index e41779cf7b..6cdfe1398e 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" @@ -330,6 +331,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_; @@ -341,6 +351,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 03d45fa384..1e8aafd7b4 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 @@ -61,6 +62,7 @@ return_type ControllerInterfaceBase::init( node_->register_on_cleanup( [this](const rclcpp_lifecycle::State & previous_state) -> CallbackReturn { + enable_introspection(false); if (is_async() && async_handler_ && async_handler_->is_running()) { async_handler_->stop_thread(); @@ -71,6 +73,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 +83,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)); @@ -137,6 +144,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(); @@ -233,4 +242,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/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 332d565238..b50e3f3b68 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -85,7 +85,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 d4b4076bb3..046eebdaf0 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" @@ -283,6 +284,8 @@ ControllerManager::ControllerManager( init_controller_manager(); } +ControllerManager::~ControllerManager() { CLEAR_ALL_REGISTRIES(); } + void ControllerManager::init_controller_manager() { // Get parameters needed for RT "update" loop to work @@ -321,6 +324,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::initialize_parameters() @@ -2562,6 +2569,8 @@ controller_interface::return_type ControllerManager::update( manage_switch(); } + PUBLISH_ASYNC_STATISTICS(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..8f4fb5b6ef --- /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 `_ 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 its 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 a187e62437..b1ccf5491b 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 `_) controller_manager ****************** @@ -83,6 +84,7 @@ controller_manager * The ``ros2_control_node`` node has a new ``lock_memory`` parameter to lock memory at startup to physical RAM in order to avoid page faults (`#1822 `_). * 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 ``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 `_). * 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 `_). hardware_interface @@ -156,6 +158,7 @@ hardware_interface * With (`#1683 `_) the ``rclcpp_lifecycle::State & get_state()`` and ``void set_state(const rclcpp_lifecycle::State & new_state)`` are replaced by ``rclcpp_lifecycle::State & get_lifecycle_state()`` and ``void set_lifecycle_state(const rclcpp_lifecycle::State & new_state)``. This change affects controllers and hardware. This is related to (`#1240 `_) as variant support introduces ``get_state`` and ``set_state`` methods for setting/getting state of handles. * 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. +* 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 f9637b4f07..75e93820c0 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/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index 8aa214e728..3933216f52 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" @@ -91,7 +92,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; @@ -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/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index 1dfd499c2c..015cca3ebb 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,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(); }; + 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 +253,28 @@ 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_ ? *value_ptr_ : std::numeric_limits::quiet_NaN(); }; + 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..ca36d88be3 --- /dev/null +++ b/hardware_interface/include/hardware_interface/introspection.hpp @@ -0,0 +1,33 @@ +// 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"; + +#define REGISTER_ROS2_CONTROL_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 455f6216a2..8a677221dc 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" @@ -91,7 +92,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; @@ -281,6 +282,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 @@ -300,6 +317,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..f5a7021b65 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" @@ -94,7 +95,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; @@ -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/package.xml b/hardware_interface/package.xml index 23b882ab02..df4f6b6cbd 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -12,14 +12,15 @@ backward_ros control_msgs + joint_limits lifecycle_msgs + pal_statistics pluginlib rclcpp_lifecycle rcpputils + 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 0e4ae95ca9..ae99472b6c 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 e77917af9d..47890b2f2e 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -635,6 +635,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 @@ -692,6 +693,7 @@ class ResourceStorage interface->get_name() + "]"); throw std::runtime_error(msg); } + interface->registerIntrospection(); return interface_name; } /// Adds exported state interfaces into internal storage. @@ -739,6 +741,7 @@ class ResourceStorage { for (const auto & interface : interface_names) { + state_interface_map_[interface]->unregisterIntrospection(); state_interface_map_.erase(interface); } } @@ -799,6 +802,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 ff193ff8e1..350044b877 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 9a27aa4f68..dcaa43c6a6 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)