From e2870e6884fd70342831c0224363520cca1d7b04 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sat, 6 Dec 2025 21:31:44 +0100 Subject: [PATCH 1/3] Fix the blocking calls of lifecycle_state in the real-time loop (#2884) (cherry picked from commit dca9b57f6f186ba321c976090265e778ada5531e) # Conflicts: # doc/release_notes.rst --- .../controller_interface_base.hpp | 17 +++++ .../src/chainable_controller_interface.cpp | 2 +- .../src/controller_interface_base.cpp | 67 ++++++++++++++----- .../test/test_controller_interface.cpp | 2 + controller_manager/doc/userdoc.rst | 3 + controller_manager/src/controller_manager.cpp | 15 ++--- .../test_chainable_controller.cpp | 28 +++++++- .../test/test_controller/test_controller.cpp | 21 ++++++ .../test_controller_manager_urdf_passing.cpp | 4 ++ doc/release_notes.rst | 9 +++ .../hardware_interface/hardware_component.hpp | 2 + .../hardware_component_interface.hpp | 14 +++- hardware_interface/src/hardware_component.cpp | 30 +++++---- hardware_interface/src/resource_manager.cpp | 28 ++++---- .../test/test_component_interfaces.cpp | 16 +++++ .../test_actuator_exclusive_interfaces.cpp | 20 ++++++ .../test/test_components/test_system.cpp | 18 +++++ 17 files changed, 234 insertions(+), 62 deletions(-) diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index 8d02b3cb5d..48074c7fb5 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -29,6 +29,7 @@ #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/loaned_state_interface.hpp" +#include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/version.h" #include "rclcpp_lifecycle/lifecycle_node.hpp" @@ -199,8 +200,23 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy std::shared_ptr get_node() const; + /** + * Get the current lifecycle state of the controller node. + * \note Accessing members of the returned rclcpp_lifecycle::State is not real-time safe and + * should not be called in the control loop. + * \note This method is thread safe. + * \returns lifecycle state of the controller node. + */ const rclcpp_lifecycle::State & get_lifecycle_state() const; + /** + * Get the lifecycle id of the controller node that is cached internally + * to avoid calls to get_lifecycle_state() in the real-time control loop. + * \note This method is real-time safe and thread safe and can be called in the control loop. + * \returns lifecycle id of the controller node. + */ + uint8_t get_lifecycle_id() const; + unsigned int get_update_rate() const; bool is_async() const; @@ -379,6 +395,7 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy controller_interface::ControllerInterfaceParams ctrl_itf_params_; std::atomic_bool skip_async_triggers_ = false; ControllerUpdateStats trigger_stats_; + mutable std::atomic lifecycle_id_ = lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN; protected: pal_statistics::RegistrationsRAII stats_registrations_; diff --git a/controller_interface/src/chainable_controller_interface.cpp b/controller_interface/src/chainable_controller_interface.cpp index d13ec19129..1a984b807a 100644 --- a/controller_interface/src/chainable_controller_interface.cpp +++ b/controller_interface/src/chainable_controller_interface.cpp @@ -200,7 +200,7 @@ bool ChainableControllerInterface::set_chained_mode(bool chained_mode) { bool result = false; - if (get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + if (get_lifecycle_id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { result = on_set_chained_mode(chained_mode); diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index 94779f2c0b..d3f39ae3ce 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -61,6 +61,7 @@ return_type ControllerInterfaceBase::init( ctrl_itf_params_.controller_name, ctrl_itf_params_.node_namespace, ctrl_itf_params_.node_options, false); // disable LifecycleNode service interfaces + lifecycle_id_.store(this->get_lifecycle_state().id(), std::memory_order_release); try { @@ -76,22 +77,12 @@ return_type ControllerInterfaceBase::init( return return_type::ERROR; } - switch (on_init()) - { - case LifecycleNodeInterface::CallbackReturn::SUCCESS: - break; - case LifecycleNodeInterface::CallbackReturn::ERROR: - case LifecycleNodeInterface::CallbackReturn::FAILURE: - RCLCPP_DEBUG( - get_node()->get_logger(), - "Calling shutdown transition of controller node '%s' due to init failure.", - get_node()->get_name()); - node_->shutdown(); - return return_type::ERROR; - } - node_->register_on_configure( - std::bind(&ControllerInterfaceBase::on_configure, this, std::placeholders::_1)); + [this](const rclcpp_lifecycle::State & previous_state) -> CallbackReturn + { + lifecycle_id_.store(this->get_lifecycle_state().id(), std::memory_order_release); + return on_configure(previous_state); + }); node_->register_on_cleanup( [this](const rclcpp_lifecycle::State & previous_state) -> CallbackReturn @@ -100,19 +91,21 @@ return_type ControllerInterfaceBase::init( // it in `on_configure` and `on_deactivate` - see the docs for details enable_introspection(false); this->stop_async_handler_thread(); + lifecycle_id_.store(this->get_lifecycle_state().id(), std::memory_order_release); return on_cleanup(previous_state); }); node_->register_on_activate( [this](const rclcpp_lifecycle::State & previous_state) -> CallbackReturn { - skip_async_triggers_.store(false); + skip_async_triggers_.store(false, std::memory_order_release); 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 async_handler_->reset_variables(); } + lifecycle_id_.store(this->get_lifecycle_state().id(), std::memory_order_release); return on_activate(previous_state); }); @@ -120,6 +113,7 @@ return_type ControllerInterfaceBase::init( [this](const rclcpp_lifecycle::State & previous_state) -> CallbackReturn { enable_introspection(false); + lifecycle_id_.store(this->get_lifecycle_state().id(), std::memory_order_release); return on_deactivate(previous_state); }); @@ -127,6 +121,7 @@ return_type ControllerInterfaceBase::init( [this](const rclcpp_lifecycle::State & previous_state) -> CallbackReturn { this->stop_async_handler_thread(); + lifecycle_id_.store(this->get_lifecycle_state().id(), std::memory_order_release); auto transition_state_status = on_shutdown(previous_state); this->release_interfaces(); return transition_state_status; @@ -136,16 +131,32 @@ return_type ControllerInterfaceBase::init( [this](const rclcpp_lifecycle::State & previous_state) -> CallbackReturn { this->stop_async_handler_thread(); + lifecycle_id_.store(this->get_lifecycle_state().id(), std::memory_order_release); auto transition_state_status = on_error(previous_state); this->release_interfaces(); return transition_state_status; }); + switch (on_init()) + { + case LifecycleNodeInterface::CallbackReturn::SUCCESS: + break; + case LifecycleNodeInterface::CallbackReturn::ERROR: + case LifecycleNodeInterface::CallbackReturn::FAILURE: + RCLCPP_ERROR( + get_node()->get_logger(), + "Calling shutdown transition of controller node '%s' due to init failure.", + get_node()->get_name()); + node_->shutdown(); + return return_type::ERROR; + } + return return_type::OK; } const rclcpp_lifecycle::State & ControllerInterfaceBase::configure() { + lifecycle_id_.store(this->get_lifecycle_state().id(), std::memory_order_release); // TODO(destogl): this should actually happen in "on_configure" but I am not sure how to get // overrides correctly in combination with std::bind. The goal is to have the following calls: // 1. CM: controller.get_node()->configure() @@ -216,7 +227,9 @@ const rclcpp_lifecycle::State & ControllerInterfaceBase::configure() REGISTER_ROS2_CONTROL_INTROSPECTION("failed_triggers", &trigger_stats_.failed_triggers); trigger_stats_.reset(); - return get_node()->configure(); + const auto & return_value = get_node()->configure(); + lifecycle_id_.store(return_value.id(), std::memory_order_release); + return return_value; } void ControllerInterfaceBase::assign_interfaces( @@ -242,6 +255,24 @@ const rclcpp_lifecycle::State & ControllerInterfaceBase::get_lifecycle_state() c return node_->get_current_state(); } +uint8_t ControllerInterfaceBase::get_lifecycle_id() const +{ + const auto id = lifecycle_id_.load(std::memory_order_acquire); + if ( + id == lifecycle_msgs::msg::State::TRANSITION_STATE_ACTIVATING || + id == lifecycle_msgs::msg::State::TRANSITION_STATE_DEACTIVATING || + id == lifecycle_msgs::msg::State::TRANSITION_STATE_CLEANINGUP || + id == lifecycle_msgs::msg::State::TRANSITION_STATE_CONFIGURING || + id == lifecycle_msgs::msg::State::TRANSITION_STATE_SHUTTINGDOWN || + id == lifecycle_msgs::msg::State::TRANSITION_STATE_ERRORPROCESSING) + { + const auto new_id = this->get_lifecycle_state().id(); + lifecycle_id_.store(new_id, std::memory_order_release); + return new_id; + } + return id; +} + ControllerUpdateStatus ControllerInterfaceBase::trigger_update( const rclcpp::Time & time, const rclcpp::Duration & period) { @@ -342,7 +373,7 @@ void ControllerInterfaceBase::wait_for_trigger_update_to_finish() void ControllerInterfaceBase::prepare_for_deactivation() { - skip_async_triggers_.store(true); + skip_async_triggers_.store(true, std::memory_order_release); this->wait_for_trigger_update_to_finish(); } diff --git a/controller_interface/test/test_controller_interface.cpp b/controller_interface/test/test_controller_interface.cpp index 65b2cba3fe..42bdcef1c6 100644 --- a/controller_interface/test/test_controller_interface.cpp +++ b/controller_interface/test/test_controller_interface.cpp @@ -221,6 +221,7 @@ TEST(TestableControllerInterfaceInitError, init_with_error) ASSERT_EQ( controller.get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED); + ASSERT_EQ(controller.get_lifecycle_id(), lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED); rclcpp::shutdown(); } @@ -240,6 +241,7 @@ TEST(TestableControllerInterfaceInitFailure, init_with_failure) ASSERT_EQ( controller.get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED); + ASSERT_EQ(controller.get_lifecycle_id(), lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED); rclcpp::shutdown(); } diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index 1d5200989e..eddc0aecd9 100644 --- a/controller_manager/doc/userdoc.rst +++ b/controller_manager/doc/userdoc.rst @@ -59,6 +59,9 @@ Alternatives to the standard kernel include Though installing a realtime-kernel will definitely get the best results when it comes to low jitter, using a lowlatency kernel can improve things a lot with being really easy to install. +.. note:: + Avoid using the get_lifecycle_state() method in the real-time control loop of the controllers and the hardware components as it is not real-time safe. + Publishers ----------- diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 1cf274bc6f..6856cd006d 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -65,14 +65,12 @@ static const rmw_qos_profile_t qos_services = { inline bool is_controller_unconfigured( const controller_interface::ControllerInterfaceBase & controller) { - return controller.get_lifecycle_state().id() == - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED; + return controller.get_lifecycle_id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED; } inline bool is_controller_inactive(const controller_interface::ControllerInterfaceBase & controller) { - return controller.get_lifecycle_state().id() == - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE; + return controller.get_lifecycle_id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE; } inline bool is_controller_inactive( @@ -83,7 +81,7 @@ inline bool is_controller_inactive( inline bool is_controller_active(const controller_interface::ControllerInterfaceBase & controller) { - return controller.get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE; + return controller.get_lifecycle_id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE; } inline bool is_controller_active( @@ -1251,7 +1249,7 @@ controller_interface::return_type ControllerManager::configure_controller( } auto controller = found_it->c; - auto state = controller->get_lifecycle_state(); + const auto & state = controller->get_lifecycle_state(); if ( state.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE || state.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) @@ -1262,7 +1260,6 @@ controller_interface::return_type ControllerManager::configure_controller( return controller_interface::return_type::ERROR; } - auto new_state = controller->get_lifecycle_state(); if (state.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { RCLCPP_DEBUG( @@ -1277,7 +1274,7 @@ controller_interface::return_type ControllerManager::configure_controller( try { - new_state = controller->configure(); + const auto & new_state = controller->configure(); if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { RCLCPP_ERROR( @@ -3070,8 +3067,6 @@ controller_interface::return_type ControllerManager::update( loaded_controller.info.name.c_str()); continue; } - // TODO(v-lopez) we could cache this information - // https://github.com/ros-controls/ros2_control/issues/153 if (is_controller_active(*loaded_controller.c)) { if ( diff --git a/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp b/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp index f377830418..c160c41227 100644 --- a/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp +++ b/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp @@ -20,6 +20,19 @@ #include "lifecycle_msgs/msg/state.hpp" +namespace +{ +void verify_internal_lifecycle_id(uint8_t expected_id, uint8_t actual_id) +{ + if (expected_id != actual_id) + { + throw std::runtime_error( + "Internal lifecycle ID does not match the expected lifecycle ID. Expected: " + + std::to_string(expected_id) + ", Actual: " + std::to_string(actual_id)); + } +} +} // namespace + namespace test_chainable_controller { TestChainableController::TestChainableController() @@ -32,6 +45,7 @@ TestChainableController::TestChainableController() controller_interface::InterfaceConfiguration TestChainableController::command_interface_configuration() const { + verify_internal_lifecycle_id(get_lifecycle_id(), get_lifecycle_state().id()); if ( get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) @@ -48,6 +62,7 @@ TestChainableController::command_interface_configuration() const controller_interface::InterfaceConfiguration TestChainableController::state_interface_configuration() const { + verify_internal_lifecycle_id(get_lifecycle_id(), get_lifecycle_state().id()); if ( get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) @@ -71,6 +86,7 @@ TestChainableController::state_interface_configuration() const controller_interface::return_type TestChainableController::update_reference_from_subscribers( const rclcpp::Time & time, const rclcpp::Duration & /*period*/) { + verify_internal_lifecycle_id(get_lifecycle_id(), get_lifecycle_state().id()); if (time.get_clock_type() != RCL_ROS_TIME) { throw std::runtime_error( @@ -102,6 +118,7 @@ controller_interface::return_type TestChainableController::update_reference_from controller_interface::return_type TestChainableController::update_and_write_commands( const rclcpp::Time & time, const rclcpp::Duration & /*period*/) { + verify_internal_lifecycle_id(get_lifecycle_id(), get_lifecycle_state().id()); if (time.get_clock_type() != RCL_ROS_TIME) { throw std::runtime_error( @@ -132,11 +149,16 @@ controller_interface::return_type TestChainableController::update_and_write_comm return update_return_value; } -CallbackReturn TestChainableController::on_init() { return CallbackReturn::SUCCESS; } +CallbackReturn TestChainableController::on_init() +{ + verify_internal_lifecycle_id(get_lifecycle_id(), get_lifecycle_state().id()); + return CallbackReturn::SUCCESS; +} CallbackReturn TestChainableController::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { + verify_internal_lifecycle_id(get_lifecycle_id(), get_lifecycle_state().id()); joints_command_subscriber_ = get_node()->create_subscription( "~/commands", rclcpp::SystemDefaultsQoS(), [this](const CmdType::SharedPtr msg) @@ -166,6 +188,7 @@ CallbackReturn TestChainableController::on_configure( CallbackReturn TestChainableController::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { + verify_internal_lifecycle_id(get_lifecycle_id(), get_lifecycle_state().id()); if (!is_in_chained_mode()) { auto msg = rt_command_ptr_.readFromRT(); @@ -178,6 +201,7 @@ CallbackReturn TestChainableController::on_activate( CallbackReturn TestChainableController::on_cleanup( const rclcpp_lifecycle::State & /*previous_state*/) { + verify_internal_lifecycle_id(get_lifecycle_id(), get_lifecycle_state().id()); joints_command_subscriber_.reset(); return CallbackReturn::SUCCESS; } @@ -185,6 +209,7 @@ CallbackReturn TestChainableController::on_cleanup( std::vector TestChainableController::on_export_state_interfaces() { + verify_internal_lifecycle_id(get_lifecycle_id(), get_lifecycle_state().id()); std::vector state_interfaces; for (size_t i = 0; i < exported_state_interface_names_.size(); ++i) @@ -200,6 +225,7 @@ TestChainableController::on_export_state_interfaces() std::vector TestChainableController::on_export_reference_interfaces() { + verify_internal_lifecycle_id(get_lifecycle_id(), get_lifecycle_state().id()); std::vector reference_interfaces; for (size_t i = 0; i < reference_interface_names_.size(); ++i) diff --git a/controller_manager/test/test_controller/test_controller.cpp b/controller_manager/test/test_controller/test_controller.cpp index b3beff5c10..137f944974 100644 --- a/controller_manager/test/test_controller/test_controller.cpp +++ b/controller_manager/test/test_controller/test_controller.cpp @@ -19,6 +19,19 @@ #include "lifecycle_msgs/msg/state.hpp" +namespace +{ +void verify_internal_lifecycle_id(uint8_t expected_id, uint8_t actual_id) +{ + if (expected_id != actual_id) + { + throw std::runtime_error( + "Internal lifecycle ID does not match the expected lifecycle ID. Expected: " + + std::to_string(expected_id) + ", Actual: " + std::to_string(actual_id)); + } +} +} // namespace + namespace test_controller { TestController::TestController() @@ -30,6 +43,7 @@ TestController::TestController() controller_interface::InterfaceConfiguration TestController::command_interface_configuration() const { + verify_internal_lifecycle_id(get_lifecycle_id(), get_lifecycle_state().id()); if ( get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) @@ -45,6 +59,7 @@ controller_interface::InterfaceConfiguration TestController::command_interface_c controller_interface::InterfaceConfiguration TestController::state_interface_configuration() const { + verify_internal_lifecycle_id(get_lifecycle_id(), get_lifecycle_state().id()); if ( get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) @@ -61,6 +76,7 @@ controller_interface::InterfaceConfiguration TestController::state_interface_con controller_interface::return_type TestController::update( const rclcpp::Time & time, const rclcpp::Duration & period) { + verify_internal_lifecycle_id(get_lifecycle_id(), get_lifecycle_state().id()); if (throw_on_update) { throw std::runtime_error("Exception from TestController::update() as requested."); @@ -107,6 +123,7 @@ controller_interface::return_type TestController::update( CallbackReturn TestController::on_init() { + verify_internal_lifecycle_id(get_lifecycle_id(), get_lifecycle_state().id()); if (throw_on_initialize) { throw std::runtime_error("Exception from TestController::on_init() as requested."); @@ -117,6 +134,7 @@ CallbackReturn TestController::on_init() CallbackReturn TestController::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) { auto ctrl_node = get_node(); + verify_internal_lifecycle_id(get_lifecycle_id(), get_lifecycle_state().id()); if (!ctrl_node->has_parameter("command_interfaces")) { ctrl_node->declare_parameter("command_interfaces", std::vector({})); @@ -163,6 +181,7 @@ CallbackReturn TestController::on_configure(const rclcpp_lifecycle::State & /*pr CallbackReturn TestController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) { + verify_internal_lifecycle_id(get_lifecycle_id(), get_lifecycle_state().id()); if (external_commands_for_testing_.empty()) { external_commands_for_testing_.resize(command_interfaces_.size(), 0.0); @@ -180,6 +199,7 @@ CallbackReturn TestController::on_activate(const rclcpp_lifecycle::State & /*pre CallbackReturn TestController::on_cleanup(const rclcpp_lifecycle::State & /*previous_state*/) { + verify_internal_lifecycle_id(get_lifecycle_id(), get_lifecycle_state().id()); if (simulate_cleanup_failure) { return CallbackReturn::FAILURE; @@ -194,6 +214,7 @@ CallbackReturn TestController::on_cleanup(const rclcpp_lifecycle::State & /*prev CallbackReturn TestController::on_shutdown(const rclcpp_lifecycle::State &) { + verify_internal_lifecycle_id(get_lifecycle_id(), get_lifecycle_state().id()); if (shutdown_calls) { (*shutdown_calls)++; diff --git a/controller_manager/test/test_controller_manager_urdf_passing.cpp b/controller_manager/test/test_controller_manager_urdf_passing.cpp index ed63a8c6de..062f976a8b 100644 --- a/controller_manager/test/test_controller_manager_urdf_passing.cpp +++ b/controller_manager/test/test_controller_manager_urdf_passing.cpp @@ -71,6 +71,7 @@ class TestControllerManagerWithTestableCM ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller_->get_lifecycle_state().id()); + ASSERT_EQ(test_controller_->get_lifecycle_id(), test_controller_->get_lifecycle_state().id()); } void configure_and_try_switch_that_returns_error() @@ -80,6 +81,7 @@ class TestControllerManagerWithTestableCM EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller_->get_lifecycle_state().id()); + ASSERT_EQ(test_controller_->get_lifecycle_id(), test_controller_->get_lifecycle_state().id()); // Set ControllerManager into Debug-Mode output to have detailed output on updating controllers cm_->get_logger().set_level(rclcpp::Logger::Level::Debug); @@ -91,6 +93,7 @@ class TestControllerManagerWithTestableCM EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller_->get_lifecycle_state().id()); + ASSERT_EQ(test_controller_->get_lifecycle_id(), test_controller_->get_lifecycle_state().id()); } void try_successful_switch() @@ -102,6 +105,7 @@ class TestControllerManagerWithTestableCM EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller_->get_lifecycle_state().id()); + ASSERT_EQ(test_controller_->get_lifecycle_id(), test_controller_->get_lifecycle_state().id()); } std::shared_ptr test_controller_; diff --git a/doc/release_notes.rst b/doc/release_notes.rst index db3a1077fb..98a07658c2 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -32,6 +32,11 @@ For details see the controller_manager section. * The new semantic command interface ``LedRgbDevice`` provides standard (command) interfaces for 3-channel LED devices (`#1945 `_) * The new ``MagneticFieldSensor`` semantic component provides an interface for reading data from magnetometers. `(#2627 `__) * The controller_manager will now deactivate the entire controller chain if a controller in the chain fails during the update cycle. `(#2681 `__) +<<<<<<< HEAD +======= +* The update rate of the controller will now be approximated to a closer achievable frequency, when its frequency is not achievable with the current controller manager update rate. (`#2828 `__) +* The lifecycle ID is cached internally in the controller to avoid calls to get_lifecycle_state() in the real-time control loop. (`#2884 `__) +>>>>>>> dca9b57 (Fix the blocking calls of lifecycle_state in the real-time loop (#2884)) controller_manager ****************** @@ -107,6 +112,7 @@ controller_manager hardware_interface ****************** +<<<<<<< HEAD * A portable version for string-to-double conversion was added: ``hardware_interface::stod`` (`#1257 `_) * ``test_components`` was moved to its own package (`#1325 `_) * The ``ros2_control`` tag now supports parsing of the limits from the URDF into the ``HardwareInfo`` structure. More conservative limits can be defined using the ``min`` and ``max`` attributes per interface (`#1472 `_) @@ -195,6 +201,9 @@ hardware_interface joint_limits ************ * Add header to import limits from standard URDF definition (`#1298 `_) +======= +* The lifecycle ID is cached internally in the controller to avoid calls to get_lifecycle_state() in the real-time control loop. (`#2884 `__) +>>>>>>> dca9b57 (Fix the blocking calls of lifecycle_state in the real-time loop (#2884)) ros2controlcli ************** diff --git a/hardware_interface/include/hardware_interface/hardware_component.hpp b/hardware_interface/include/hardware_interface/hardware_component.hpp index 2d506f8bd1..8eada597e1 100644 --- a/hardware_interface/include/hardware_interface/hardware_component.hpp +++ b/hardware_interface/include/hardware_interface/hardware_component.hpp @@ -97,6 +97,8 @@ class HardwareComponent final const rclcpp_lifecycle::State & get_lifecycle_state() const; + uint8_t get_lifecycle_id() const; + const rclcpp::Time & get_last_read_time() const; const rclcpp::Time & get_last_write_time() const; diff --git a/hardware_interface/include/hardware_interface/hardware_component_interface.hpp b/hardware_interface/include/hardware_interface/hardware_component_interface.hpp index 82e378ef3f..4e0569d58a 100644 --- a/hardware_interface/include/hardware_interface/hardware_component_interface.hpp +++ b/hardware_interface/include/hardware_interface/hardware_component_interface.hpp @@ -181,8 +181,8 @@ class HardwareComponentInterface : public rclcpp_lifecycle::node_interfaces::Lif return ret_read; } if ( - !is_sensor_type && - this->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + !is_sensor_type && lifecycle_id_cache_.load(std::memory_order_acquire) == + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { const auto write_start_time = std::chrono::steady_clock::now(); const auto ret_write = write(time, period); @@ -610,19 +610,28 @@ class HardwareComponentInterface : public rclcpp_lifecycle::node_interfaces::Lif /// Get life-cycle state of the hardware. /** + * \note Accessing members of the returned rclcpp_lifecycle::State is not real-time safe and + * should not be called in the control loop. + * \note This method is thread safe. * \return state. */ const rclcpp_lifecycle::State & get_lifecycle_state() const { return lifecycle_state_; } /// Set life-cycle state of the hardware. /** + * Get the lifecycle id of the hardware component interface that is cached internally to avoid + * calls to get_lifecycle_state() in the real-time control loop. + * \note This method is real-time safe and thread safe and can be called in the control loop. * \return state. */ void set_lifecycle_state(const rclcpp_lifecycle::State & new_state) { lifecycle_state_ = new_state; + lifecycle_id_cache_.store(new_state.id(), std::memory_order_release); } + uint8_t get_lifecycle_id() const { return lifecycle_id_cache_.load(std::memory_order_acquire); } + /// Does the state interface exist? /** * \param[in] interface_name The name of the state interface. @@ -948,6 +957,7 @@ class HardwareComponentInterface : public rclcpp_lifecycle::node_interfaces::Lif std::unordered_map unlisted_command_interfaces_; rclcpp_lifecycle::State lifecycle_state_; + std::atomic lifecycle_id_cache_ = lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN; std::unique_ptr> async_handler_; // Exported Command- and StateInterfaces in order they are listed in the hardware description. diff --git a/hardware_interface/src/hardware_component.cpp b/hardware_interface/src/hardware_component.cpp index 24a166ce05..8e85350b66 100644 --- a/hardware_interface/src/hardware_component.cpp +++ b/hardware_interface/src/hardware_component.cpp @@ -71,7 +71,7 @@ const rclcpp_lifecycle::State & HardwareComponent::initialize( const hardware_interface::HardwareComponentParams & params) { std::unique_lock lock(component_mutex_); - if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) + if (impl_->get_lifecycle_id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) { switch (impl_->init(params)) { @@ -95,7 +95,7 @@ const rclcpp_lifecycle::State & HardwareComponent::initialize( const rclcpp_lifecycle::State & HardwareComponent::configure() { std::unique_lock lock(component_mutex_); - if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) + if (impl_->get_lifecycle_id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) { impl_->pause_async_operations(); switch (impl_->on_configure(impl_->get_lifecycle_state())) @@ -123,7 +123,7 @@ const rclcpp_lifecycle::State & HardwareComponent::cleanup() { std::unique_lock lock(component_mutex_); impl_->enable_introspection(false); - if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + if (impl_->get_lifecycle_id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { impl_->pause_async_operations(); switch (impl_->on_cleanup(impl_->get_lifecycle_state())) @@ -148,8 +148,8 @@ const rclcpp_lifecycle::State & HardwareComponent::shutdown() std::unique_lock lock(component_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) + impl_->get_lifecycle_id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN && + impl_->get_lifecycle_id() != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) { impl_->pause_async_operations(); switch (impl_->on_shutdown(impl_->get_lifecycle_state())) @@ -178,7 +178,7 @@ const rclcpp_lifecycle::State & HardwareComponent::activate() { write_statistics_.reset_statistics(); } - if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + if (impl_->get_lifecycle_id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { impl_->pause_async_operations(); impl_->prepare_for_activation(); @@ -207,7 +207,7 @@ const rclcpp_lifecycle::State & HardwareComponent::deactivate() { std::unique_lock lock(component_mutex_); impl_->enable_introspection(false); - if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + if (impl_->get_lifecycle_id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { impl_->pause_async_operations(); switch (impl_->on_deactivate(impl_->get_lifecycle_state())) @@ -235,8 +235,8 @@ const rclcpp_lifecycle::State & HardwareComponent::error() std::unique_lock lock(component_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) + impl_->get_lifecycle_id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN && + impl_->get_lifecycle_id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) { impl_->pause_async_operations(); switch (impl_->on_error(impl_->get_lifecycle_state())) @@ -342,6 +342,8 @@ const rclcpp_lifecycle::State & HardwareComponent::get_lifecycle_state() const return impl_->get_lifecycle_state(); } +uint8_t HardwareComponent::get_lifecycle_id() const { return impl_->get_lifecycle_id(); } + const rclcpp::Time & HardwareComponent::get_last_read_time() const { return last_read_cycle_time_; } const rclcpp::Time & HardwareComponent::get_last_write_time() const @@ -361,14 +363,14 @@ const HardwareComponentStatisticsCollector & HardwareComponent::get_write_statis return_type HardwareComponent::read(const rclcpp::Time & time, const rclcpp::Duration & period) { - if (lifecycleStateThatRequiresNoAction(impl_->get_lifecycle_state().id())) + if (lifecycleStateThatRequiresNoAction(impl_->get_lifecycle_id())) { last_read_cycle_time_ = time; return return_type::OK; } if ( - impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || - impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + impl_->get_lifecycle_id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || + impl_->get_lifecycle_id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { const auto trigger_result = impl_->trigger_read(time, period); if (trigger_result.result == return_type::ERROR) @@ -401,13 +403,13 @@ return_type HardwareComponent::write(const rclcpp::Time & time, const rclcpp::Du return return_type::OK; } - if (lifecycleStateThatRequiresNoAction(impl_->get_lifecycle_state().id())) + if (lifecycleStateThatRequiresNoAction(impl_->get_lifecycle_id())) { last_write_cycle_time_ = time; return return_type::OK; } // only call write in the active state - if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + if (impl_->get_lifecycle_id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { const auto trigger_result = impl_->trigger_write(time, period); if (trigger_result.result == return_type::ERROR) diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 9dd14e19da..b42cdb7454 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -569,7 +569,7 @@ class ResourceStorage switch (target_state.id()) { case State::PRIMARY_STATE_UNCONFIGURED: - switch (hardware.get_lifecycle_state().id()) + switch (hardware.get_lifecycle_id()) { case State::PRIMARY_STATE_UNCONFIGURED: result = true; @@ -593,7 +593,7 @@ class ResourceStorage } break; case State::PRIMARY_STATE_INACTIVE: - switch (hardware.get_lifecycle_state().id()) + switch (hardware.get_lifecycle_id()) { case State::PRIMARY_STATE_UNCONFIGURED: result = configure_hardware(hardware); @@ -613,7 +613,7 @@ class ResourceStorage } break; case State::PRIMARY_STATE_ACTIVE: - switch (hardware.get_lifecycle_state().id()) + switch (hardware.get_lifecycle_id()) { case State::PRIMARY_STATE_UNCONFIGURED: result = configure_hardware(hardware); @@ -637,7 +637,7 @@ class ResourceStorage } break; case State::PRIMARY_STATE_FINALIZED: - switch (hardware.get_lifecycle_state().id()) + switch (hardware.get_lifecycle_id()) { case State::PRIMARY_STATE_UNCONFIGURED: result = shutdown_hardware(hardware); @@ -1976,8 +1976,8 @@ ResourceManager::get_components_status() { for (auto & component : container) { - resource_storage_->hardware_info_map_[component.get_name()].state = - component.get_lifecycle_state(); + resource_storage_->hardware_info_map_[component.get_name()].state = rclcpp_lifecycle::State( + component.get_lifecycle_state().id(), component.get_lifecycle_state().label()); } }; @@ -2086,8 +2086,7 @@ bool ResourceManager::prepare_command_mode_switch( } if ( !start_interfaces_buffer.empty() && - component.get_lifecycle_state().id() == - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE && + component.get_lifecycle_id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE && !allow_controller_activation_with_inactive_hardware) { RCLCPP_WARN( @@ -2097,9 +2096,8 @@ bool ResourceManager::prepare_command_mode_switch( return false; } if ( - component.get_lifecycle_state().id() == - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || - component.get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + component.get_lifecycle_id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || + component.get_lifecycle_id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { try { @@ -2193,8 +2191,7 @@ bool ResourceManager::perform_command_mode_switch( } if ( !start_interfaces_buffer.empty() && - component.get_lifecycle_state().id() == - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE && + component.get_lifecycle_id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE && !allow_controller_activation_with_inactive_hardware) { RCLCPP_WARN( @@ -2204,9 +2201,8 @@ bool ResourceManager::perform_command_mode_switch( return false; } if ( - component.get_lifecycle_state().id() == - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || - component.get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + component.get_lifecycle_id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || + component.get_lifecycle_id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { try { diff --git a/hardware_interface/test/test_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp index 81bcc020dd..78166e5d8e 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -1557,6 +1557,7 @@ TEST(TestComponentInterfaces, dummy_actuator_read_error_behavior) state = actuator_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); + ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, actuator_hw.get_lifecycle_id()); ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); @@ -1571,6 +1572,7 @@ TEST(TestComponentInterfaces, dummy_actuator_read_error_behavior) state = actuator_hw.get_lifecycle_state(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + ASSERT_EQ(actuator_hw.get_lifecycle_id(), actuator_hw.get_lifecycle_state().id()); // activate again and expect reset values state = actuator_hw.configure(); @@ -1580,6 +1582,7 @@ TEST(TestComponentInterfaces, dummy_actuator_read_error_behavior) state = actuator_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); + ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, actuator_hw.get_lifecycle_id()); ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); @@ -1594,11 +1597,13 @@ TEST(TestComponentInterfaces, dummy_actuator_read_error_behavior) state = actuator_hw.get_lifecycle_state(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); + ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, actuator_hw.get_lifecycle_id()); // can not change state anymore state = actuator_hw.configure(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); + ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, actuator_hw.get_lifecycle_id()); } // END @@ -1836,6 +1841,7 @@ TEST(TestComponentInterfaces, dummy_sensor_read_error_behavior) state = sensor_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); + ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, sensor_hw.get_lifecycle_id()); ASSERT_EQ(hardware_interface::return_type::OK, sensor_hw.read(TIME, PERIOD)); @@ -1849,6 +1855,7 @@ TEST(TestComponentInterfaces, dummy_sensor_read_error_behavior) state = sensor_hw.get_lifecycle_state(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, sensor_hw.get_lifecycle_id()); // Noting should change because it is UNCONFIGURED for (auto step = 0u; step < 10; ++step) @@ -1863,6 +1870,7 @@ TEST(TestComponentInterfaces, dummy_sensor_read_error_behavior) state = sensor_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); + ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, sensor_hw.get_lifecycle_id()); // Initiate unrecoverable error - call read 99 times OK and on 100-time will return error for (auto i = 1ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) @@ -1874,6 +1882,7 @@ TEST(TestComponentInterfaces, dummy_sensor_read_error_behavior) state = sensor_hw.get_lifecycle_state(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); + ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, sensor_hw.get_lifecycle_id()); // Noting should change because it is FINALIZED for (auto step = 0u; step < 10; ++step) @@ -1885,6 +1894,7 @@ TEST(TestComponentInterfaces, dummy_sensor_read_error_behavior) state = sensor_hw.configure(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); + ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, sensor_hw.get_lifecycle_id()); } // END @@ -2188,6 +2198,7 @@ TEST(TestComponentInterfaces, dummy_system_default_write_error_behavior) auto state = system_hw.initialize(params); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, system_hw.get_lifecycle_id()); auto state_interfaces = system_hw.export_state_interfaces(); auto command_interfaces = system_hw.export_command_interfaces(); @@ -2195,6 +2206,7 @@ TEST(TestComponentInterfaces, dummy_system_default_write_error_behavior) state = system_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); + ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, system_hw.get_lifecycle_id()); ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); @@ -2209,6 +2221,7 @@ TEST(TestComponentInterfaces, dummy_system_default_write_error_behavior) state = system_hw.get_lifecycle_state(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, system_hw.get_lifecycle_id()); // activate again and expect reset values state = system_hw.configure(); @@ -2223,6 +2236,7 @@ TEST(TestComponentInterfaces, dummy_system_default_write_error_behavior) state = system_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); + ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, system_hw.get_lifecycle_id()); ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); @@ -2237,11 +2251,13 @@ TEST(TestComponentInterfaces, dummy_system_default_write_error_behavior) state = system_hw.get_lifecycle_state(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); + ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, system_hw.get_lifecycle_id()); // can not change state anymore state = system_hw.configure(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); + ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, system_hw.get_lifecycle_id()); } int main(int argc, char ** argv) diff --git a/hardware_interface_testing/test/test_components/test_actuator_exclusive_interfaces.cpp b/hardware_interface_testing/test/test_components/test_actuator_exclusive_interfaces.cpp index 5b85c96056..53b132adaa 100644 --- a/hardware_interface_testing/test/test_components/test_actuator_exclusive_interfaces.cpp +++ b/hardware_interface_testing/test/test_components/test_actuator_exclusive_interfaces.cpp @@ -25,6 +25,19 @@ using hardware_interface::CommandInterface; using hardware_interface::return_type; using hardware_interface::StateInterface; +namespace +{ +void verify_internal_lifecycle_id(uint8_t expected_id, uint8_t actual_id) +{ + if (expected_id != actual_id) + { + throw std::runtime_error( + "Internal lifecycle ID does not match the expected lifecycle ID. Expected: " + + std::to_string(expected_id) + ", Actual: " + std::to_string(actual_id)); + } +} +} // namespace + static std::pair extract_joint_and_interface( const std::string & full_name) { @@ -50,6 +63,7 @@ class TestActuatorExclusiveInterfaces : public ActuatorInterface { return CallbackReturn::ERROR; } + verify_internal_lifecycle_id(get_lifecycle_id(), get_lifecycle_state().id()); for (const auto & j : get_hardware_info().joints) { @@ -62,6 +76,7 @@ class TestActuatorExclusiveInterfaces : public ActuatorInterface std::vector export_state_interfaces() override { + verify_internal_lifecycle_id(get_lifecycle_id(), get_lifecycle_state().id()); std::vector state_interfaces; for (std::size_t i = 0; i < info_.joints.size(); ++i) { @@ -82,6 +97,7 @@ class TestActuatorExclusiveInterfaces : public ActuatorInterface std::vector export_command_interfaces() override { + verify_internal_lifecycle_id(get_lifecycle_id(), get_lifecycle_state().id()); std::vector command_interfaces; for (std::size_t i = 0; i < info_.joints.size(); ++i) { @@ -104,6 +120,7 @@ class TestActuatorExclusiveInterfaces : public ActuatorInterface const std::vector & start_interfaces, const std::vector & stop_interfaces) override { + verify_internal_lifecycle_id(get_lifecycle_id(), get_lifecycle_state().id()); std::vector claimed_joint_copy = currently_claimed_joints_; for (const auto & interface : stop_interfaces) @@ -150,6 +167,7 @@ class TestActuatorExclusiveInterfaces : public ActuatorInterface const std::vector & start_interfaces, const std::vector & stop_interfaces) override { + verify_internal_lifecycle_id(get_lifecycle_id(), get_lifecycle_state().id()); for (const auto & interface : stop_interfaces) { const auto && [joint_name, interface_name] = extract_joint_and_interface(interface); @@ -170,11 +188,13 @@ class TestActuatorExclusiveInterfaces : public ActuatorInterface return_type read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { + verify_internal_lifecycle_id(get_lifecycle_id(), get_lifecycle_state().id()); return return_type::OK; } return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { + verify_internal_lifecycle_id(get_lifecycle_id(), get_lifecycle_state().id()); return return_type::OK; } diff --git a/hardware_interface_testing/test/test_components/test_system.cpp b/hardware_interface_testing/test/test_components/test_system.cpp index e27d0aebfd..4958add664 100644 --- a/hardware_interface_testing/test/test_components/test_system.cpp +++ b/hardware_interface_testing/test/test_components/test_system.cpp @@ -25,6 +25,19 @@ using hardware_interface::return_type; using hardware_interface::StateInterface; using hardware_interface::SystemInterface; +namespace +{ +void verify_internal_lifecycle_id(uint8_t expected_id, uint8_t actual_id) +{ + if (expected_id != actual_id) + { + throw std::runtime_error( + "Internal lifecycle ID does not match the expected lifecycle ID. Expected: " + + std::to_string(expected_id) + ", Actual: " + std::to_string(actual_id)); + } +} +} // namespace + class TestSystem : public SystemInterface { CallbackReturn on_init( @@ -34,6 +47,7 @@ class TestSystem : public SystemInterface { return CallbackReturn::ERROR; } + verify_internal_lifecycle_id(get_lifecycle_id(), get_lifecycle_state().id()); // Simulating initialization error if (get_hardware_info().joints[0].state_interfaces[1].name == "does_not_exist") @@ -54,6 +68,7 @@ class TestSystem : public SystemInterface std::vector export_state_interfaces() override { + verify_internal_lifecycle_id(get_lifecycle_id(), get_lifecycle_state().id()); const auto info = get_hardware_info(); std::vector state_interfaces; for (auto i = 0u; i < info.joints.size(); ++i) @@ -82,6 +97,7 @@ class TestSystem : public SystemInterface std::vector export_command_interfaces() override { + verify_internal_lifecycle_id(get_lifecycle_id(), get_lifecycle_state().id()); const auto info = get_hardware_info(); std::vector command_interfaces; for (auto i = 0u; i < info.joints.size(); ++i) @@ -109,6 +125,7 @@ class TestSystem : public SystemInterface return_type read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { + verify_internal_lifecycle_id(get_lifecycle_id(), get_lifecycle_state().id()); if (get_hardware_info().is_async) { std::this_thread::sleep_for( @@ -138,6 +155,7 @@ class TestSystem : public SystemInterface return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { + verify_internal_lifecycle_id(get_lifecycle_id(), get_lifecycle_state().id()); if (get_hardware_info().is_async) { std::this_thread::sleep_for( From 7a2d3f929624513e451b2d246ba807b10e03e80b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Sat, 6 Dec 2025 21:39:45 +0100 Subject: [PATCH 2/3] Update release_notes.rst --- doc/release_notes.rst | 4 ---- 1 file changed, 4 deletions(-) diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 98a07658c2..41c68df32d 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -32,11 +32,7 @@ For details see the controller_manager section. * The new semantic command interface ``LedRgbDevice`` provides standard (command) interfaces for 3-channel LED devices (`#1945 `_) * The new ``MagneticFieldSensor`` semantic component provides an interface for reading data from magnetometers. `(#2627 `__) * The controller_manager will now deactivate the entire controller chain if a controller in the chain fails during the update cycle. `(#2681 `__) -<<<<<<< HEAD -======= -* The update rate of the controller will now be approximated to a closer achievable frequency, when its frequency is not achievable with the current controller manager update rate. (`#2828 `__) * The lifecycle ID is cached internally in the controller to avoid calls to get_lifecycle_state() in the real-time control loop. (`#2884 `__) ->>>>>>> dca9b57 (Fix the blocking calls of lifecycle_state in the real-time loop (#2884)) controller_manager ****************** From 117285a971490f43dc8b9e8349a276540b2a6d61 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Sat, 6 Dec 2025 22:15:36 +0100 Subject: [PATCH 3/3] Update release_notes.rst --- doc/release_notes.rst | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 41c68df32d..8df4010518 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -108,7 +108,6 @@ controller_manager hardware_interface ****************** -<<<<<<< HEAD * A portable version for string-to-double conversion was added: ``hardware_interface::stod`` (`#1257 `_) * ``test_components`` was moved to its own package (`#1325 `_) * The ``ros2_control`` tag now supports parsing of the limits from the URDF into the ``HardwareInfo`` structure. More conservative limits can be defined using the ``min`` and ``max`` attributes per interface (`#1472 `_) @@ -193,13 +192,11 @@ hardware_interface * The hardware interface is now treated similarly as ERROR, when a hardware component returns ERROR on the read cycle (`#2334 `_). * The controllers are now deactivated when a hardware component returns DEACTIVATE on the write cycle. The parameter ``deactivate_controllers_on_hardware_self_deactivate`` is added to control this behavior temporarily. It is recommended to set this parameter to true in order to avoid controllers to use inactive hardware components and to avoid any unexpected behavior. This feature parameter will be removed in future releases and will be defaulted to true (`#2334 `_ & `#2501 `_). * The controllers are not allowed to be activated when the hardware component is in INACTIVE state. The parameter ``allow_controller_activation_with_inactive_hardware`` is added to control this behavior temporarily. It is recommended to set this parameter to false in order to avoid controllers to use inactive hardware components and to avoid any unexpected behavior. This feature parameter will be removed in future releases and will be defaulted to false (`#2347 `_). +* The lifecycle ID is cached internally in the controller to avoid calls to get_lifecycle_state() in the real-time control loop. (`#2884 `__) joint_limits ************ * Add header to import limits from standard URDF definition (`#1298 `_) -======= -* The lifecycle ID is cached internally in the controller to avoid calls to get_lifecycle_state() in the real-time control loop. (`#2884 `__) ->>>>>>> dca9b57 (Fix the blocking calls of lifecycle_state in the real-time loop (#2884)) ros2controlcli **************