diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index 94e36d4fc0..e6d35f1d13 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" @@ -200,8 +201,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; @@ -380,6 +396,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 59a2aefdf7..44b65400f6 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,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 @@ -116,19 +107,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); }); @@ -136,6 +129,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); }); @@ -143,6 +137,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; @@ -152,16 +147,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() @@ -253,7 +264,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 +292,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) { @@ -379,7 +410,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 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(); } 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/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 0e38af810d..18d34e2724 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( @@ -3071,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 (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 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 ************** 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/include/hardware_interface/hardware_component_interface.hpp b/hardware_interface/include/hardware_interface/hardware_component_interface.hpp index 79e1a0771e..9a5c3c00d6 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); @@ -678,19 +678,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. @@ -1016,6 +1025,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..b2a33061a3 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())) @@ -321,6 +321,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 @@ -340,14 +342,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 +382,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 a7acd80e2d..a5cff86f12 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,8 +2061,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( @@ -2072,9 +2071,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 { @@ -2168,8 +2166,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( @@ -2179,9 +2176,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(