From 7a5779c43d73b4be6ca5344d62a75b89b3ca76c7 Mon Sep 17 00:00:00 2001 From: Manuel Muth Date: Mon, 26 Aug 2024 14:09:18 +0200 Subject: [PATCH] Rename `get_state` and `set_state` Functions to `get/set_lifecylce_state` (variant support) (#1683) --- .../controller_interface/async_controller.hpp | 3 +- .../controller_interface_base.hpp | 2 +- .../src/chainable_controller_interface.cpp | 5 +- .../src/controller_interface_base.cpp | 4 +- controller_manager/src/controller_manager.cpp | 13 +- .../test_chainable_controller.cpp | 8 +- .../test/test_controller/test_controller.cpp | 8 +- .../test/test_controller_manager.cpp | 47 ++- ...roller_manager_hardware_error_handling.cpp | 137 ++++--- .../test/test_controller_manager_srvs.cpp | 30 +- .../test_controller_manager_urdf_passing.cpp | 13 +- ...llers_chaining_with_controller_manager.cpp | 355 +++++++++++------- .../test/test_load_controller.cpp | 98 +++-- .../test/test_release_interfaces.cpp | 32 +- .../test/test_spawner_unspawner.cpp | 65 ++-- doc/release_notes.rst | 5 +- .../include/hardware_interface/actuator.hpp | 2 +- .../hardware_interface/actuator_interface.hpp | 7 +- .../hardware_interface/async_components.hpp | 4 +- .../include/hardware_interface/sensor.hpp | 2 +- .../hardware_interface/sensor_interface.hpp | 7 +- .../include/hardware_interface/system.hpp | 2 +- .../hardware_interface/system_interface.hpp | 7 +- hardware_interface/src/actuator.cpp | 97 ++--- hardware_interface/src/resource_manager.cpp | 21 +- hardware_interface/src/sensor.cpp | 89 ++--- hardware_interface/src/system.cpp | 97 ++--- .../test/test_component_interfaces.cpp | 20 +- 28 files changed, 700 insertions(+), 480 deletions(-) diff --git a/controller_interface/include/controller_interface/async_controller.hpp b/controller_interface/include/controller_interface/async_controller.hpp index 5601ff4703..357b3a2ce3 100644 --- a/controller_interface/include/controller_interface/async_controller.hpp +++ b/controller_interface/include/controller_interface/async_controller.hpp @@ -82,7 +82,8 @@ class AsyncControllerThread TimePoint next_iteration_time = TimePoint(std::chrono::nanoseconds(controller_->get_node()->now().nanoseconds())); - if (controller_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + if ( + controller_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { auto const current_time = controller_->get_node()->now(); auto const measured_period = current_time - previous_time; diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index 9eaee9f15b..fbb2111cc9 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -147,7 +147,7 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy std::shared_ptr get_node() const; CONTROLLER_INTERFACE_PUBLIC - const rclcpp_lifecycle::State & get_state() const; + const rclcpp_lifecycle::State & get_lifecycle_state() const; CONTROLLER_INTERFACE_PUBLIC unsigned int get_update_rate() const; diff --git a/controller_interface/src/chainable_controller_interface.cpp b/controller_interface/src/chainable_controller_interface.cpp index 9f4a171ec3..ae5cd326b6 100644 --- a/controller_interface/src/chainable_controller_interface.cpp +++ b/controller_interface/src/chainable_controller_interface.cpp @@ -97,7 +97,7 @@ bool ChainableControllerInterface::set_chained_mode(bool chained_mode) { bool result = false; - if (get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + if (get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { result = on_set_chained_mode(chained_mode); @@ -112,7 +112,8 @@ bool ChainableControllerInterface::set_chained_mode(bool chained_mode) get_node()->get_logger(), "Can not change controller's chained mode because it is no in '%s' state. " "Current state is '%s'.", - hardware_interface::lifecycle_state_names::UNCONFIGURED, get_state().label().c_str()); + hardware_interface::lifecycle_state_names::UNCONFIGURED, + get_lifecycle_state().label().c_str()); } return result; diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index db9fb99d81..61d2c780b8 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -83,7 +83,7 @@ const rclcpp_lifecycle::State & ControllerInterfaceBase::configure() // Then we don't need to do state-machine related checks. // // Other solution is to add check into the LifecycleNode if a transition is valid to trigger - if (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) + if (get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) { update_rate_ = static_cast(get_node()->get_parameter("update_rate").as_int()); is_async_ = get_node()->get_parameter("is_async").as_bool(); @@ -106,7 +106,7 @@ void ControllerInterfaceBase::release_interfaces() state_interfaces_.clear(); } -const rclcpp_lifecycle::State & ControllerInterfaceBase::get_state() const +const rclcpp_lifecycle::State & ControllerInterfaceBase::get_lifecycle_state() const { return node_->get_current_state(); } diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 70b9c8d628..275f12f542 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -58,7 +58,8 @@ static const rmw_qos_profile_t qos_services = { inline bool is_controller_inactive(const controller_interface::ControllerInterfaceBase & controller) { - return controller.get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE; + return controller.get_lifecycle_state().id() == + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE; } inline bool is_controller_inactive( @@ -69,7 +70,7 @@ inline bool is_controller_inactive( inline bool is_controller_active(const controller_interface::ControllerInterfaceBase & controller) { - return controller.get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE; + return controller.get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE; } inline bool is_controller_active( @@ -659,7 +660,7 @@ controller_interface::return_type ControllerManager::configure_controller( } auto controller = found_it->c; - auto state = controller->get_state(); + 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) @@ -670,7 +671,7 @@ controller_interface::return_type ControllerManager::configure_controller( return controller_interface::return_type::ERROR; } - auto new_state = controller->get_state(); + auto new_state = controller->get_lifecycle_state(); if (state.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { RCLCPP_DEBUG( @@ -1744,7 +1745,7 @@ void ControllerManager::list_controllers_srv_cb( controller_state.name = controllers[i].info.name; controller_state.type = controllers[i].info.type; controller_state.claimed_interfaces = controllers[i].info.claimed_interfaces; - controller_state.state = controllers[i].c->get_state().label(); + controller_state.state = controllers[i].c->get_lifecycle_state().label(); controller_state.is_chainable = controllers[i].c->is_chainable(); controller_state.is_chained = controllers[i].c->is_in_chained_mode(); @@ -2728,7 +2729,7 @@ void ControllerManager::controller_activity_diagnostic_callback( { all_active = false; } - stat.add(controllers[i].info.name, controllers[i].c->get_state().label()); + stat.add(controllers[i].info.name, controllers[i].c->get_lifecycle_state().label()); } if (all_active) 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 e43f2a13a1..e68af6345f 100644 --- a/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp +++ b/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp @@ -33,8 +33,8 @@ controller_interface::InterfaceConfiguration TestChainableController::command_interface_configuration() const { if ( - get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || - get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || + get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { return cmd_iface_cfg_; } @@ -49,8 +49,8 @@ controller_interface::InterfaceConfiguration TestChainableController::state_interface_configuration() const { if ( - get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || - get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || + get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { auto state_iface_cfg = state_iface_cfg_; if (imu_sensor_) diff --git a/controller_manager/test/test_controller/test_controller.cpp b/controller_manager/test/test_controller/test_controller.cpp index 7e3419fbc9..7b9af6ecfc 100644 --- a/controller_manager/test/test_controller/test_controller.cpp +++ b/controller_manager/test/test_controller/test_controller.cpp @@ -31,8 +31,8 @@ TestController::TestController() controller_interface::InterfaceConfiguration TestController::command_interface_configuration() const { if ( - get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || - get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || + get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { return cmd_iface_cfg_; } @@ -46,8 +46,8 @@ controller_interface::InterfaceConfiguration TestController::command_interface_c controller_interface::InterfaceConfiguration TestController::state_interface_configuration() const { if ( - get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || - get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || + get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { return state_iface_cfg_; } diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index 5093250dfe..28e9f0477c 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -127,7 +127,8 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle) << "Update should not reach an unconfigured controller"; EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller->get_lifecycle_state().id()); // configure controller { @@ -141,7 +142,9 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle) EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is not started"; EXPECT_EQ(0u, test_controller2->internal_counter) << "Controller is not started"; - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller->get_lifecycle_state().id()); // Start controller, will take effect at the end of the update function std::vector start_controllers = {"fake_controller", TEST_CONTROLLER2_NAME}; @@ -182,7 +185,8 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle) ControllerManagerRunner cm_runner(this); EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); } - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_lifecycle_state().id()); EXPECT_EQ( controller_interface::return_type::OK, @@ -210,7 +214,9 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle) EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); } - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller->get_lifecycle_state().id()); auto unload_future = std::async( std::launch::async, &controller_manager::ControllerManager::unload_controller, cm_, test_controller::TEST_CONTROLLER_NAME); @@ -221,7 +227,8 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle) EXPECT_EQ(controller_interface::return_type::OK, unload_future.get()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller->get_lifecycle_state().id()); EXPECT_EQ(1, test_controller.use_count()); } @@ -242,7 +249,8 @@ TEST_P(TestControllerManagerWithStrictness, per_controller_update_rate) << "Update should not reach an unconfigured controller"; EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller->get_lifecycle_state().id()); test_controller->get_node()->set_parameter({"update_rate", 4}); // configure controller @@ -255,7 +263,9 @@ TEST_P(TestControllerManagerWithStrictness, per_controller_update_rate) cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is not started"; - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller->get_lifecycle_state().id()); // Start controller, will take effect at the end of the update function std::vector start_controllers = {test_controller::TEST_CONTROLLER_NAME}; @@ -276,7 +286,8 @@ TEST_P(TestControllerManagerWithStrictness, per_controller_update_rate) EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); } - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_lifecycle_state().id()); // As the controller frequency is 4Hz, it needs to pass 25 iterations for 1 update cycle for (size_t i = 0; i < 25; i++) @@ -323,7 +334,8 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd << "Update should not reach an unconfigured controller"; EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller->get_lifecycle_state().id()); rclcpp::Parameter update_rate_parameter("update_rate", static_cast(ctrl_update_rate)); test_controller->get_node()->set_parameter(update_rate_parameter); @@ -337,7 +349,9 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); EXPECT_EQ(last_internal_counter, test_controller->internal_counter) << "Controller is not started"; - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller->get_lifecycle_state().id()); // Start controller, will take effect at the end of the update function std::vector start_controllers = {test_controller::TEST_CONTROLLER_NAME}; @@ -360,7 +374,8 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); } - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_lifecycle_state().id()); const auto pre_internal_counter = test_controller->internal_counter; rclcpp::Rate loop_rate(cm_->get_update_rate()); @@ -430,7 +445,8 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate) EXPECT_EQ(2, test_controller.use_count()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller->get_lifecycle_state().id()); test_controller->get_node()->set_parameter({"update_rate", static_cast(ctrl_update_rate)}); // configure controller @@ -443,7 +459,9 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate) cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is not started"; - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller->get_lifecycle_state().id()); // Start controller, will take effect at the end of the update function time_ = test_controller->get_node()->now(); // set to something nonzero @@ -467,7 +485,8 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate) EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); } - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_lifecycle_state().id()); EXPECT_EQ(test_controller->get_update_rate(), ctrl_update_rate); const auto cm_update_rate = cm_->get_update_rate(); diff --git a/controller_manager/test/test_controller_manager_hardware_error_handling.cpp b/controller_manager/test/test_controller_manager_hardware_error_handling.cpp index 6e2fba23db..936a2dd4c4 100644 --- a/controller_manager/test/test_controller_manager_hardware_error_handling.cpp +++ b/controller_manager/test/test_controller_manager_hardware_error_handling.cpp @@ -122,16 +122,16 @@ class TestControllerManagerWithTestableCM // check if all controllers are added correctly EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, - test_controller_actuator->get_state().id()); + test_controller_actuator->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, - test_controller_system->get_state().id()); + test_controller_system->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, - test_broadcaster_all->get_state().id()); + test_broadcaster_all->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, - test_broadcaster_sensor->get_state().id()); + test_broadcaster_sensor->get_lifecycle_state().id()); // configure controllers cm_->configure_controller(TEST_CONTROLLER_ACTUATOR_NAME); @@ -147,14 +147,16 @@ class TestControllerManagerWithTestableCM EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - test_controller_actuator->get_state().id()); + test_controller_actuator->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller_system->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_system->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_broadcaster_all->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_broadcaster_all->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - test_broadcaster_sensor->get_state().id()); + test_broadcaster_sensor->get_lifecycle_state().id()); // Start controller, will take effect at the end of the update function switch_test_controllers( @@ -180,13 +182,17 @@ TEST_P(TestControllerManagerWithTestableCM, check_cached_controllers_for_hardwar SetupAndConfigureControllers(strictness); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller_actuator->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_actuator->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller_system->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_system->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_all->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_broadcaster_all->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_sensor->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_broadcaster_sensor->get_lifecycle_state().id()); { auto controllers = @@ -235,13 +241,17 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_read_er } EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller_actuator->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_actuator->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller_system->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_system->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_all->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_broadcaster_all->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_sensor->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_broadcaster_sensor->get_lifecycle_state().id()); // Execute first time without any errors { @@ -278,13 +288,16 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_read_er EXPECT_NO_THROW(cm_->read(time_, PERIOD)); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - test_controller_actuator->get_state().id()); + test_controller_actuator->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller_system->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_system->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_broadcaster_all->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_broadcaster_all->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_sensor->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_broadcaster_sensor->get_lifecycle_state().id()); EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter) @@ -350,13 +363,16 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_read_er EXPECT_NO_THROW(cm_->read(time_, PERIOD)); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - test_controller_actuator->get_state().id()); + test_controller_actuator->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller_system->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_system->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_broadcaster_all->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_broadcaster_all->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_sensor->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_broadcaster_sensor->get_lifecycle_state().id()); EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter_lower) @@ -427,13 +443,17 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_controller_error } EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller_actuator->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_actuator->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller_system->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_system->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_all->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_broadcaster_all->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_sensor->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_broadcaster_sensor->get_lifecycle_state().id()); // Execute first time without any errors { @@ -456,12 +476,13 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_controller_error EXPECT_EQ(test_controller_actuator->internal_counter, new_counter) << "Executes the current cycle and returns ERROR"; EXPECT_EQ( - test_controller_actuator->get_state().id(), + test_controller_actuator->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); EXPECT_EQ(test_controller_system->internal_counter, new_counter) << "Executes the current cycle and returns ERROR"; EXPECT_EQ( - test_controller_system->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + test_controller_system->get_lifecycle_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); EXPECT_EQ(test_broadcaster_all->internal_counter, new_counter) << "Execute without errors to write value"; EXPECT_EQ(test_broadcaster_sensor->internal_counter, new_counter) @@ -474,13 +495,16 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_controller_error EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - test_controller_actuator->get_state().id()); + test_controller_actuator->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller_system->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_system->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_all->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_broadcaster_all->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_sensor->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_broadcaster_sensor->get_lifecycle_state().id()); EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter) @@ -495,13 +519,16 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_controller_error // The states shouldn't change as there are no more controller errors EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - test_controller_actuator->get_state().id()); + test_controller_actuator->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller_system->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_system->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_all->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_broadcaster_all->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_sensor->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_broadcaster_sensor->get_lifecycle_state().id()); } } @@ -527,13 +554,17 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_write_e } EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller_actuator->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_actuator->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller_system->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_system->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_all->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_broadcaster_all->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_sensor->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_broadcaster_sensor->get_lifecycle_state().id()); // Execute first time without any errors { @@ -570,13 +601,16 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_write_e EXPECT_NO_THROW(cm_->write(time_, PERIOD)); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - test_controller_actuator->get_state().id()); + test_controller_actuator->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller_system->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_system->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_broadcaster_all->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_broadcaster_all->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_sensor->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_broadcaster_sensor->get_lifecycle_state().id()); EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter) @@ -644,13 +678,16 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_write_e EXPECT_NO_THROW(cm_->write(time_, PERIOD)); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - test_controller_actuator->get_state().id()); + test_controller_actuator->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller_system->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_system->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_broadcaster_all->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_broadcaster_all->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_sensor->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_broadcaster_sensor->get_lifecycle_state().id()); EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter_lower) diff --git a/controller_manager/test/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp index e53ad10578..26a5299a07 100644 --- a/controller_manager/test/test_controller_manager_srvs.cpp +++ b/controller_manager/test/test_controller_manager_srvs.cpp @@ -365,7 +365,8 @@ TEST_F(TestControllerManagerSrvs, reload_controller_libraries_srv) std::weak_ptr test_controller_weak(test_controller); ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller->get_lifecycle_state().id()); ASSERT_GT(test_controller.use_count(), 1) << "Controller manager should have have a copy of this shared ptr"; @@ -388,7 +389,9 @@ TEST_F(TestControllerManagerSrvs, reload_controller_libraries_srv) test_controller_weak = test_controller; cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller->get_lifecycle_state().id()); ASSERT_GT(test_controller.use_count(), 1) << "Controller manager should have have a copy of this shared ptr"; @@ -412,13 +415,15 @@ TEST_F(TestControllerManagerSrvs, reload_controller_libraries_srv) cm_->switch_controller( {test_controller::TEST_CONTROLLER_NAME}, {}, controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0)); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_lifecycle_state().id()); // Failed reload due to active controller request->force_kill = false; result = call_service_and_wait(*client, request, srv_executor); ASSERT_FALSE(result->ok) << "Cannot reload if controllers are running"; - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_lifecycle_state().id()); ASSERT_GT(test_controller.use_count(), 1) << "Controller manager should still have have a copy of " "this shared ptr, no unloading was performed"; @@ -460,7 +465,7 @@ TEST_F(TestControllerManagerSrvs, load_controller_srv) EXPECT_EQ(1u, cm_->get_loaded_controllers().size()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, - cm_->get_loaded_controllers()[0].c->get_state().id()); + cm_->get_loaded_controllers()[0].c->get_lifecycle_state().id()); } TEST_F(TestControllerManagerSrvs, unload_controller_srv) @@ -486,7 +491,8 @@ TEST_F(TestControllerManagerSrvs, unload_controller_srv) result = call_service_and_wait(*client, request, srv_executor, true); ASSERT_TRUE(result->ok); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller->get_lifecycle_state().id()); EXPECT_EQ(0u, cm_->get_loaded_controllers().size()); } @@ -520,7 +526,8 @@ TEST_F(TestControllerManagerSrvs, robot_description_on_load_and_unload_controlle unload_request->name = test_controller::TEST_CONTROLLER_NAME; auto result = call_service_and_wait(*unload_client, unload_request, srv_executor, true); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller->get_lifecycle_state().id()); EXPECT_EQ(0u, cm_->get_loaded_controllers().size()); // now load it and check if it got the new robot description @@ -561,15 +568,18 @@ TEST_F(TestControllerManagerSrvs, configure_controller_srv) EXPECT_EQ(1u, cm_->get_loaded_controllers().size()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - cm_->get_loaded_controllers()[0].c->get_state().id()); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_state().id()); + cm_->get_loaded_controllers()[0].c->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller->get_lifecycle_state().id()); // now unload the controller and check the state auto unload_request = std::make_shared(); unload_request->name = test_controller::TEST_CONTROLLER_NAME; ASSERT_TRUE(call_service_and_wait(*unload_client, unload_request, srv_executor, true)->ok); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller->get_lifecycle_state().id()); EXPECT_EQ(0u, cm_->get_loaded_controllers().size()); } diff --git a/controller_manager/test/test_controller_manager_urdf_passing.cpp b/controller_manager/test/test_controller_manager_urdf_passing.cpp index 1556211a7f..e44db4838b 100644 --- a/controller_manager/test/test_controller_manager_urdf_passing.cpp +++ b/controller_manager/test/test_controller_manager_urdf_passing.cpp @@ -68,7 +68,8 @@ class TestControllerManagerWithTestableCM cm_->add_controller(test_controller_, CONTROLLER_NAME, TEST_CONTROLLER_CLASS_NAME); ASSERT_NE(test_controller_, nullptr); ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller_->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_->get_lifecycle_state().id()); } void configure_and_try_switch_that_returns_error() @@ -76,7 +77,8 @@ class TestControllerManagerWithTestableCM // configure controller cm_->configure_controller(CONTROLLER_NAME); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller_->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + 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); @@ -86,7 +88,8 @@ class TestControllerManagerWithTestableCM controller_interface::return_type::ERROR); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller_->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_->get_lifecycle_state().id()); } void try_successful_switch() @@ -95,7 +98,9 @@ class TestControllerManagerWithTestableCM strvec{CONTROLLER_NAME}, strvec{}, STRICT, std::future_status::timeout, controller_interface::return_type::OK); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller_->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_->get_lifecycle_state().id()); } std::shared_ptr test_controller_; diff --git a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp index 4a81f3bf12..c143ab4862 100644 --- a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp +++ b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp @@ -220,31 +220,31 @@ class TestControllerChainingWithControllerManager EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, - pid_left_wheel_controller->get_state().id()); + pid_left_wheel_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, - pid_right_wheel_controller->get_state().id()); + pid_right_wheel_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, - diff_drive_controller->get_state().id()); + diff_drive_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, - diff_drive_controller_two->get_state().id()); + diff_drive_controller_two->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, - position_tracking_controller->get_state().id()); + position_tracking_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, - sensor_fusion_controller->get_state().id()); + sensor_fusion_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, - robot_localization_controller->get_state().id()); + robot_localization_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, - odom_publisher_controller->get_state().id()); + odom_publisher_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, - position_tracking_controller_two->get_state().id()); + position_tracking_controller_two->get_lifecycle_state().id()); } // order or controller configuration is not important therefore we can reuse the same method @@ -257,7 +257,7 @@ class TestControllerChainingWithControllerManager // configure chainable controller and check exported interfaces cm_->configure_controller(PID_LEFT_WHEEL); EXPECT_EQ( - pid_left_wheel_controller->get_state().id(), + pid_left_wheel_controller->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); EXPECT_EQ(cm_->resource_manager_->command_interface_keys().size(), number_of_cmd_itfs + 1); EXPECT_EQ(cm_->resource_manager_->state_interface_keys().size(), number_of_state_itfs); @@ -270,7 +270,7 @@ class TestControllerChainingWithControllerManager cm_->configure_controller(PID_RIGHT_WHEEL); EXPECT_EQ( - pid_right_wheel_controller->get_state().id(), + pid_right_wheel_controller->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); EXPECT_EQ(cm_->resource_manager_->command_interface_keys().size(), number_of_cmd_itfs + 2); EXPECT_EQ(cm_->resource_manager_->state_interface_keys().size(), number_of_state_itfs); @@ -287,7 +287,8 @@ class TestControllerChainingWithControllerManager RCLCPP_ERROR_STREAM(cm_->get_logger(), x); } EXPECT_EQ( - diff_drive_controller->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + diff_drive_controller->get_lifecycle_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); EXPECT_EQ(cm_->resource_manager_->command_interface_keys().size(), number_of_cmd_itfs + 5); EXPECT_EQ(cm_->resource_manager_->state_interface_keys().size(), number_of_state_itfs + 2); for (const auto & interface : @@ -306,7 +307,7 @@ class TestControllerChainingWithControllerManager cm_->configure_controller(DIFF_DRIVE_CONTROLLER_TWO); EXPECT_EQ( - diff_drive_controller_two->get_state().id(), + diff_drive_controller_two->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); EXPECT_EQ(cm_->resource_manager_->command_interface_keys().size(), number_of_cmd_itfs + 8); EXPECT_EQ(cm_->resource_manager_->state_interface_keys().size(), number_of_state_itfs + 4); @@ -326,35 +327,35 @@ class TestControllerChainingWithControllerManager cm_->configure_controller(POSITION_TRACKING_CONTROLLER); EXPECT_EQ( - position_tracking_controller->get_state().id(), + position_tracking_controller->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); EXPECT_EQ(cm_->resource_manager_->command_interface_keys().size(), number_of_cmd_itfs + 8); EXPECT_EQ(cm_->resource_manager_->state_interface_keys().size(), number_of_state_itfs + 4); cm_->configure_controller(SENSOR_FUSION_CONTROLLER); EXPECT_EQ( - position_tracking_controller->get_state().id(), + position_tracking_controller->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); EXPECT_EQ(cm_->resource_manager_->command_interface_keys().size(), number_of_cmd_itfs + 8); EXPECT_EQ(cm_->resource_manager_->state_interface_keys().size(), number_of_state_itfs + 7); cm_->configure_controller(ROBOT_LOCALIZATION_CONTROLLER); EXPECT_EQ( - position_tracking_controller->get_state().id(), + position_tracking_controller->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); EXPECT_EQ(cm_->resource_manager_->command_interface_keys().size(), number_of_cmd_itfs + 8); EXPECT_EQ(cm_->resource_manager_->state_interface_keys().size(), number_of_state_itfs + 8); cm_->configure_controller(ODOM_PUBLISHER_CONTROLLER); EXPECT_EQ( - position_tracking_controller->get_state().id(), + position_tracking_controller->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); EXPECT_EQ(cm_->resource_manager_->command_interface_keys().size(), number_of_cmd_itfs + 8); EXPECT_EQ(cm_->resource_manager_->state_interface_keys().size(), number_of_state_itfs + 8); cm_->configure_controller(POSITION_TRACKING_CONTROLLER_TWO); EXPECT_EQ( - position_tracking_controller_two->get_state().id(), + position_tracking_controller_two->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); EXPECT_EQ(cm_->resource_manager_->command_interface_keys().size(), number_of_cmd_itfs + 8); EXPECT_EQ(cm_->resource_manager_->state_interface_keys().size(), number_of_state_itfs + 8); @@ -963,17 +964,17 @@ TEST_P( EXPECT_TRUE(pid_right_wheel_controller->is_in_chained_mode()); ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + diff_drive_controller->get_lifecycle_state().id()); // SensorFusionController continues to stay in the chained mode as it is still using the state // interfaces ASSERT_FALSE(sensor_fusion_controller->is_in_chained_mode()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, sensor_fusion_controller->get_state().id()); - EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, odom_publisher_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + odom_publisher_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - robot_localization_controller->get_state().id()); + robot_localization_controller->get_lifecycle_state().id()); // Deactivate the robot localization controller and see that the sensor fusion controller is still // active but not in the chained mode @@ -984,14 +985,17 @@ TEST_P( ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); ASSERT_FALSE(sensor_fusion_controller->is_in_chained_mode()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + diff_drive_controller->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, sensor_fusion_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, odom_publisher_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + odom_publisher_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - robot_localization_controller->get_state().id()); + robot_localization_controller->get_lifecycle_state().id()); // Deactivate the odometry publisher controller DeactivateAndCheckController(odom_publisher_controller, ODOM_PUBLISHER_CONTROLLER, {}, 8u, true); @@ -1000,15 +1004,17 @@ TEST_P( ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); ASSERT_FALSE(sensor_fusion_controller->is_in_chained_mode()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + diff_drive_controller->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, sensor_fusion_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - odom_publisher_controller->get_state().id()); + odom_publisher_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - robot_localization_controller->get_state().id()); + robot_localization_controller->get_lifecycle_state().id()); // Deactivate the sensor_fusion controller and see that the diff_drive_controller is still active // but not in the chained mode @@ -1018,15 +1024,17 @@ TEST_P( ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); ASSERT_FALSE(sensor_fusion_controller->is_in_chained_mode()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + diff_drive_controller->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, sensor_fusion_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - odom_publisher_controller->get_state().id()); + odom_publisher_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - robot_localization_controller->get_state().id()); + robot_localization_controller->get_lifecycle_state().id()); // Deactivate the diff_drive_controller as all it's following controllers that uses it's // interfaces are deactivated @@ -1037,12 +1045,14 @@ TEST_P( ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); ASSERT_FALSE(sensor_fusion_controller->is_in_chained_mode()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, diff_drive_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + diff_drive_controller->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, sensor_fusion_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - odom_publisher_controller->get_state().id()); + odom_publisher_controller->get_lifecycle_state().id()); // all controllers are deactivated --> chained mode is not changed DeactivateAndCheckController( @@ -1054,15 +1064,17 @@ TEST_P( ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); ASSERT_FALSE(sensor_fusion_controller->is_in_chained_mode()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, diff_drive_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + diff_drive_controller->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, sensor_fusion_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - odom_publisher_controller->get_state().id()); + odom_publisher_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - robot_localization_controller->get_state().id()); + robot_localization_controller->get_lifecycle_state().id()); } TEST_P( @@ -1131,18 +1143,22 @@ TEST_P( // Check if the controller activated (Should not be activated) ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, diff_drive_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + diff_drive_controller->get_lifecycle_state().id()); ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, sensor_fusion_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); ActivateController( SENSOR_FUSION_CONTROLLER, expected.at(test_param.strictness).return_type, std::future_status::ready); // Check if the controller activated (Should not be activated) ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, diff_drive_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + diff_drive_controller->get_lifecycle_state().id()); ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, sensor_fusion_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); // Test Case 2: Try to activate a preceding controller the same time when trying to // deactivate a following controller (using switch_controller function) @@ -1167,16 +1183,18 @@ TEST_P( // should be deactivated (if BEST_EFFORT) // If STRICT, preceding controller should stay deactivated and following controller // should stay activated - EXPECT_EQ(expected.at(test_param.strictness).state, pid_right_wheel_controller->get_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, pid_left_wheel_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + pid_left_wheel_controller->get_lifecycle_state().id()); ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, diff_drive_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + diff_drive_controller->get_lifecycle_state().id()); ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, sensor_fusion_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - odom_publisher_controller->get_state().id()); + odom_publisher_controller->get_lifecycle_state().id()); } TEST_P( @@ -1243,20 +1261,22 @@ TEST_P( // depending controllers are active EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - diff_drive_controller_two->get_state().id()); + diff_drive_controller_two->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, sensor_fusion_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - robot_localization_controller->get_state().id()); + robot_localization_controller->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, odom_publisher_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + odom_publisher_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - position_tracking_controller->get_state().id()); + position_tracking_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - position_tracking_controller_two->get_state().id()); + position_tracking_controller_two->get_lifecycle_state().id()); // Deactivate position_tracking_controller and activate position_tracking_controller_two switch_test_controllers( @@ -1264,10 +1284,10 @@ TEST_P( std::future_status::timeout, controller_interface::return_type::OK); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - position_tracking_controller->get_state().id()); + position_tracking_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - position_tracking_controller_two->get_state().id()); + position_tracking_controller_two->get_lifecycle_state().id()); // Now deactivate the position_tracking_controller_two and it should be in inactive state switch_test_controllers( @@ -1275,7 +1295,7 @@ TEST_P( controller_interface::return_type::OK); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - position_tracking_controller_two->get_state().id()); + position_tracking_controller_two->get_lifecycle_state().id()); // Activate it again and deactivate it others to see if we can deactivate it in a group switch_test_controllers( @@ -1283,10 +1303,10 @@ TEST_P( controller_interface::return_type::OK); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - position_tracking_controller->get_state().id()); + position_tracking_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - position_tracking_controller_two->get_state().id()); + position_tracking_controller_two->get_lifecycle_state().id()); // Deactivate the first preceding controller (diff_drive_controller) and // activate the other preceding controller (diff_drive_controller_two) @@ -1298,29 +1318,34 @@ TEST_P( // Following controllers should stay active EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, pid_left_wheel_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + pid_left_wheel_controller->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, pid_right_wheel_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + pid_right_wheel_controller->get_lifecycle_state().id()); // The original preceding controller (diff_drive_controller) should be inactive while // the other preceding controller should be active (diff_drive_controller_two) ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, diff_drive_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + diff_drive_controller->get_lifecycle_state().id()); ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller_two->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + diff_drive_controller_two->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, sensor_fusion_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - robot_localization_controller->get_state().id()); + robot_localization_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - odom_publisher_controller->get_state().id()); + odom_publisher_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - position_tracking_controller->get_state().id()); + position_tracking_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - position_tracking_controller_two->get_state().id()); + position_tracking_controller_two->get_lifecycle_state().id()); // Activate all the controllers again in group and deactivate the diff_drive_controller_two switch_test_controllers( @@ -1330,32 +1355,37 @@ TEST_P( controller_interface::return_type::OK); // Following controllers should stay active EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, pid_left_wheel_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + pid_left_wheel_controller->get_lifecycle_state().id()); EXPECT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, pid_right_wheel_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + pid_right_wheel_controller->get_lifecycle_state().id()); EXPECT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + diff_drive_controller->get_lifecycle_state().id()); EXPECT_TRUE(diff_drive_controller->is_in_chained_mode()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - diff_drive_controller_two->get_state().id()); + diff_drive_controller_two->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, sensor_fusion_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); // This is false, because it only uses the state interfaces and exposes state interfaces EXPECT_FALSE(sensor_fusion_controller->is_in_chained_mode()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - robot_localization_controller->get_state().id()); + robot_localization_controller->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, odom_publisher_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + odom_publisher_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - position_tracking_controller->get_state().id()); + position_tracking_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - position_tracking_controller_two->get_state().id()); + position_tracking_controller_two->get_lifecycle_state().id()); } TEST_P( @@ -1426,15 +1456,17 @@ TEST_P( // Verify preceding controller (diff_drive_controller) is inactive EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, diff_drive_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + diff_drive_controller->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, sensor_fusion_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - odom_publisher_controller->get_state().id()); + odom_publisher_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - robot_localization_controller->get_state().id()); + robot_localization_controller->get_lifecycle_state().id()); // Attempt to deactivate inactive controller (diff_drive_controller) DeactivateController( @@ -1453,19 +1485,23 @@ TEST_P( // Check to see preceding controller (diff_drive_controller) is still inactive and // following controllers (pid_left_wheel_controller) (pid_left_wheel_controller) are still active EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, pid_left_wheel_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + pid_left_wheel_controller->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, pid_right_wheel_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + pid_right_wheel_controller->get_lifecycle_state().id()); ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, diff_drive_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + diff_drive_controller->get_lifecycle_state().id()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - odom_publisher_controller->get_state().id()); + odom_publisher_controller->get_lifecycle_state().id()); ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, sensor_fusion_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - robot_localization_controller->get_state().id()); + robot_localization_controller->get_lifecycle_state().id()); // Test Case 6: following controller is deactivated but preceding controller will be activated // --> return error; controllers stay in the same state @@ -1479,10 +1515,15 @@ TEST_P( // should be deactivated (if BEST_EFFORT) // If STRICT, preceding controller should stay deactivated and following controller // should stay activated - EXPECT_EQ(expected.at(test_param.strictness).state, pid_right_wheel_controller->get_state().id()); - EXPECT_EQ(expected.at(test_param.strictness).state, pid_left_wheel_controller->get_state().id()); + EXPECT_EQ( + expected.at(test_param.strictness).state, + pid_right_wheel_controller->get_lifecycle_state().id()); + EXPECT_EQ( + expected.at(test_param.strictness).state, + pid_left_wheel_controller->get_lifecycle_state().id()); ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, diff_drive_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + diff_drive_controller->get_lifecycle_state().id()); // Test Case 7: following controller deactivation but preceding controller is active // --> return error; controllers stay in the same state as they were @@ -1499,11 +1540,14 @@ TEST_P( // Expect all controllers to be active ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, pid_left_wheel_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + pid_left_wheel_controller->get_lifecycle_state().id()); ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, pid_right_wheel_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + pid_right_wheel_controller->get_lifecycle_state().id()); ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + diff_drive_controller->get_lifecycle_state().id()); // Attempt to deactivate following controllers switch_test_controllers( @@ -1512,11 +1556,14 @@ TEST_P( // All controllers should still be active ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, pid_left_wheel_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + pid_left_wheel_controller->get_lifecycle_state().id()); ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, pid_right_wheel_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + pid_right_wheel_controller->get_lifecycle_state().id()); ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + diff_drive_controller->get_lifecycle_state().id()); // Attempt to deactivate a following controller switch_test_controllers( @@ -1525,11 +1572,11 @@ TEST_P( // All controllers should still be active ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, pid_left_wheel_controller->get_state().id()); - ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, pid_right_wheel_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + pid_left_wheel_controller->get_lifecycle_state().id()); ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + diff_drive_controller->get_lifecycle_state().id()); } TEST_P( @@ -1602,21 +1649,26 @@ TEST_P( // Verify that initially all of them are in active state EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, pid_left_wheel_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + pid_left_wheel_controller->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, pid_right_wheel_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + pid_right_wheel_controller->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + diff_drive_controller->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, sensor_fusion_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - robot_localization_controller->get_state().id()); + robot_localization_controller->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, odom_publisher_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + odom_publisher_controller->get_lifecycle_state().id()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - position_tracking_controller->get_state().id()); + position_tracking_controller->get_lifecycle_state().id()); // There is different error and timeout behavior depending on strictness static std::unordered_map expected = { @@ -1643,16 +1695,19 @@ TEST_P( EXPECT_FALSE(diff_drive_controller->is_in_chained_mode()); EXPECT_FALSE(sensor_fusion_controller->is_in_chained_mode()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + diff_drive_controller->get_lifecycle_state().id()); // SensorFusionController continues to stay in the chained mode as it is still using the state // interfaces EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, sensor_fusion_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, odom_publisher_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + odom_publisher_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - robot_localization_controller->get_state().id()); + robot_localization_controller->get_lifecycle_state().id()); // DiffDrive (preceding) controller is activated --> PID controller in chained mod // Let's try to deactivate the diff_drive_control, it should fail as there are still other @@ -1668,14 +1723,17 @@ TEST_P( ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); EXPECT_FALSE(sensor_fusion_controller->is_in_chained_mode()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + diff_drive_controller->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, sensor_fusion_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, odom_publisher_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + odom_publisher_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - robot_localization_controller->get_state().id()); + robot_localization_controller->get_lifecycle_state().id()); // Trying to deactivate the sensor fusion controller, however, it won't be deactivated as the // robot localization controller is still active @@ -1688,14 +1746,17 @@ TEST_P( ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); EXPECT_FALSE(sensor_fusion_controller->is_in_chained_mode()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + diff_drive_controller->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, sensor_fusion_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, odom_publisher_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + odom_publisher_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - robot_localization_controller->get_state().id()); + robot_localization_controller->get_lifecycle_state().id()); // Deactivate the robot localization controller and see that the sensor fusion controller is still // active but not in the chained mode @@ -1706,14 +1767,17 @@ TEST_P( ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); ASSERT_FALSE(sensor_fusion_controller->is_in_chained_mode()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + diff_drive_controller->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, sensor_fusion_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, odom_publisher_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + odom_publisher_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - robot_localization_controller->get_state().id()); + robot_localization_controller->get_lifecycle_state().id()); // Deactivate the sensor_fusion controller and this should be successful as there are no other // controllers using it's interfaces @@ -1723,14 +1787,17 @@ TEST_P( ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); ASSERT_FALSE(sensor_fusion_controller->is_in_chained_mode()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + diff_drive_controller->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, sensor_fusion_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, odom_publisher_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + odom_publisher_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - robot_localization_controller->get_state().id()); + robot_localization_controller->get_lifecycle_state().id()); // Deactivate the odometry publisher controller and now the diff_drive should continue active but // not in chained mode @@ -1740,15 +1807,17 @@ TEST_P( EXPECT_FALSE(diff_drive_controller->is_in_chained_mode()); ASSERT_FALSE(sensor_fusion_controller->is_in_chained_mode()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + diff_drive_controller->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, sensor_fusion_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - odom_publisher_controller->get_state().id()); + odom_publisher_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - robot_localization_controller->get_state().id()); + robot_localization_controller->get_lifecycle_state().id()); // Deactivate the diff_drive_controller as all it's following controllers that uses it's // interfaces are deactivated @@ -1759,12 +1828,14 @@ TEST_P( EXPECT_FALSE(diff_drive_controller->is_in_chained_mode()); ASSERT_FALSE(sensor_fusion_controller->is_in_chained_mode()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, diff_drive_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + diff_drive_controller->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, sensor_fusion_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - odom_publisher_controller->get_state().id()); + odom_publisher_controller->get_lifecycle_state().id()); // all controllers are deactivated --> chained mode is not changed DeactivateAndCheckController( @@ -1776,15 +1847,17 @@ TEST_P( EXPECT_FALSE(diff_drive_controller->is_in_chained_mode()); ASSERT_FALSE(sensor_fusion_controller->is_in_chained_mode()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, diff_drive_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + diff_drive_controller->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, sensor_fusion_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - odom_publisher_controller->get_state().id()); + odom_publisher_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - robot_localization_controller->get_state().id()); + robot_localization_controller->get_lifecycle_state().id()); } TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_adding_in_random_order) diff --git a/controller_manager/test/test_load_controller.cpp b/controller_manager/test/test_load_controller.cpp index 19d34c0c11..3645c06c24 100644 --- a/controller_manager/test/test_load_controller.cpp +++ b/controller_manager/test/test_load_controller.cpp @@ -63,12 +63,14 @@ TEST_F(TestLoadController, can_set_and_get_non_default_update_rate) ASSERT_NE(controller_if, nullptr); ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + controller_if->get_lifecycle_state().id()); controller_if->get_node()->set_parameter({"update_rate", 1337}); cm_->configure_controller("test_controller_01"); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_lifecycle_state().id()); EXPECT_EQ(1337u, controller_if->get_update_rate()); } @@ -119,10 +121,12 @@ TEST_P(TestLoadedControllerParametrized, load_and_configure_one_known_controller EXPECT_EQ(1u, cm_->get_loaded_controllers().size()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + controller_if->get_lifecycle_state().id()); cm_->configure_controller(CONTROLLER_NAME_1); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_lifecycle_state().id()); } TEST_P(TestLoadedControllerParametrized, can_start_configured_controller) @@ -131,7 +135,8 @@ TEST_P(TestLoadedControllerParametrized, can_start_configured_controller) EXPECT_EQ(cm_->configure_controller(CONTROLLER_NAME_1), controller_interface::return_type::OK); start_test_controller(test_param.strictness); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if->get_lifecycle_state().id()); } TEST_P(TestLoadedControllerParametrized, can_stop_active_controller) @@ -144,7 +149,8 @@ TEST_P(TestLoadedControllerParametrized, can_stop_active_controller) // Stop controller stop_test_controller(test_param.strictness); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_lifecycle_state().id()); } TEST_P(TestLoadedControllerParametrized, starting_and_stopping_a_controller) @@ -152,14 +158,16 @@ TEST_P(TestLoadedControllerParametrized, starting_and_stopping_a_controller) const auto test_param = GetParam(); ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + controller_if->get_lifecycle_state().id()); { // Test starting unconfigured controller, and starting configured afterwards start_test_controller( test_param.strictness, std::future_status::ready, test_param.expected_return); ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + controller_if->get_lifecycle_state().id()); // Activate configured controller { @@ -167,12 +175,15 @@ TEST_P(TestLoadedControllerParametrized, starting_and_stopping_a_controller) cm_->configure_controller(CONTROLLER_NAME_1); } start_test_controller(test_param.strictness); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if->get_lifecycle_state().id()); } { // Stop controller stop_test_controller(test_param.strictness); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + controller_if->get_lifecycle_state().id()); } } @@ -185,7 +196,8 @@ TEST_P(TestLoadedControllerParametrized, can_not_configure_active_controller) // Can not configure active controller EXPECT_EQ(cm_->configure_controller(CONTROLLER_NAME_1), controller_interface::return_type::ERROR); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if->get_lifecycle_state().id()); } TEST_P(TestLoadedControllerParametrized, can_not_start_finalized_controller) @@ -193,7 +205,8 @@ TEST_P(TestLoadedControllerParametrized, can_not_start_finalized_controller) const auto test_param = GetParam(); ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + controller_if->get_lifecycle_state().id()); // Shutdown controller on purpose for testing ASSERT_EQ( @@ -206,7 +219,8 @@ TEST_P(TestLoadedControllerParametrized, can_not_start_finalized_controller) // Can not configure finalize controller EXPECT_EQ(cm_->configure_controller(CONTROLLER_NAME_1), controller_interface::return_type::ERROR); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, controller_if->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, controller_if->get_lifecycle_state().id()); } TEST_P(TestLoadedControllerParametrized, inactive_controller_cannot_be_cleaned_up) @@ -219,7 +233,8 @@ TEST_P(TestLoadedControllerParametrized, inactive_controller_cannot_be_cleaned_u stop_test_controller(test_param.strictness); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_lifecycle_state().id()); std::shared_ptr test_controller = std::dynamic_pointer_cast(controller_if); @@ -228,7 +243,8 @@ TEST_P(TestLoadedControllerParametrized, inactive_controller_cannot_be_cleaned_u // Configure from inactive state: controller can no be cleaned-up test_controller->simulate_cleanup_failure = true; EXPECT_EQ(cm_->configure_controller(CONTROLLER_NAME_1), controller_interface::return_type::ERROR); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_lifecycle_state().id()); EXPECT_EQ(0u, cleanup_calls); } @@ -241,7 +257,8 @@ TEST_P(TestLoadedControllerParametrized, inactive_controller_cannot_be_configure start_test_controller(test_param.strictness); stop_test_controller(test_param.strictness); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_lifecycle_state().id()); std::shared_ptr test_controller = std::dynamic_pointer_cast(controller_if); @@ -253,7 +270,8 @@ TEST_P(TestLoadedControllerParametrized, inactive_controller_cannot_be_configure ControllerManagerRunner cm_runner(this); EXPECT_EQ(cm_->configure_controller(CONTROLLER_NAME_1), controller_interface::return_type::OK); } - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_lifecycle_state().id()); EXPECT_EQ(1u, cleanup_calls); } @@ -278,7 +296,8 @@ TEST_P(SwitchTest, EmptyListOrNonExistentTest) EXPECT_EQ(1u, cm_->get_loaded_controllers().size()); ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + controller_if->get_lifecycle_state().id()); auto params = GetParam(); auto result = std::get<0>(params); @@ -382,19 +401,23 @@ class TestTwoLoadedControllers : public TestLoadController, ASSERT_NE(controller_if2, nullptr); EXPECT_EQ(2u, cm_->get_loaded_controllers().size()); ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if1->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + controller_if1->get_lifecycle_state().id()); ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if2->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + controller_if2->get_lifecycle_state().id()); } }; TEST_F(TestTwoLoadedControllers, load_and_configure_two_known_controllers) { cm_->configure_controller(CONTROLLER_NAME_1); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if1->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if1->get_lifecycle_state().id()); cm_->configure_controller(CONTROLLER_NAME_2); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if2->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if2->get_lifecycle_state().id()); } TEST_P(TestTwoLoadedControllers, switch_multiple_controllers) @@ -406,9 +429,11 @@ TEST_P(TestTwoLoadedControllers, switch_multiple_controllers) // Start controller #1 RCLCPP_INFO(cm_->get_logger(), "Starting stopped controller #1"); switch_test_controllers(strvec{CONTROLLER_NAME_1}, strvec{}, test_param.strictness); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if1->get_state().id()); ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if2->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if1->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + controller_if2->get_lifecycle_state().id()); // Stop controller 1, start controller 2 // Both fail because controller 2 because it is not configured and STRICT is used @@ -423,9 +448,9 @@ TEST_P(TestTwoLoadedControllers, switch_multiple_controllers) switch_test_controllers( strvec{CONTROLLER_NAME_2}, strvec{CONTROLLER_NAME_1}, STRICT, std::future_status::ready, controller_interface::return_type::ERROR); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if1->get_state().id()); ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if2->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + controller_if2->get_lifecycle_state().id()); { ControllerManagerRunner cm_runner(this); @@ -435,26 +460,33 @@ TEST_P(TestTwoLoadedControllers, switch_multiple_controllers) // Stop controller 1 RCLCPP_INFO(cm_->get_logger(), "Stopping controller #1"); switch_test_controllers(strvec{}, strvec{CONTROLLER_NAME_1}, test_param.strictness); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if1->get_state().id()); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if2->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if1->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if2->get_lifecycle_state().id()); // Start controller 1 again RCLCPP_INFO(cm_->get_logger(), "Starting stopped controller #1"); switch_test_controllers(strvec{CONTROLLER_NAME_1}, strvec{}, test_param.strictness); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if1->get_state().id()); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if2->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if1->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if2->get_lifecycle_state().id()); // Stop controller 1, start controller 2 RCLCPP_INFO(cm_->get_logger(), "Stopping controller #1, starting controller #2"); switch_test_controllers( strvec{CONTROLLER_NAME_2}, strvec{CONTROLLER_NAME_1}, test_param.strictness); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if1->get_state().id()); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if2->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if1->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if2->get_lifecycle_state().id()); // Stop controller 2 RCLCPP_INFO(cm_->get_logger(), "Stopping controller #2"); switch_test_controllers(strvec{}, strvec{CONTROLLER_NAME_2}, test_param.strictness); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if2->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if2->get_lifecycle_state().id()); } INSTANTIATE_TEST_SUITE_P( diff --git a/controller_manager/test/test_release_interfaces.cpp b/controller_manager/test/test_release_interfaces.cpp index c0305324b6..9caef761ab 100644 --- a/controller_manager/test/test_release_interfaces.cpp +++ b/controller_manager/test/test_release_interfaces.cpp @@ -49,10 +49,10 @@ TEST_F(TestReleaseInterfaces, switch_controllers_same_interface) ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - abstract_test_controller1.c->get_state().id()); + abstract_test_controller1.c->get_lifecycle_state().id()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - abstract_test_controller2.c->get_state().id()); + abstract_test_controller2.c->get_lifecycle_state().id()); { // Test starting the first controller RCLCPP_INFO(cm_->get_logger(), "Starting controller #1"); @@ -67,10 +67,10 @@ TEST_F(TestReleaseInterfaces, switch_controllers_same_interface) EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - abstract_test_controller1.c->get_state().id()); + abstract_test_controller1.c->get_lifecycle_state().id()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - abstract_test_controller2.c->get_state().id()); + abstract_test_controller2.c->get_lifecycle_state().id()); } { // Test starting the second controller when the first is running @@ -87,10 +87,10 @@ TEST_F(TestReleaseInterfaces, switch_controllers_same_interface) EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - abstract_test_controller1.c->get_state().id()); + abstract_test_controller1.c->get_lifecycle_state().id()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - abstract_test_controller2.c->get_state().id()); + abstract_test_controller2.c->get_lifecycle_state().id()); } { // Test stopping controller #1 and starting controller #2 @@ -106,10 +106,10 @@ TEST_F(TestReleaseInterfaces, switch_controllers_same_interface) EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - abstract_test_controller1.c->get_state().id()); + abstract_test_controller1.c->get_lifecycle_state().id()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - abstract_test_controller2.c->get_state().id()); + abstract_test_controller2.c->get_lifecycle_state().id()); } { // Test stopping controller #2 and starting controller #1 @@ -125,10 +125,10 @@ TEST_F(TestReleaseInterfaces, switch_controllers_same_interface) EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - abstract_test_controller1.c->get_state().id()); + abstract_test_controller1.c->get_lifecycle_state().id()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - abstract_test_controller2.c->get_state().id()); + abstract_test_controller2.c->get_lifecycle_state().id()); } { // Test stopping both controllers when only controller #1 is running @@ -149,10 +149,10 @@ TEST_F(TestReleaseInterfaces, switch_controllers_same_interface) EXPECT_EQ(controller_interface::return_type::ERROR, switch_future.get()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - abstract_test_controller1.c->get_state().id()); + abstract_test_controller1.c->get_lifecycle_state().id()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - abstract_test_controller2.c->get_state().id()); + abstract_test_controller2.c->get_lifecycle_state().id()); } { // Test stopping both controllers when only controller #1 is running @@ -170,10 +170,10 @@ TEST_F(TestReleaseInterfaces, switch_controllers_same_interface) EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - abstract_test_controller1.c->get_state().id()); + abstract_test_controller1.c->get_lifecycle_state().id()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - abstract_test_controller2.c->get_state().id()); + abstract_test_controller2.c->get_lifecycle_state().id()); } { // Test starting both controllers at the same time @@ -191,9 +191,9 @@ TEST_F(TestReleaseInterfaces, switch_controllers_same_interface) EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - abstract_test_controller1.c->get_state().id()); + abstract_test_controller1.c->get_lifecycle_state().id()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - abstract_test_controller2.c->get_state().id()); + abstract_test_controller2.c->get_lifecycle_state().id()); } } diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index 49326f9e79..d073abaf95 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -111,7 +111,8 @@ TEST_F(TestLoadController, spawner_test_type_in_param) auto ctrl_1 = cm_->get_loaded_controllers()[0]; ASSERT_EQ(ctrl_1.info.name, "ctrl_1"); ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); - ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + ASSERT_EQ( + ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); } // Try to spawn again, it should fail because already active @@ -131,7 +132,8 @@ TEST_F(TestLoadController, spawner_test_type_in_param) auto ctrl_1 = cm_->get_loaded_controllers()[0]; ASSERT_EQ(ctrl_1.info.name, "ctrl_1"); ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); - ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + ASSERT_EQ( + ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); } cm_->switch_controller( @@ -147,7 +149,8 @@ TEST_F(TestLoadController, spawner_test_type_in_param) auto ctrl_1 = cm_->get_loaded_controllers()[0]; ASSERT_EQ(ctrl_1.info.name, "ctrl_1"); ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); - ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + ASSERT_EQ( + ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); } // Unload and reload @@ -159,7 +162,8 @@ TEST_F(TestLoadController, spawner_test_type_in_param) auto ctrl_1 = cm_->get_loaded_controllers()[0]; ASSERT_EQ(ctrl_1.info.name, "ctrl_1"); ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); - ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + ASSERT_EQ( + ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); } } @@ -186,7 +190,8 @@ TEST_F(TestLoadController, multi_ctrls_test_type_in_param) [&](const auto & controller) { return controller.info.name == controller_name; }); ASSERT_TRUE(it != loaded_controllers.end()); ASSERT_EQ(ctrl.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); - ASSERT_EQ(ctrl.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + ASSERT_EQ( + ctrl.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); } }; @@ -270,7 +275,7 @@ TEST_F(TestLoadController, spawner_test_type_in_params_file) ASSERT_EQ(ctrl_with_parameters_and_type.info.name, "ctrl_with_parameters_and_type"); ASSERT_EQ(ctrl_with_parameters_and_type.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_EQ( - ctrl_with_parameters_and_type.c->get_state().id(), + ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); auto chain_ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[1]; @@ -280,7 +285,7 @@ TEST_F(TestLoadController, spawner_test_type_in_params_file) chain_ctrl_with_parameters_and_type.info.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_EQ( - chain_ctrl_with_parameters_and_type.c->get_state().id(), + chain_ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); EXPECT_EQ( @@ -295,12 +300,14 @@ TEST_F(TestLoadController, spawner_test_type_in_params_file) auto ctrl_1 = cm_->get_loaded_controllers()[0]; ASSERT_EQ(ctrl_1.info.name, "ctrl_with_parameters_and_type"); ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); - ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + ASSERT_EQ( + ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); auto ctrl_2 = cm_->get_loaded_controllers()[1]; ASSERT_EQ(ctrl_2.info.name, "chainable_ctrl_with_parameters_and_type"); ASSERT_EQ(ctrl_2.info.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); - ASSERT_EQ(ctrl_2.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + ASSERT_EQ( + ctrl_2.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); } TEST_F(TestLoadController, unload_on_kill) @@ -339,7 +346,8 @@ TEST_F(TestLoadController, spawner_test_fallback_controllers) ASSERT_EQ(ctrl_1.info.name, "ctrl_1"); ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_TRUE(ctrl_1.info.fallback_controllers_names.empty()); - ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + ASSERT_EQ( + ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); } // Try to spawn now the controller with fallback controllers inside the yaml @@ -352,20 +360,23 @@ TEST_F(TestLoadController, spawner_test_fallback_controllers) ASSERT_EQ(ctrl_1.info.name, "ctrl_1"); ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_TRUE(ctrl_1.info.fallback_controllers_names.empty()); - ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + ASSERT_EQ( + ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); auto ctrl_2 = cm_->get_loaded_controllers()[1]; ASSERT_EQ(ctrl_2.info.name, "ctrl_2"); ASSERT_EQ(ctrl_2.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_THAT( ctrl_2.info.fallback_controllers_names, testing::ElementsAre("ctrl_6", "ctrl_7", "ctrl_8")); - ASSERT_EQ(ctrl_2.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + ASSERT_EQ( + ctrl_2.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); auto ctrl_3 = cm_->get_loaded_controllers()[2]; ASSERT_EQ(ctrl_3.info.name, "ctrl_3"); ASSERT_EQ(ctrl_3.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_THAT(ctrl_3.info.fallback_controllers_names, testing::ElementsAre("ctrl_9")); - ASSERT_EQ(ctrl_3.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + ASSERT_EQ( + ctrl_3.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); } } @@ -391,7 +402,7 @@ TEST_F(TestLoadController, spawner_with_many_controllers) { auto ctrl = cm_->get_loaded_controllers()[i]; ASSERT_EQ(ctrl.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); - ASSERT_EQ(ctrl.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + ASSERT_EQ(ctrl.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); } } @@ -472,7 +483,8 @@ TEST_F( auto ctrl_1 = cm_->get_loaded_controllers()[0]; ASSERT_EQ(ctrl_1.info.name, "ctrl_1"); ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); - ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + ASSERT_EQ( + ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); } } @@ -548,7 +560,8 @@ TEST_F(TestLoadControllerWithNamespacedCM, multi_ctrls_test_type_in_param) [&](const auto & controller) { return controller.info.name == controller_name; }); ASSERT_TRUE(it != loaded_controllers.end()); ASSERT_EQ(ctrl.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); - ASSERT_EQ(ctrl.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + ASSERT_EQ( + ctrl.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); } }; @@ -646,7 +659,7 @@ TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_type_in_params_file) ASSERT_EQ(ctrl_with_parameters_and_type.info.name, "ctrl_with_parameters_and_type"); ASSERT_EQ(ctrl_with_parameters_and_type.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_EQ( - ctrl_with_parameters_and_type.c->get_state().id(), + ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); auto chain_ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[1]; @@ -656,7 +669,7 @@ TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_type_in_params_file) chain_ctrl_with_parameters_and_type.info.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_EQ( - chain_ctrl_with_parameters_and_type.c->get_state().id(), + chain_ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); EXPECT_EQ( @@ -671,12 +684,14 @@ TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_type_in_params_file) auto ctrl_1 = cm_->get_loaded_controllers()[0]; ASSERT_EQ(ctrl_1.info.name, "ctrl_with_parameters_and_type"); ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); - ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + ASSERT_EQ( + ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); auto ctrl_2 = cm_->get_loaded_controllers()[1]; ASSERT_EQ(ctrl_2.info.name, "chainable_ctrl_with_parameters_and_type"); ASSERT_EQ(ctrl_2.info.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); - ASSERT_EQ(ctrl_2.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + ASSERT_EQ( + ctrl_2.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); } TEST_F( @@ -722,7 +737,7 @@ TEST_F( ASSERT_EQ(ctrl_with_parameters_and_type.info.name, "ctrl_with_parameters_and_type"); ASSERT_EQ(ctrl_with_parameters_and_type.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_EQ( - ctrl_with_parameters_and_type.c->get_state().id(), + ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); auto chain_ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[1]; @@ -732,7 +747,7 @@ TEST_F( chain_ctrl_with_parameters_and_type.info.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_EQ( - chain_ctrl_with_parameters_and_type.c->get_state().id(), + chain_ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); EXPECT_EQ( @@ -747,10 +762,12 @@ TEST_F( auto ctrl_1 = cm_->get_loaded_controllers()[0]; ASSERT_EQ(ctrl_1.info.name, "ctrl_with_parameters_and_type"); ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); - ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + ASSERT_EQ( + ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); auto ctrl_2 = cm_->get_loaded_controllers()[1]; ASSERT_EQ(ctrl_2.info.name, "chainable_ctrl_with_parameters_and_type"); ASSERT_EQ(ctrl_2.info.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); - ASSERT_EQ(ctrl_2.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + ASSERT_EQ( + ctrl_2.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); } diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 4aaa828091..d0fe7b5962 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -22,8 +22,8 @@ For details see the controller_manager section. * A method to get node options to setup the controller node #api-breaking (`#1169 `_) * Export state interfaces from the chainable controller #api-breaking (`#1021 `_) * All chainable controllers must implement the method ``export_state_interfaces`` to export the state interfaces, similar to ``export_reference_interfaces`` method that is exporting the reference interfaces. -* The controllers will now set ``use_global_arguments`` from `NodeOptions `__ to false, to avoid getting influenced by global arguments (Issue : `#1684 `_) (`#1694 `_). - From now on, in order to set the parameters to the controller, the ``--param-file`` option from spawner should be used. +* The controllers will now set ``use_global_arguments`` from `NodeOptions `__ to false, to avoid getting influenced by global arguments (Issue : `#1684 `_) (`#1694 `_).From now on, in order to set the parameters to the controller, the ``--param-file`` option from spawner should be used. +* With (`#1683 `_) the ``rclcpp_lifecycle::State & get_state()`` and ``void set_state(const rclcpp_lifecycle::State & new_state)`` are replaced by ``rclcpp_lifecycle::State & get_lifecycle_state()`` and ``void set_lifecycle_state(const rclcpp_lifecycle::State & new_state)``. This change affects controllers and hardware. This is related to (`#1240 `_) as variant support introduces ``get_state`` and ``set_state`` methods for setting/getting state of handles. controller_manager ****************** @@ -103,6 +103,7 @@ hardware_interface * Soft limits are also parsed from the URDF into the ``HardwareInfo`` structure for the defined joints (`#1488 `_) * Access to logger and clock through ``get_logger`` and ``get_clock`` methods in ResourceManager and HardwareComponents ``Actuator``, ``Sensor`` and ``System`` (`#1585 `_) * Added ``get_hardware_info`` method to the hardware components interface to access the ``HardwareInfo`` instead of accessing the variable ``info_`` directly (`#1643 `_) +* With (`#1683 `_) the ``rclcpp_lifecycle::State & get_state()`` and ``void set_state(const rclcpp_lifecycle::State & new_state)`` are replaced by ``rclcpp_lifecycle::State & get_lifecycle_state()`` and ``void set_lifecycle_state(const rclcpp_lifecycle::State & new_state)``. This change affects controllers and hardware. This is related to (`#1240 `_) as variant support introduces ``get_state`` and ``set_state`` methods for setting/getting state of handles. joint_limits ************ diff --git a/hardware_interface/include/hardware_interface/actuator.hpp b/hardware_interface/include/hardware_interface/actuator.hpp index bb91b62057..4fcdd93307 100644 --- a/hardware_interface/include/hardware_interface/actuator.hpp +++ b/hardware_interface/include/hardware_interface/actuator.hpp @@ -93,7 +93,7 @@ class Actuator final std::string get_group_name() const; HARDWARE_INTERFACE_PUBLIC - const rclcpp_lifecycle::State & get_state() const; + const rclcpp_lifecycle::State & get_lifecycle_state() const; HARDWARE_INTERFACE_PUBLIC return_type read(const rclcpp::Time & time, const rclcpp::Duration & period); diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index a74d2a4964..d88fcd1f2d 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -220,13 +220,16 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod /** * \return state. */ - const rclcpp_lifecycle::State & get_state() const { return lifecycle_state_; } + const rclcpp_lifecycle::State & get_lifecycle_state() const { return lifecycle_state_; } /// Set life-cycle state of the actuator hardware. /** * \return state. */ - void set_state(const rclcpp_lifecycle::State & new_state) { lifecycle_state_ = new_state; } + void set_lifecycle_state(const rclcpp_lifecycle::State & new_state) + { + lifecycle_state_ = new_state; + } /// Get the logger of the ActuatorInterface. /** diff --git a/hardware_interface/include/hardware_interface/async_components.hpp b/hardware_interface/include/hardware_interface/async_components.hpp index 0504419b38..052c4ba921 100644 --- a/hardware_interface/include/hardware_interface/async_components.hpp +++ b/hardware_interface/include/hardware_interface/async_components.hpp @@ -84,7 +84,9 @@ class AsyncComponentThread TimePoint next_iteration_time = TimePoint(std::chrono::nanoseconds(clock_interface_->get_clock()->now().nanoseconds())); - if (component->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + if ( + component->get_lifecycle_state().id() == + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { auto current_time = clock_interface_->get_clock()->now(); auto measured_period = current_time - previous_time; diff --git a/hardware_interface/include/hardware_interface/sensor.hpp b/hardware_interface/include/hardware_interface/sensor.hpp index 98197bd0e9..e1490c91fb 100644 --- a/hardware_interface/include/hardware_interface/sensor.hpp +++ b/hardware_interface/include/hardware_interface/sensor.hpp @@ -80,7 +80,7 @@ class Sensor final std::string get_group_name() const; HARDWARE_INTERFACE_PUBLIC - const rclcpp_lifecycle::State & get_state() const; + const rclcpp_lifecycle::State & get_lifecycle_state() const; HARDWARE_INTERFACE_PUBLIC return_type read(const rclcpp::Time & time, const rclcpp::Duration & period); diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index 0c7f43e7fc..4e9ac48d5d 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -159,13 +159,16 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI /** * \return state. */ - const rclcpp_lifecycle::State & get_state() const { return lifecycle_state_; } + const rclcpp_lifecycle::State & get_lifecycle_state() const { return lifecycle_state_; } /// Set life-cycle state of the actuator hardware. /** * \return state. */ - void set_state(const rclcpp_lifecycle::State & new_state) { lifecycle_state_ = new_state; } + void set_lifecycle_state(const rclcpp_lifecycle::State & new_state) + { + lifecycle_state_ = new_state; + } /// Get the logger of the SensorInterface. /** diff --git a/hardware_interface/include/hardware_interface/system.hpp b/hardware_interface/include/hardware_interface/system.hpp index 1cc29a085d..34056c982e 100644 --- a/hardware_interface/include/hardware_interface/system.hpp +++ b/hardware_interface/include/hardware_interface/system.hpp @@ -93,7 +93,7 @@ class System final std::string get_group_name() const; HARDWARE_INTERFACE_PUBLIC - const rclcpp_lifecycle::State & get_state() const; + const rclcpp_lifecycle::State & get_lifecycle_state() const; HARDWARE_INTERFACE_PUBLIC return_type read(const rclcpp::Time & time, const rclcpp::Duration & period); diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index 54f1af33a7..37745b36fb 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -222,13 +222,16 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI /** * \return state. */ - const rclcpp_lifecycle::State & get_state() const { return lifecycle_state_; } + const rclcpp_lifecycle::State & get_lifecycle_state() const { return lifecycle_state_; } /// Set life-cycle state of the actuator hardware. /** * \return state. */ - void set_state(const rclcpp_lifecycle::State & new_state) { lifecycle_state_ = new_state; } + void set_lifecycle_state(const rclcpp_lifecycle::State & new_state) + { + lifecycle_state_ = new_state; + } /// Get the logger of the SystemInterface. /** diff --git a/hardware_interface/src/actuator.cpp b/hardware_interface/src/actuator.cpp index 52cfc681a1..d586b463af 100644 --- a/hardware_interface/src/actuator.cpp +++ b/hardware_interface/src/actuator.cpp @@ -47,158 +47,158 @@ const rclcpp_lifecycle::State & Actuator::initialize( rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) { std::unique_lock lock(actuators_mutex_); - if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) + if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) { switch (impl_->init(actuator_info, logger, clock_interface)) { case CallbackReturn::SUCCESS: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, lifecycle_state_names::UNCONFIGURED)); break; case CallbackReturn::FAILURE: case CallbackReturn::ERROR: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, lifecycle_state_names::FINALIZED)); break; } } - return impl_->get_state(); + return impl_->get_lifecycle_state(); } const rclcpp_lifecycle::State & Actuator::configure() { std::unique_lock lock(actuators_mutex_); - if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) + if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) { - switch (impl_->on_configure(impl_->get_state())) + switch (impl_->on_configure(impl_->get_lifecycle_state())) { case CallbackReturn::SUCCESS: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE)); break; case CallbackReturn::FAILURE: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, lifecycle_state_names::UNCONFIGURED)); break; case CallbackReturn::ERROR: - impl_->set_state(error()); + impl_->set_lifecycle_state(error()); break; } } - return impl_->get_state(); + return impl_->get_lifecycle_state(); } const rclcpp_lifecycle::State & Actuator::cleanup() { std::unique_lock lock(actuators_mutex_); - if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { - switch (impl_->on_cleanup(impl_->get_state())) + switch (impl_->on_cleanup(impl_->get_lifecycle_state())) { case CallbackReturn::SUCCESS: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, lifecycle_state_names::UNCONFIGURED)); break; case CallbackReturn::FAILURE: case CallbackReturn::ERROR: - impl_->set_state(error()); + impl_->set_lifecycle_state(error()); break; } } - return impl_->get_state(); + return impl_->get_lifecycle_state(); } const rclcpp_lifecycle::State & Actuator::shutdown() { std::unique_lock lock(actuators_mutex_); if ( - impl_->get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN && - impl_->get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) + impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN && + impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) { - switch (impl_->on_shutdown(impl_->get_state())) + switch (impl_->on_shutdown(impl_->get_lifecycle_state())) { case CallbackReturn::SUCCESS: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, lifecycle_state_names::FINALIZED)); break; case CallbackReturn::FAILURE: case CallbackReturn::ERROR: - impl_->set_state(error()); + impl_->set_lifecycle_state(error()); break; } } - return impl_->get_state(); + return impl_->get_lifecycle_state(); } const rclcpp_lifecycle::State & Actuator::activate() { std::unique_lock lock(actuators_mutex_); - if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { - switch (impl_->on_activate(impl_->get_state())) + switch (impl_->on_activate(impl_->get_lifecycle_state())) { case CallbackReturn::SUCCESS: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE)); break; case CallbackReturn::FAILURE: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE)); break; case CallbackReturn::ERROR: - impl_->set_state(error()); + impl_->set_lifecycle_state(error()); break; } } - return impl_->get_state(); + return impl_->get_lifecycle_state(); } const rclcpp_lifecycle::State & Actuator::deactivate() { std::unique_lock lock(actuators_mutex_); - if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { - switch (impl_->on_deactivate(impl_->get_state())) + switch (impl_->on_deactivate(impl_->get_lifecycle_state())) { case CallbackReturn::SUCCESS: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE)); break; case CallbackReturn::FAILURE: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE)); break; case CallbackReturn::ERROR: - impl_->set_state(error()); + impl_->set_lifecycle_state(error()); break; } } - return impl_->get_state(); + return impl_->get_lifecycle_state(); } const rclcpp_lifecycle::State & Actuator::error() { std::unique_lock lock(actuators_mutex_); - if (impl_->get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) + if (impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) { - switch (impl_->on_error(impl_->get_state())) + switch (impl_->on_error(impl_->get_lifecycle_state())) { case CallbackReturn::SUCCESS: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, lifecycle_state_names::UNCONFIGURED)); break; case CallbackReturn::FAILURE: case CallbackReturn::ERROR: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, lifecycle_state_names::FINALIZED)); break; } } - return impl_->get_state(); + return impl_->get_lifecycle_state(); } std::vector Actuator::export_state_interfaces() @@ -231,7 +231,10 @@ std::string Actuator::get_name() const { return impl_->get_name(); } std::string Actuator::get_group_name() const { return impl_->get_group_name(); } -const rclcpp_lifecycle::State & Actuator::get_state() const { return impl_->get_state(); } +const rclcpp_lifecycle::State & Actuator::get_lifecycle_state() const +{ + return impl_->get_lifecycle_state(); +} return_type Actuator::read(const rclcpp::Time & time, const rclcpp::Duration & period) { @@ -244,15 +247,15 @@ return_type Actuator::read(const rclcpp::Time & time, const rclcpp::Duration & p return return_type::OK; } if ( - impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED || - impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) + impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED || + impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) { return return_type::OK; } return_type result = return_type::ERROR; if ( - impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || - impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || + impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { result = impl_->read(time, period); if (result == return_type::ERROR) @@ -274,15 +277,15 @@ return_type Actuator::write(const rclcpp::Time & time, const rclcpp::Duration & return return_type::OK; } if ( - impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED || - impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) + impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED || + impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) { return return_type::OK; } return_type result = return_type::ERROR; if ( - impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || - impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || + impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { result = impl_->write(time, period); if (result == return_type::ERROR) diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 53fcad94d2..72e966fcab 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -514,7 +514,7 @@ class ResourceStorage switch (target_state.id()) { case State::PRIMARY_STATE_UNCONFIGURED: - switch (hardware.get_state().id()) + switch (hardware.get_lifecycle_state().id()) { case State::PRIMARY_STATE_UNCONFIGURED: result = true; @@ -538,7 +538,7 @@ class ResourceStorage } break; case State::PRIMARY_STATE_INACTIVE: - switch (hardware.get_state().id()) + switch (hardware.get_lifecycle_state().id()) { case State::PRIMARY_STATE_UNCONFIGURED: result = configure_hardware(hardware); @@ -558,7 +558,7 @@ class ResourceStorage } break; case State::PRIMARY_STATE_ACTIVE: - switch (hardware.get_state().id()) + switch (hardware.get_lifecycle_state().id()) { case State::PRIMARY_STATE_UNCONFIGURED: result = configure_hardware(hardware); @@ -582,7 +582,7 @@ class ResourceStorage } break; case State::PRIMARY_STATE_FINALIZED: - switch (hardware.get_state().id()) + switch (hardware.get_lifecycle_state().id()) { case State::PRIMARY_STATE_UNCONFIGURED: result = shutdown_hardware(hardware); @@ -1462,7 +1462,8 @@ std::unordered_map ResourceManager::get_comp { for (auto & component : container) { - resource_storage_->hardware_info_map_[component.get_name()].state = component.get_state(); + resource_storage_->hardware_info_map_[component.get_name()].state = + component.get_lifecycle_state(); } }; @@ -1546,8 +1547,9 @@ bool ResourceManager::prepare_command_mode_switch( for (auto & component : components) { if ( - component.get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || - component.get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + component.get_lifecycle_state().id() == + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || + component.get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { try { @@ -1611,8 +1613,9 @@ bool ResourceManager::perform_command_mode_switch( for (auto & component : components) { if ( - component.get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || - component.get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + component.get_lifecycle_state().id() == + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || + component.get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { try { diff --git a/hardware_interface/src/sensor.cpp b/hardware_interface/src/sensor.cpp index 9f1d4b73e1..2cffa649fd 100644 --- a/hardware_interface/src/sensor.cpp +++ b/hardware_interface/src/sensor.cpp @@ -46,158 +46,158 @@ const rclcpp_lifecycle::State & Sensor::initialize( rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) { std::unique_lock lock(sensors_mutex_); - if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) + if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) { switch (impl_->init(sensor_info, logger, clock_interface)) { case CallbackReturn::SUCCESS: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, lifecycle_state_names::UNCONFIGURED)); break; case CallbackReturn::FAILURE: case CallbackReturn::ERROR: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, lifecycle_state_names::FINALIZED)); break; } } - return impl_->get_state(); + return impl_->get_lifecycle_state(); } const rclcpp_lifecycle::State & Sensor::configure() { std::unique_lock lock(sensors_mutex_); - if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) + if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) { - switch (impl_->on_configure(impl_->get_state())) + switch (impl_->on_configure(impl_->get_lifecycle_state())) { case CallbackReturn::SUCCESS: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE)); break; case CallbackReturn::FAILURE: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, lifecycle_state_names::UNCONFIGURED)); break; case CallbackReturn::ERROR: - impl_->set_state(error()); + impl_->set_lifecycle_state(error()); break; } } - return impl_->get_state(); + return impl_->get_lifecycle_state(); } const rclcpp_lifecycle::State & Sensor::cleanup() { std::unique_lock lock(sensors_mutex_); - if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { - switch (impl_->on_cleanup(impl_->get_state())) + switch (impl_->on_cleanup(impl_->get_lifecycle_state())) { case CallbackReturn::SUCCESS: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, lifecycle_state_names::UNCONFIGURED)); break; case CallbackReturn::FAILURE: case CallbackReturn::ERROR: - impl_->set_state(error()); + impl_->set_lifecycle_state(error()); break; } } - return impl_->get_state(); + return impl_->get_lifecycle_state(); } const rclcpp_lifecycle::State & Sensor::shutdown() { std::unique_lock lock(sensors_mutex_); if ( - impl_->get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN && - impl_->get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) + impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN && + impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) { - switch (impl_->on_shutdown(impl_->get_state())) + switch (impl_->on_shutdown(impl_->get_lifecycle_state())) { case CallbackReturn::SUCCESS: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, lifecycle_state_names::FINALIZED)); break; case CallbackReturn::FAILURE: case CallbackReturn::ERROR: - impl_->set_state(error()); + impl_->set_lifecycle_state(error()); break; } } - return impl_->get_state(); + return impl_->get_lifecycle_state(); } const rclcpp_lifecycle::State & Sensor::activate() { std::unique_lock lock(sensors_mutex_); - if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { - switch (impl_->on_activate(impl_->get_state())) + switch (impl_->on_activate(impl_->get_lifecycle_state())) { case CallbackReturn::SUCCESS: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE)); break; case CallbackReturn::FAILURE: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE)); break; case CallbackReturn::ERROR: - impl_->set_state(error()); + impl_->set_lifecycle_state(error()); break; } } - return impl_->get_state(); + return impl_->get_lifecycle_state(); } const rclcpp_lifecycle::State & Sensor::deactivate() { std::unique_lock lock(sensors_mutex_); - if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { - switch (impl_->on_deactivate(impl_->get_state())) + switch (impl_->on_deactivate(impl_->get_lifecycle_state())) { case CallbackReturn::SUCCESS: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE)); break; case CallbackReturn::FAILURE: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE)); break; case CallbackReturn::ERROR: - impl_->set_state(error()); + impl_->set_lifecycle_state(error()); break; } } - return impl_->get_state(); + return impl_->get_lifecycle_state(); } const rclcpp_lifecycle::State & Sensor::error() { std::unique_lock lock(sensors_mutex_); - if (impl_->get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) + if (impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) { - switch (impl_->on_error(impl_->get_state())) + switch (impl_->on_error(impl_->get_lifecycle_state())) { case CallbackReturn::SUCCESS: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, lifecycle_state_names::UNCONFIGURED)); break; case CallbackReturn::FAILURE: case CallbackReturn::ERROR: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, lifecycle_state_names::FINALIZED)); break; } } - return impl_->get_state(); + return impl_->get_lifecycle_state(); } std::vector Sensor::export_state_interfaces() @@ -209,7 +209,10 @@ std::string Sensor::get_name() const { return impl_->get_name(); } std::string Sensor::get_group_name() const { return impl_->get_group_name(); } -const rclcpp_lifecycle::State & Sensor::get_state() const { return impl_->get_state(); } +const rclcpp_lifecycle::State & Sensor::get_lifecycle_state() const +{ + return impl_->get_lifecycle_state(); +} return_type Sensor::read(const rclcpp::Time & time, const rclcpp::Duration & period) { @@ -222,15 +225,15 @@ return_type Sensor::read(const rclcpp::Time & time, const rclcpp::Duration & per return return_type::OK; } if ( - impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED || - impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) + impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED || + impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) { return return_type::OK; } return_type result = return_type::ERROR; if ( - impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || - impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || + impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { result = impl_->read(time, period); if (result == return_type::ERROR) diff --git a/hardware_interface/src/system.cpp b/hardware_interface/src/system.cpp index 82473563b6..8f455a7bd5 100644 --- a/hardware_interface/src/system.cpp +++ b/hardware_interface/src/system.cpp @@ -45,158 +45,158 @@ const rclcpp_lifecycle::State & System::initialize( rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) { std::unique_lock lock(system_mutex_); - if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) + if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) { switch (impl_->init(system_info, logger, clock_interface)) { case CallbackReturn::SUCCESS: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, lifecycle_state_names::UNCONFIGURED)); break; case CallbackReturn::FAILURE: case CallbackReturn::ERROR: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, lifecycle_state_names::FINALIZED)); break; } } - return impl_->get_state(); + return impl_->get_lifecycle_state(); } const rclcpp_lifecycle::State & System::configure() { std::unique_lock lock(system_mutex_); - if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) + if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) { - switch (impl_->on_configure(impl_->get_state())) + switch (impl_->on_configure(impl_->get_lifecycle_state())) { case CallbackReturn::SUCCESS: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE)); break; case CallbackReturn::FAILURE: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, lifecycle_state_names::UNCONFIGURED)); break; case CallbackReturn::ERROR: - impl_->set_state(error()); + impl_->set_lifecycle_state(error()); break; } } - return impl_->get_state(); + return impl_->get_lifecycle_state(); } const rclcpp_lifecycle::State & System::cleanup() { std::unique_lock lock(system_mutex_); - if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { - switch (impl_->on_cleanup(impl_->get_state())) + switch (impl_->on_cleanup(impl_->get_lifecycle_state())) { case CallbackReturn::SUCCESS: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, lifecycle_state_names::UNCONFIGURED)); break; case CallbackReturn::FAILURE: case CallbackReturn::ERROR: - impl_->set_state(error()); + impl_->set_lifecycle_state(error()); break; } } - return impl_->get_state(); + return impl_->get_lifecycle_state(); } const rclcpp_lifecycle::State & System::shutdown() { std::unique_lock lock(system_mutex_); if ( - impl_->get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN && - impl_->get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) + impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN && + impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) { - switch (impl_->on_shutdown(impl_->get_state())) + switch (impl_->on_shutdown(impl_->get_lifecycle_state())) { case CallbackReturn::SUCCESS: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, lifecycle_state_names::FINALIZED)); break; case CallbackReturn::FAILURE: case CallbackReturn::ERROR: - impl_->set_state(error()); + impl_->set_lifecycle_state(error()); break; } } - return impl_->get_state(); + return impl_->get_lifecycle_state(); } const rclcpp_lifecycle::State & System::activate() { std::unique_lock lock(system_mutex_); - if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { - switch (impl_->on_activate(impl_->get_state())) + switch (impl_->on_activate(impl_->get_lifecycle_state())) { case CallbackReturn::SUCCESS: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE)); break; case CallbackReturn::FAILURE: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE)); break; case CallbackReturn::ERROR: - impl_->set_state(error()); + impl_->set_lifecycle_state(error()); break; } } - return impl_->get_state(); + return impl_->get_lifecycle_state(); } const rclcpp_lifecycle::State & System::deactivate() { std::unique_lock lock(system_mutex_); - if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { - switch (impl_->on_deactivate(impl_->get_state())) + switch (impl_->on_deactivate(impl_->get_lifecycle_state())) { case CallbackReturn::SUCCESS: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE)); break; case CallbackReturn::FAILURE: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE)); break; case CallbackReturn::ERROR: - impl_->set_state(error()); + impl_->set_lifecycle_state(error()); break; } } - return impl_->get_state(); + return impl_->get_lifecycle_state(); } const rclcpp_lifecycle::State & System::error() { std::unique_lock lock(system_mutex_); - if (impl_->get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) + if (impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) { - switch (impl_->on_error(impl_->get_state())) + switch (impl_->on_error(impl_->get_lifecycle_state())) { case CallbackReturn::SUCCESS: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, lifecycle_state_names::UNCONFIGURED)); break; case CallbackReturn::FAILURE: case CallbackReturn::ERROR: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, lifecycle_state_names::FINALIZED)); break; } } - return impl_->get_state(); + return impl_->get_lifecycle_state(); } std::vector System::export_state_interfaces() @@ -227,7 +227,10 @@ std::string System::get_name() const { return impl_->get_name(); } std::string System::get_group_name() const { return impl_->get_group_name(); } -const rclcpp_lifecycle::State & System::get_state() const { return impl_->get_state(); } +const rclcpp_lifecycle::State & System::get_lifecycle_state() const +{ + return impl_->get_lifecycle_state(); +} return_type System::read(const rclcpp::Time & time, const rclcpp::Duration & period) { @@ -240,15 +243,15 @@ return_type System::read(const rclcpp::Time & time, const rclcpp::Duration & per return return_type::OK; } if ( - impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED || - impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) + impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED || + impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) { return return_type::OK; } return_type result = return_type::ERROR; if ( - impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || - impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || + impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { result = impl_->read(time, period); if (result == return_type::ERROR) @@ -270,15 +273,15 @@ return_type System::write(const rclcpp::Time & time, const rclcpp::Duration & pe return return_type::OK; } if ( - impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED || - impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) + impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED || + impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) { return return_type::OK; } return_type result = return_type::ERROR; if ( - impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || - impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || + impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { result = impl_->write(time, period); if (result == return_type::ERROR) diff --git a/hardware_interface/test/test_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp index 986c32d37b..b6a8cc81c8 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -726,7 +726,7 @@ TEST(TestComponentInterfaces, dummy_actuator_read_error_behavior) } ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.read(TIME, PERIOD)); - state = actuator_hw.get_state(); + 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()); @@ -749,7 +749,7 @@ TEST(TestComponentInterfaces, dummy_actuator_read_error_behavior) } ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.read(TIME, PERIOD)); - state = actuator_hw.get_state(); + 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()); @@ -786,7 +786,7 @@ TEST(TestComponentInterfaces, dummy_actuator_write_error_behavior) } ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.write(TIME, PERIOD)); - state = actuator_hw.get_state(); + 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()); @@ -809,7 +809,7 @@ TEST(TestComponentInterfaces, dummy_actuator_write_error_behavior) } ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.write(TIME, PERIOD)); - state = actuator_hw.get_state(); + 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()); @@ -843,7 +843,7 @@ TEST(TestComponentInterfaces, dummy_sensor_read_error_behavior) } ASSERT_EQ(hardware_interface::return_type::ERROR, sensor_hw.read(TIME, PERIOD)); - state = sensor_hw.get_state(); + 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()); @@ -868,7 +868,7 @@ TEST(TestComponentInterfaces, dummy_sensor_read_error_behavior) } ASSERT_EQ(hardware_interface::return_type::ERROR, sensor_hw.read(TIME, PERIOD)); - state = sensor_hw.get_state(); + 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()); @@ -911,7 +911,7 @@ TEST(TestComponentInterfaces, dummy_system_read_error_behavior) } ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.read(TIME, PERIOD)); - state = system_hw.get_state(); + 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()); @@ -939,7 +939,7 @@ TEST(TestComponentInterfaces, dummy_system_read_error_behavior) } ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.read(TIME, PERIOD)); - state = system_hw.get_state(); + 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()); @@ -976,7 +976,7 @@ TEST(TestComponentInterfaces, dummy_system_write_error_behavior) } ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.write(TIME, PERIOD)); - state = system_hw.get_state(); + 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()); @@ -1004,7 +1004,7 @@ TEST(TestComponentInterfaces, dummy_system_write_error_behavior) } ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.write(TIME, PERIOD)); - state = system_hw.get_state(); + 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());