From df0fb95458fd9772dcc64a4d969625806fef9939 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 2 Dec 2025 23:58:09 +0100 Subject: [PATCH 01/16] Fix hardware components by caching the state id --- .../hardware_component_interface.hpp | 8 ++++-- hardware_interface/src/hardware_component.cpp | 28 +++++++++---------- 2 files changed, 20 insertions(+), 16 deletions(-) diff --git a/hardware_interface/include/hardware_interface/hardware_component_interface.hpp b/hardware_interface/include/hardware_interface/hardware_component_interface.hpp index 79e1a0771e..86ea92e04d 100644 --- a/hardware_interface/include/hardware_interface/hardware_component_interface.hpp +++ b/hardware_interface/include/hardware_interface/hardware_component_interface.hpp @@ -142,8 +142,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); @@ -689,8 +689,11 @@ class HardwareComponentInterface : public rclcpp_lifecycle::node_interfaces::Lif 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. @@ -1016,6 +1019,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 9d7a937e36..90ae939693 100644 --- a/hardware_interface/src/hardware_component.cpp +++ b/hardware_interface/src/hardware_component.cpp @@ -50,7 +50,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)) { @@ -74,7 +74,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())) @@ -102,7 +102,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())) @@ -127,8 +127,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())) @@ -157,7 +157,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(); @@ -186,7 +186,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())) @@ -214,8 +214,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())) @@ -340,14 +340,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) @@ -380,13 +380,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) From 03d93ed17b0ec7024ac410f3dc34ab84273d9565 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 3 Dec 2025 23:26:47 +0100 Subject: [PATCH 02/16] Add fixes for the controller interface base --- .../controller_interface_base.hpp | 5 ++++ .../src/controller_interface_base.cpp | 27 ++++++++++++++----- controller_manager/src/controller_manager.cpp | 2 +- 3 files changed, 27 insertions(+), 7 deletions(-) diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index 94e36d4fc0..65476a0e6b 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -29,8 +29,10 @@ #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" +#include "rclcpp_lifecycle/managed_entity.hpp" namespace controller_interface { @@ -202,6 +204,8 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy const rclcpp_lifecycle::State & get_lifecycle_state() const; + uint8_t get_lifecycle_id() const; + unsigned int get_update_rate() const; bool is_async() const; @@ -380,6 +384,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_; + std::atomic lifecycle_id_ = lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN; protected: pal_statistics::RegistrationsRAII stats_registrations_; diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index 59a2aefdf7..c636a2cc69 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -116,27 +116,33 @@ return_type ControllerInterfaceBase::init( // it in `on_configure` and `on_deactivate` - see the docs for details enable_introspection(false); this->stop_async_handler_thread(); - return on_cleanup(previous_state); + const auto return_value = on_cleanup(previous_state); + lifecycle_id_.store(this->get_lifecycle_state().id(), std::memory_order_release); + return return_value; }); 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(); } - return on_activate(previous_state); + const auto return_value = on_activate(previous_state); + lifecycle_id_.store(this->get_lifecycle_state().id(), std::memory_order_release); + return return_value; }); node_->register_on_deactivate( [this](const rclcpp_lifecycle::State & previous_state) -> CallbackReturn { enable_introspection(false); - return on_deactivate(previous_state); + const auto return_value = on_deactivate(previous_state); + lifecycle_id_.store(this->get_lifecycle_state().id(), std::memory_order_release); + return return_value; }); node_->register_on_shutdown( @@ -144,6 +150,7 @@ return_type ControllerInterfaceBase::init( { this->stop_async_handler_thread(); auto transition_state_status = on_shutdown(previous_state); + lifecycle_id_.store(this->get_lifecycle_state().id(), std::memory_order_release); this->release_interfaces(); return transition_state_status; }); @@ -153,6 +160,7 @@ return_type ControllerInterfaceBase::init( { this->stop_async_handler_thread(); auto transition_state_status = on_error(previous_state); + lifecycle_id_.store(this->get_lifecycle_state().id(), std::memory_order_release); this->release_interfaces(); return transition_state_status; }); @@ -253,7 +261,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( @@ -279,6 +289,11 @@ const rclcpp_lifecycle::State & ControllerInterfaceBase::get_lifecycle_state() c return node_->get_current_state(); } +uint8_t ControllerInterfaceBase::get_lifecycle_id() const +{ + return lifecycle_id_.load(std::memory_order_acquire); +} + ControllerUpdateStatus ControllerInterfaceBase::trigger_update( const rclcpp::Time & time, const rclcpp::Duration & period) { @@ -379,7 +394,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_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 0e38af810d..64ec6e4c20 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -3073,7 +3073,7 @@ controller_interface::return_type ControllerManager::update( } // 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 (loaded_controller.c->get_lifecycle_id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { if ( switch_params_.do_switch && loaded_controller.c->is_async() && From 27f31b06b088f8de106f63564bdc487c3334356e Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 4 Dec 2025 15:21:09 +0100 Subject: [PATCH 03/16] If the lifecycle_id is in transition state update the cache --- .../controller_interface_base.hpp | 2 +- .../src/controller_interface_base.cpp | 14 +++++++++++++- 2 files changed, 14 insertions(+), 2 deletions(-) diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index 65476a0e6b..a88b5f8b5a 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -384,7 +384,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_; - std::atomic lifecycle_id_ = lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN; + mutable std::atomic lifecycle_id_ = lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN; protected: pal_statistics::RegistrationsRAII stats_registrations_; diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index c636a2cc69..7dc1677c91 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -291,7 +291,19 @@ const rclcpp_lifecycle::State & ControllerInterfaceBase::get_lifecycle_state() c uint8_t ControllerInterfaceBase::get_lifecycle_id() const { - return lifecycle_id_.load(std::memory_order_acquire); + 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( From 1927fe961e33cdab6f90ce0f7cac1eaaa1aa3e5a Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 4 Dec 2025 15:22:04 +0100 Subject: [PATCH 04/16] Use get_lifecycle_id of the controllers --- controller_manager/src/controller_manager.cpp | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 64ec6e4c20..42d35f3149 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -65,13 +65,13 @@ static const rmw_qos_profile_t qos_services = { inline bool is_controller_unconfigured( const controller_interface::ControllerInterfaceBase & controller) { - return controller.get_lifecycle_state().id() == + 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() == + return controller.get_lifecycle_id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE; } @@ -83,7 +83,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 +1251,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 +1262,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 +1276,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( From e768215cadf2ceec055d3b28c507330f044be86d Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 4 Dec 2025 15:22:45 +0100 Subject: [PATCH 05/16] Use get_lifecycle_id in the hardware_interface package --- .../hardware_interface/hardware_component.hpp | 2 ++ hardware_interface/src/hardware_component.cpp | 5 ++++ hardware_interface/src/resource_manager.cpp | 24 +++++++++---------- 3 files changed, 19 insertions(+), 12 deletions(-) diff --git a/hardware_interface/include/hardware_interface/hardware_component.hpp b/hardware_interface/include/hardware_interface/hardware_component.hpp index c7b2cd2e98..59a5c09bcc 100644 --- a/hardware_interface/include/hardware_interface/hardware_component.hpp +++ b/hardware_interface/include/hardware_interface/hardware_component.hpp @@ -84,6 +84,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/src/hardware_component.cpp b/hardware_interface/src/hardware_component.cpp index 90ae939693..25c668d347 100644 --- a/hardware_interface/src/hardware_component.cpp +++ b/hardware_interface/src/hardware_component.cpp @@ -321,6 +321,11 @@ 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 diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index a7acd80e2d..d91fbbb6b9 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -583,7 +583,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; @@ -607,7 +607,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); @@ -627,7 +627,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); @@ -651,7 +651,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); @@ -1951,8 +1951,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()); } }; @@ -2061,7 +2061,7 @@ bool ResourceManager::prepare_command_mode_switch( } if ( !start_interfaces_buffer.empty() && - component.get_lifecycle_state().id() == + component.get_lifecycle_id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE && !allow_controller_activation_with_inactive_hardware) { @@ -2072,9 +2072,9 @@ bool ResourceManager::prepare_command_mode_switch( return false; } if ( - component.get_lifecycle_state().id() == + component.get_lifecycle_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_ACTIVE) { try { @@ -2168,7 +2168,7 @@ bool ResourceManager::perform_command_mode_switch( } if ( !start_interfaces_buffer.empty() && - component.get_lifecycle_state().id() == + component.get_lifecycle_id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE && !allow_controller_activation_with_inactive_hardware) { @@ -2179,9 +2179,9 @@ bool ResourceManager::perform_command_mode_switch( return false; } if ( - component.get_lifecycle_state().id() == + component.get_lifecycle_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_ACTIVE) { try { From ffdb9fc1fa1e8bfe6ab7517265e6954a6d1e7f8f Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 4 Dec 2025 17:53:07 +0100 Subject: [PATCH 06/16] pre-commit changes --- .../src/controller_interface_base.cpp | 3 ++- controller_manager/src/controller_manager.cpp | 6 ++---- hardware_interface/src/hardware_component.cpp | 5 +---- hardware_interface/src/resource_manager.cpp | 12 ++++-------- 4 files changed, 9 insertions(+), 17 deletions(-) diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index 7dc1677c91..e8b1f462e9 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -292,7 +292,8 @@ const rclcpp_lifecycle::State & ControllerInterfaceBase::get_lifecycle_state() c 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 || + 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 || diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 42d35f3149..11792c1457 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_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_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( diff --git a/hardware_interface/src/hardware_component.cpp b/hardware_interface/src/hardware_component.cpp index 25c668d347..b2a33061a3 100644 --- a/hardware_interface/src/hardware_component.cpp +++ b/hardware_interface/src/hardware_component.cpp @@ -321,10 +321,7 @@ 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(); -} +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_; } diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index d91fbbb6b9..a5cff86f12 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -2061,8 +2061,7 @@ bool ResourceManager::prepare_command_mode_switch( } if ( !start_interfaces_buffer.empty() && - component.get_lifecycle_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( @@ -2072,8 +2071,7 @@ bool ResourceManager::prepare_command_mode_switch( return false; } if ( - component.get_lifecycle_id() == - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || + component.get_lifecycle_id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || component.get_lifecycle_id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { try @@ -2168,8 +2166,7 @@ bool ResourceManager::perform_command_mode_switch( } if ( !start_interfaces_buffer.empty() && - component.get_lifecycle_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( @@ -2179,8 +2176,7 @@ bool ResourceManager::perform_command_mode_switch( return false; } if ( - component.get_lifecycle_id() == - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || + component.get_lifecycle_id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || component.get_lifecycle_id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { try From bb02bfcec5d3a75322765cc75f04bf0ac3530ea0 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 4 Dec 2025 19:26:01 +0100 Subject: [PATCH 07/16] Add necessary changes to the controller interface class --- .../src/chainable_controller_interface.cpp | 2 +- .../src/controller_interface_base.cpp | 47 +++++++++++-------- .../test/test_controller_interface.cpp | 2 + 3 files changed, 30 insertions(+), 21 deletions(-) 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 e8b1f462e9..0016e416d6 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -62,6 +62,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); if (ctrl_itf_params_.controller_manager_update_rate == 0 && ctrl_itf_params_.update_rate != 0) { @@ -92,22 +93,13 @@ 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); + const auto return_value = on_configure(previous_state); + return return_value; + }); node_->register_on_cleanup( [this](const rclcpp_lifecycle::State & previous_state) -> CallbackReturn @@ -116,8 +108,8 @@ return_type ControllerInterfaceBase::init( // it in `on_configure` and `on_deactivate` - see the docs for details enable_introspection(false); this->stop_async_handler_thread(); - const auto return_value = on_cleanup(previous_state); lifecycle_id_.store(this->get_lifecycle_state().id(), std::memory_order_release); + const auto return_value = on_cleanup(previous_state); return return_value; }); @@ -131,8 +123,8 @@ return_type ControllerInterfaceBase::init( // This is needed if it is disabled due to a thrown exception in the async callback thread async_handler_->reset_variables(); } - const auto return_value = on_activate(previous_state); lifecycle_id_.store(this->get_lifecycle_state().id(), std::memory_order_release); + const auto return_value = on_activate(previous_state); return return_value; }); @@ -140,8 +132,8 @@ return_type ControllerInterfaceBase::init( [this](const rclcpp_lifecycle::State & previous_state) -> CallbackReturn { enable_introspection(false); - const auto return_value = on_deactivate(previous_state); lifecycle_id_.store(this->get_lifecycle_state().id(), std::memory_order_release); + const auto return_value = on_deactivate(previous_state); return return_value; }); @@ -149,8 +141,8 @@ return_type ControllerInterfaceBase::init( [this](const rclcpp_lifecycle::State & previous_state) -> CallbackReturn { this->stop_async_handler_thread(); - auto transition_state_status = on_shutdown(previous_state); 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; }); @@ -159,17 +151,32 @@ return_type ControllerInterfaceBase::init( [this](const rclcpp_lifecycle::State & previous_state) -> CallbackReturn { this->stop_async_handler_thread(); - auto transition_state_status = on_error(previous_state); 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_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; + } + 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() diff --git a/controller_interface/test/test_controller_interface.cpp b/controller_interface/test/test_controller_interface.cpp index 10a6e5f9a2..45e21224fb 100644 --- a/controller_interface/test/test_controller_interface.cpp +++ b/controller_interface/test/test_controller_interface.cpp @@ -233,6 +233,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(); } @@ -255,6 +256,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(); } From 5fe7fa365b92e0d73cd8fa4465ad08218cfa70ff Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 4 Dec 2025 19:26:23 +0100 Subject: [PATCH 08/16] Add tests for the newly added get_lifecycle_id cache method --- .../test_chainable_controller.cpp | 28 ++++++++++++++++++- .../test/test_controller/test_controller.cpp | 21 ++++++++++++++ .../test_controller_manager_urdf_passing.cpp | 4 +++ 3 files changed, 52 insertions(+), 1 deletion(-) 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_; From 7e6951e4e8c3e5e2da2a67b025dc8b1c878e6937 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 4 Dec 2025 19:29:58 +0100 Subject: [PATCH 09/16] Add tests for the hardware components interfaces --- .../test/test_component_interfaces.cpp | 16 +++++++++++++++ .../test_actuator_exclusive_interfaces.cpp | 20 +++++++++++++++++++ .../test/test_components/test_system.cpp | 18 +++++++++++++++++ 3 files changed, 54 insertions(+) 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 f909deb41632fa5d950e4eb391cbc1966c5747ec Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 4 Dec 2025 19:34:48 +0100 Subject: [PATCH 10/16] Update the documentation --- .../controller_interface_base.hpp | 12 ++++++++++++ .../hardware_component_interface.hpp | 5 +++++ 2 files changed, 17 insertions(+) diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index a88b5f8b5a..e4c2b0f28b 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -202,8 +202,20 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy std::shared_ptr get_node() const; + /** + * Get the current lifecycle state of the controller node. + * \note This method 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; diff --git a/hardware_interface/include/hardware_interface/hardware_component_interface.hpp b/hardware_interface/include/hardware_interface/hardware_component_interface.hpp index 86ea92e04d..1072217974 100644 --- a/hardware_interface/include/hardware_interface/hardware_component_interface.hpp +++ b/hardware_interface/include/hardware_interface/hardware_component_interface.hpp @@ -678,12 +678,17 @@ class HardwareComponentInterface : public rclcpp_lifecycle::node_interfaces::Lif /// Get life-cycle state of the hardware. /** + * \note This method 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) From 85925f0951e145bf3fd8af6fa0429d9d655b5d4a Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 4 Dec 2025 19:43:08 +0100 Subject: [PATCH 11/16] update documentation --- controller_manager/doc/userdoc.rst | 3 +++ doc/release_notes.rst | 2 ++ 2 files changed, 5 insertions(+) diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index a2aaf766d9..15967f5cef 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/doc/release_notes.rst b/doc/release_notes.rst index 0216ab4499..bcf93d0389 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -10,6 +10,7 @@ controller_interface * 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 `__) * 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 `__) controller_manager ****************** @@ -20,6 +21,7 @@ controller_manager hardware_interface ****************** +* The lifecycle ID is cached internally in the controller to avoid calls to get_lifecycle_state() in the real-time control loop. (`#2884 `__) ros2controlcli ************** From c7e4f6e501a92aab8fb6b27acc7018efd3e03f5e Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 4 Dec 2025 23:14:17 +0100 Subject: [PATCH 12/16] Update controller_interface/include/controller_interface/controller_interface_base.hpp --- .../include/controller_interface/controller_interface_base.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index e4c2b0f28b..f72ce13447 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -32,7 +32,6 @@ #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/version.h" #include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "rclcpp_lifecycle/managed_entity.hpp" namespace controller_interface { From 9dd147f65a5c3947b9fcedc68595e36745891f9f Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 5 Dec 2025 19:11:42 +0100 Subject: [PATCH 13/16] Update controller_interface/src/controller_interface_base.cpp --- controller_interface/src/controller_interface_base.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index 0016e416d6..6429aa4029 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -163,7 +163,7 @@ return_type ControllerInterfaceBase::init( break; case LifecycleNodeInterface::CallbackReturn::ERROR: case LifecycleNodeInterface::CallbackReturn::FAILURE: - RCLCPP_DEBUG( + RCLCPP_ERROR( get_node()->get_logger(), "Calling shutdown transition of controller node '%s' due to init failure.", get_node()->get_name()); From 530453feaee235bee260b43a95651ae0cd9f1638 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 5 Dec 2025 23:57:31 +0100 Subject: [PATCH 14/16] Apply suggestions from code review MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Christoph Fröhlich --- .../include/controller_interface/controller_interface_base.hpp | 2 +- controller_interface/src/controller_interface_base.cpp | 3 +-- controller_manager/src/controller_manager.cpp | 2 -- 3 files changed, 2 insertions(+), 5 deletions(-) diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index f72ce13447..e9898f4b97 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -203,7 +203,7 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy /** * Get the current lifecycle state of the controller node. - * \note This method is not real-time safe and should not be called in the control loop. + * \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. */ diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index 6429aa4029..a5fe4aaa3e 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -97,8 +97,7 @@ return_type ControllerInterfaceBase::init( [this](const rclcpp_lifecycle::State & previous_state) -> CallbackReturn { lifecycle_id_.store(this->get_lifecycle_state().id(), std::memory_order_release); - const auto return_value = on_configure(previous_state); - return return_value; + return on_configure(previous_state); }); node_->register_on_cleanup( diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 11792c1457..b3229fe4d0 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -3068,8 +3068,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 (loaded_controller.c->get_lifecycle_id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { if ( From dcb995b46af20ee642dd41482235c631ff71d002 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 5 Dec 2025 23:59:00 +0100 Subject: [PATCH 15/16] Update controller_manager/src/controller_manager.cpp MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Christoph Fröhlich --- controller_manager/src/controller_manager.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index b3229fe4d0..18d34e2724 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -3068,7 +3068,7 @@ controller_interface::return_type ControllerManager::update( loaded_controller.info.name.c_str()); continue; } - if (loaded_controller.c->get_lifecycle_id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + if (is_controller_active(*loaded_controller.c)) { if ( switch_params_.do_switch && loaded_controller.c->is_async() && From 122deaeb7517883e9efa88e37205ee663d50edb3 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sat, 6 Dec 2025 00:02:45 +0100 Subject: [PATCH 16/16] address review comments --- .../controller_interface/controller_interface_base.hpp | 3 ++- controller_interface/src/controller_interface_base.cpp | 9 +++------ .../hardware_interface/hardware_component_interface.hpp | 3 ++- 3 files changed, 7 insertions(+), 8 deletions(-) diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index e9898f4b97..e6d35f1d13 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -203,7 +203,8 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy /** * 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 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. */ diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index a5fe4aaa3e..44b65400f6 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -108,8 +108,7 @@ return_type ControllerInterfaceBase::init( enable_introspection(false); this->stop_async_handler_thread(); lifecycle_id_.store(this->get_lifecycle_state().id(), std::memory_order_release); - const auto return_value = on_cleanup(previous_state); - return return_value; + return on_cleanup(previous_state); }); node_->register_on_activate( @@ -123,8 +122,7 @@ return_type ControllerInterfaceBase::init( async_handler_->reset_variables(); } lifecycle_id_.store(this->get_lifecycle_state().id(), std::memory_order_release); - const auto return_value = on_activate(previous_state); - return return_value; + return on_activate(previous_state); }); node_->register_on_deactivate( @@ -132,8 +130,7 @@ return_type ControllerInterfaceBase::init( { enable_introspection(false); lifecycle_id_.store(this->get_lifecycle_state().id(), std::memory_order_release); - const auto return_value = on_deactivate(previous_state); - return return_value; + return on_deactivate(previous_state); }); node_->register_on_shutdown( diff --git a/hardware_interface/include/hardware_interface/hardware_component_interface.hpp b/hardware_interface/include/hardware_interface/hardware_component_interface.hpp index 1072217974..9a5c3c00d6 100644 --- a/hardware_interface/include/hardware_interface/hardware_component_interface.hpp +++ b/hardware_interface/include/hardware_interface/hardware_component_interface.hpp @@ -678,7 +678,8 @@ class HardwareComponentInterface : public rclcpp_lifecycle::node_interfaces::Lif /// Get life-cycle state of the hardware. /** - * \note This method is not real-time safe and should not be called in the control loop. + * \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. */