From 74a9c2944a2038800cf57c89ff9d567be487c595 Mon Sep 17 00:00:00 2001 From: Vatan Aksoy Tezer Date: Tue, 21 Sep 2021 03:31:51 +0300 Subject: [PATCH 01/16] Add lifecycle nodes --- .../controller_interface.hpp | 48 +----- .../src/controller_interface.cpp | 155 ++++-------------- .../test/test_controller_with_options.cpp | 4 +- .../test/test_controller_with_options.hpp | 9 +- controller_manager/src/controller_manager.cpp | 39 +++-- .../test/test_controller_manager.cpp | 14 +- .../test/test_controller_manager_srvs.cpp | 18 +- .../test/test_release_interfaces.cpp | 32 ++-- .../test/test_spawner_unspawner.cpp | 12 +- 9 files changed, 104 insertions(+), 227 deletions(-) diff --git a/controller_interface/include/controller_interface/controller_interface.hpp b/controller_interface/include/controller_interface/controller_interface.hpp index 0d58da4d0d..5d4ad745c5 100644 --- a/controller_interface/include/controller_interface/controller_interface.hpp +++ b/controller_interface/include/controller_interface/controller_interface.hpp @@ -23,10 +23,9 @@ #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/loaned_state_interface.hpp" -#include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/rclcpp.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" namespace controller_interface { @@ -89,7 +88,10 @@ class ControllerInterface : public rclcpp_lifecycle::node_interfaces::LifecycleN virtual return_type update(const rclcpp::Time & time, const rclcpp::Duration & period) = 0; CONTROLLER_INTERFACE_PUBLIC - std::shared_ptr get_node(); + std::shared_ptr get_lifecycle_node(); + + CONTROLLER_INTERFACE_PUBLIC + unsigned int get_update_rate() const; /// Declare and initialize a parameter with a type. /** @@ -101,52 +103,20 @@ class ControllerInterface : public rclcpp_lifecycle::node_interfaces::LifecycleN template auto auto_declare(const std::string & name, const ParameterT & default_value) { - if (!node_->has_parameter(name)) + if (!lifecycle_node_->has_parameter(name)) { - return node_->declare_parameter(name, default_value); + return lifecycle_node_->declare_parameter(name, default_value); } else { - return node_->get_parameter(name).get_value(); + return lifecycle_node_->get_parameter(name).get_value(); } } - /** - * The methods below are a substitute to the LifecycleNode methods with the same name. - * The Life cycle is shown in ROS2 design document: - * https://design.ros2.org/articles/node_lifecycle.html - * We cannot use a LifecycleNode because it would expose change-state services to the rest - * of the ROS system. - * Only the Controller Manager should have possibility to change state of a controller. - * - * Hopefully in the future we can use a LifecycleNode where we disable modifications from the outside. - */ - CONTROLLER_INTERFACE_PUBLIC - const rclcpp_lifecycle::State & configure(); - - CONTROLLER_INTERFACE_PUBLIC - const rclcpp_lifecycle::State & cleanup(); - - CONTROLLER_INTERFACE_PUBLIC - const rclcpp_lifecycle::State & deactivate(); - - CONTROLLER_INTERFACE_PUBLIC - const rclcpp_lifecycle::State & activate(); - - CONTROLLER_INTERFACE_PUBLIC - const rclcpp_lifecycle::State & shutdown(); - - CONTROLLER_INTERFACE_PUBLIC - const rclcpp_lifecycle::State & get_state() const; - - CONTROLLER_INTERFACE_PUBLIC - unsigned int get_update_rate() const; - protected: std::vector command_interfaces_; std::vector state_interfaces_; - std::shared_ptr node_; - rclcpp_lifecycle::State lifecycle_state_; + std::shared_ptr lifecycle_node_; unsigned int update_rate_ = 0; }; diff --git a/controller_interface/src/controller_interface.cpp b/controller_interface/src/controller_interface.cpp index 703ef33c82..10e2d32659 100644 --- a/controller_interface/src/controller_interface.cpp +++ b/controller_interface/src/controller_interface.cpp @@ -20,148 +20,53 @@ #include #include "hardware_interface/types/lifecycle_state_names.hpp" -#include "lifecycle_msgs/msg/state.hpp" namespace controller_interface { return_type ControllerInterface::init(const std::string & controller_name) { - node_ = std::make_shared( - controller_name, rclcpp::NodeOptions() - .allow_undeclared_parameters(true) - .automatically_declare_parameters_from_overrides(true)); + lifecycle_node_ = std::make_shared( + controller_name, + rclcpp::NodeOptions().allow_undeclared_parameters(true). + automatically_declare_parameters_from_overrides(true)); + return_type result = return_type::OK; switch (on_init()) { case LifecycleNodeInterface::CallbackReturn::SUCCESS: - lifecycle_state_ = rclcpp_lifecycle::State( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, - hardware_interface::lifecycle_state_names::UNCONFIGURED); break; case LifecycleNodeInterface::CallbackReturn::ERROR: case LifecycleNodeInterface::CallbackReturn::FAILURE: result = return_type::ERROR; break; } - return result; -} -const rclcpp_lifecycle::State & ControllerInterface::configure() -{ - if (lifecycle_state_.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) - { - switch (on_configure(lifecycle_state_)) - { - case LifecycleNodeInterface::CallbackReturn::SUCCESS: - lifecycle_state_ = rclcpp_lifecycle::State( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - hardware_interface::lifecycle_state_names::INACTIVE); - break; - case LifecycleNodeInterface::CallbackReturn::ERROR: - on_error(lifecycle_state_); - lifecycle_state_ = rclcpp_lifecycle::State( - lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, - hardware_interface::lifecycle_state_names::FINALIZED); - break; - case LifecycleNodeInterface::CallbackReturn::FAILURE: - break; - } - - if (node_->has_parameter("update_rate")) - { - update_rate_ = node_->get_parameter("update_rate").as_int(); - } - } - return lifecycle_state_; -} + lifecycle_node_->register_on_configure( + std::bind(&ControllerInterface::on_configure, this, std::placeholders::_1)); -const rclcpp_lifecycle::State & ControllerInterface::cleanup() -{ - switch (on_cleanup(lifecycle_state_)) - { - case LifecycleNodeInterface::CallbackReturn::SUCCESS: - lifecycle_state_ = rclcpp_lifecycle::State( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, - hardware_interface::lifecycle_state_names::UNCONFIGURED); - break; - case LifecycleNodeInterface::CallbackReturn::ERROR: - on_error(lifecycle_state_); - lifecycle_state_ = rclcpp_lifecycle::State( - lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, - hardware_interface::lifecycle_state_names::FINALIZED); - break; - case LifecycleNodeInterface::CallbackReturn::FAILURE: - break; - } - return lifecycle_state_; -} -const rclcpp_lifecycle::State & ControllerInterface::deactivate() -{ - switch (on_deactivate(lifecycle_state_)) - { - case LifecycleNodeInterface::CallbackReturn::SUCCESS: - lifecycle_state_ = rclcpp_lifecycle::State( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - hardware_interface::lifecycle_state_names::INACTIVE); - break; - case LifecycleNodeInterface::CallbackReturn::ERROR: - on_error(lifecycle_state_); - lifecycle_state_ = rclcpp_lifecycle::State( - lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, - hardware_interface::lifecycle_state_names::FINALIZED); - break; - case LifecycleNodeInterface::CallbackReturn::FAILURE: - break; - } - return lifecycle_state_; -} -const rclcpp_lifecycle::State & ControllerInterface::activate() -{ - if (lifecycle_state_.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) - { - switch (on_activate(lifecycle_state_)) - { - case LifecycleNodeInterface::CallbackReturn::SUCCESS: - lifecycle_state_ = rclcpp_lifecycle::State( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - hardware_interface::lifecycle_state_names::ACTIVE); - break; - case LifecycleNodeInterface::CallbackReturn::ERROR: - on_error(lifecycle_state_); - lifecycle_state_ = rclcpp_lifecycle::State( - lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, - hardware_interface::lifecycle_state_names::FINALIZED); - break; - case LifecycleNodeInterface::CallbackReturn::FAILURE: - break; - } - } - return lifecycle_state_; -} + lifecycle_node_->register_on_cleanup( + std::bind(&ControllerInterface::on_cleanup, this, std::placeholders::_1)); -const rclcpp_lifecycle::State & ControllerInterface::shutdown() -{ - switch (on_shutdown(lifecycle_state_)) + lifecycle_node_->register_on_activate( + std::bind(&ControllerInterface::on_activate, this, std::placeholders::_1)); + + lifecycle_node_->register_on_deactivate( + std::bind(&ControllerInterface::on_deactivate, this, std::placeholders::_1)); + + lifecycle_node_->register_on_shutdown( + std::bind(&ControllerInterface::on_shutdown, this, std::placeholders::_1)); + + lifecycle_node_->register_on_error( + std::bind(&ControllerInterface::on_error, this, std::placeholders::_1)); + + if (lifecycle_node_->has_parameter("update_rate")) { - case LifecycleNodeInterface::CallbackReturn::SUCCESS: - lifecycle_state_ = rclcpp_lifecycle::State( - lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, - hardware_interface::lifecycle_state_names::FINALIZED); - break; - case LifecycleNodeInterface::CallbackReturn::ERROR: - on_error(lifecycle_state_); - lifecycle_state_ = rclcpp_lifecycle::State( - lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, - hardware_interface::lifecycle_state_names::FINALIZED); - break; - case LifecycleNodeInterface::CallbackReturn::FAILURE: - break; + update_rate_ = lifecycle_node_->get_parameter("update_rate").as_int(); } - return lifecycle_state_; -} -const rclcpp_lifecycle::State & ControllerInterface::get_state() const { return lifecycle_state_; } + return result; +} void ControllerInterface::assign_interfaces( std::vector && command_interfaces, @@ -177,13 +82,15 @@ void ControllerInterface::release_interfaces() state_interfaces_.clear(); } -std::shared_ptr ControllerInterface::get_node() +std::shared_ptr +ControllerInterface::get_lifecycle_node() { - if (!node_.get()) + + if(!lifecycle_node_.get()) { - throw std::runtime_error("Node hasn't been initialized yet!"); + throw std::runtime_error("Lifecycle node hasn't been initialized yet!"); } - return node_; + return lifecycle_node_; } unsigned int ControllerInterface::get_update_rate() const { return update_rate_; } diff --git a/controller_interface/test/test_controller_with_options.cpp b/controller_interface/test/test_controller_with_options.cpp index 1426c4e19c..40480ed899 100644 --- a/controller_interface/test/test_controller_with_options.cpp +++ b/controller_interface/test/test_controller_with_options.cpp @@ -42,7 +42,7 @@ TEST(ControllerWithOption, init_with_overrides) FriendControllerWithOptions controller; EXPECT_EQ(controller.init("controller_name"), controller_interface::return_type::OK); // checks that the node options have been updated - const auto & node_options = controller.node_->get_node_options(); + const auto & node_options = controller.lifecycle_node_->get_node_options(); EXPECT_TRUE(node_options.allow_undeclared_parameters()); EXPECT_TRUE(node_options.automatically_declare_parameters_from_overrides()); // checks that the parameters have been correctly processed @@ -63,7 +63,7 @@ TEST(ControllerWithOption, init_without_overrides) FriendControllerWithOptions controller; EXPECT_EQ(controller.init("controller_name"), controller_interface::return_type::ERROR); // checks that the node options have been updated - const auto & node_options = controller.node_->get_node_options(); + const auto & node_options = controller.lifecycle_node_->get_node_options(); EXPECT_TRUE(node_options.allow_undeclared_parameters()); EXPECT_TRUE(node_options.automatically_declare_parameters_from_overrides()); // checks that no parameter has been declared from overrides diff --git a/controller_interface/test/test_controller_with_options.hpp b/controller_interface/test/test_controller_with_options.hpp index 5ce95453d2..17d77119b3 100644 --- a/controller_interface/test/test_controller_with_options.hpp +++ b/controller_interface/test/test_controller_with_options.hpp @@ -44,22 +44,19 @@ class ControllerWithOptions : public controller_interface::ControllerInterface { rclcpp::NodeOptions options; options.allow_undeclared_parameters(true).automatically_declare_parameters_from_overrides(true); - node_ = std::make_shared(controller_name, options); + lifecycle_node_ = std::make_shared(controller_name, options); switch (on_init()) { case LifecycleNodeInterface::CallbackReturn::SUCCESS: - lifecycle_state_ = rclcpp_lifecycle::State( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, - hardware_interface::lifecycle_state_names::UNCONFIGURED); break; case LifecycleNodeInterface::CallbackReturn::ERROR: case LifecycleNodeInterface::CallbackReturn::FAILURE: return controller_interface::return_type::ERROR; } - if (node_->get_parameters("parameter_list", params)) + if (lifecycle_node_->get_parameters("parameter_list", params)) { - RCLCPP_INFO_STREAM(node_->get_logger(), "I found " << params.size() << " parameters."); + RCLCPP_INFO_STREAM(lifecycle_node_->get_logger(), "I found " << params.size() << " parameters."); return controller_interface::return_type::OK; } else diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index d42aef00f0..7ece7ed649 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -47,7 +47,7 @@ static const rmw_qos_profile_t rmw_qos_profile_services_hist_keep_all = { inline bool is_controller_inactive(const controller_interface::ControllerInterface & controller) { - return controller.get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE; + return controller.get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE; } inline bool is_controller_inactive( @@ -312,8 +312,8 @@ controller_interface::return_type ControllerManager::unload_controller( } RCLCPP_DEBUG(get_logger(), "Cleanup controller"); - controller.c->cleanup(); - executor_->remove_node(controller.c->get_node()); + controller.c->get_lifecycle_node()->cleanup(); + executor_->remove_node(controller.c->get_lifecycle_node()->get_node_base_interface()); to.erase(found_it); // Destroys the old controllers list when the realtime thread is finished with it. @@ -355,7 +355,7 @@ controller_interface::return_type ControllerManager::configure_controller( } auto controller = found_it->c; - auto state = controller->get_state(); + auto state = controller->get_lifecycle_node()->get_current_state(); if ( state.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE || state.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) @@ -366,12 +366,12 @@ 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_node()->get_current_state(); if (state.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { RCLCPP_DEBUG( get_logger(), "Controller '%s' is cleaned-up before configuring", controller_name.c_str()); - new_state = controller->cleanup(); + new_state = controller->get_lifecycle_node()->cleanup(); if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) { RCLCPP_ERROR( @@ -381,7 +381,7 @@ controller_interface::return_type ControllerManager::configure_controller( } } - new_state = controller->configure(); + new_state = controller->get_lifecycle_node()->configure(); if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { RCLCPP_ERROR( @@ -722,9 +722,10 @@ controller_interface::ControllerInterfaceSharedPtr ControllerManager::add_contro "Setting use_sim_time=True for %s to match controller manager " "(see ros2_control#325 for details)", controller.info.name.c_str()); - controller.c->get_node()->set_parameter(use_sim_time); + controller.c->get_lifecycle_node()->set_parameter(use_sim_time); } - executor_->add_node(controller.c->get_node()); + controller.c->get_lifecycle_node()->configure(); + executor_->add_node(controller.c->get_lifecycle_node()->get_node_base_interface()); to.emplace_back(controller); // Destroys the old controllers list when the realtime thread is finished with it. @@ -780,12 +781,9 @@ void ControllerManager::stop_controllers() continue; } auto controller = found_it->c; - if (is_controller_active(*controller)) - { - const auto new_state = controller->deactivate(); - controller->release_interfaces(); - if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) - { + if (is_controller_active(*controller)) { + const auto new_state = controller->get_lifecycle_node()->deactivate(); + if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { RCLCPP_ERROR( get_logger(), "After deactivating, controller '%s' is in state '%s', expected Inactive", request.c_str(), new_state.label().c_str()); @@ -893,12 +891,13 @@ void ControllerManager::start_controllers() } controller->assign_interfaces(std::move(command_loans), std::move(state_loans)); - const auto new_state = controller->activate(); - if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) - { + const auto new_state = controller->get_lifecycle_node()->activate(); + if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { RCLCPP_ERROR( - get_logger(), "After activating, controller '%s' is in state '%s', expected Active", - controller->get_node()->get_name(), new_state.label().c_str()); + get_logger(), + "After activating, controller %s is in state %s, expected Active", + controller->get_lifecycle_node()->get_name(), + new_state.label().c_str()); } } // All controllers started, switching done diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index ec73b315b2..5238e3f778 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -59,7 +59,8 @@ TEST_P(TestControllerManager, 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_node()->get_current_state().id()); // configure controller cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME); @@ -70,7 +71,7 @@ TEST_P(TestControllerManager, 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_node()->get_current_state().id()); // Start controller, will take effect at the end of the update function std::vector start_controllers = {"fake_controller", TEST_CONTROLLER2_NAME}; @@ -111,7 +112,7 @@ TEST_P(TestControllerManager, 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_node()->get_current_state().id()); EXPECT_EQ( controller_interface::return_type::OK, @@ -139,7 +140,9 @@ TEST_P(TestControllerManager, 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_node()->get_current_state().id()); auto unload_future = std::async( std::launch::async, &controller_manager::ControllerManager::unload_controller, cm_, test_controller::TEST_CONTROLLER_NAME); @@ -150,7 +153,8 @@ TEST_P(TestControllerManager, 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_node()->get_current_state().id()); EXPECT_EQ(1, test_controller.use_count()); } diff --git a/controller_manager/test/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp index 7cd73ab2e3..1945c2b0c3 100644 --- a/controller_manager/test/test_controller_manager_srvs.cpp +++ b/controller_manager/test/test_controller_manager_srvs.cpp @@ -210,7 +210,7 @@ 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_node()->get_current_state().id()); ASSERT_GT(test_controller.use_count(), 1) << "Controller manager should have have a copy of this shared ptr"; @@ -232,7 +232,7 @@ 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_node()->get_current_state().id()); ASSERT_GT(test_controller.use_count(), 1) << "Controller manager should have have a copy of this shared ptr"; @@ -256,13 +256,13 @@ 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_node()->get_current_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_node()->get_current_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"; @@ -304,7 +304,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_node()->get_current_state().id()); } TEST_F(TestControllerManagerSrvs, unload_controller_srv) @@ -357,7 +357,7 @@ 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()); + cm_->get_loaded_controllers()[0].c->get_lifecycle_node()->get_current_state().id()); } TEST_F(TestControllerManagerSrvs, load_configure_controller_srv) @@ -383,7 +383,7 @@ TEST_F(TestControllerManagerSrvs, load_configure_controller_srv) EXPECT_EQ(test_controller::TEST_CONTROLLER_NAME, cm_->get_loaded_controllers()[0].info.name); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - cm_->get_loaded_controllers()[0].c->get_state().id()); + cm_->get_loaded_controllers()[0].c->get_lifecycle_node()->get_current_state().id()); } TEST_F(TestControllerManagerSrvs, load_start_controller_srv) @@ -409,7 +409,7 @@ TEST_F(TestControllerManagerSrvs, load_start_controller_srv) EXPECT_EQ(test_controller::TEST_CONTROLLER_NAME, cm_->get_loaded_controllers()[0].info.name); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - cm_->get_loaded_controllers()[0].c->get_state().id()); + cm_->get_loaded_controllers()[0].c->get_lifecycle_node()->get_current_state().id()); } TEST_F(TestControllerManagerSrvs, configure_start_controller_srv) @@ -438,5 +438,5 @@ TEST_F(TestControllerManagerSrvs, configure_start_controller_srv) EXPECT_EQ(1u, cm_->get_loaded_controllers().size()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - cm_->get_loaded_controllers()[0].c->get_state().id()); + cm_->get_loaded_controllers()[0].c->get_lifecycle_node()->get_current_state().id()); } diff --git a/controller_manager/test/test_release_interfaces.cpp b/controller_manager/test/test_release_interfaces.cpp index 3fddb8bbe4..5f283ab0c7 100644 --- a/controller_manager/test/test_release_interfaces.cpp +++ b/controller_manager/test/test_release_interfaces.cpp @@ -51,10 +51,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_node()->get_current_state().id()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - abstract_test_controller2.c->get_state().id()); + abstract_test_controller2.c->get_lifecycle_node()->get_current_state().id()); { // Test starting the first controller RCLCPP_INFO(cm_->get_logger(), "Starting controller #1"); @@ -69,10 +69,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_node()->get_current_state().id()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - abstract_test_controller2.c->get_state().id()); + abstract_test_controller2.c->get_lifecycle_node()->get_current_state().id()); } { // Test starting the second controller when the first is running @@ -89,10 +89,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_node()->get_current_state().id()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - abstract_test_controller2.c->get_state().id()); + abstract_test_controller2.c->get_lifecycle_node()->get_current_state().id()); } { // Test stopping controller #1 and starting controller #2 @@ -108,10 +108,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_node()->get_current_state().id()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - abstract_test_controller2.c->get_state().id()); + abstract_test_controller2.c->get_lifecycle_node()->get_current_state().id()); } { // Test stopping controller #2 and starting controller #1 @@ -127,10 +127,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_node()->get_current_state().id()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - abstract_test_controller2.c->get_state().id()); + abstract_test_controller2.c->get_lifecycle_node()->get_current_state().id()); } { // Test stopping both controllers when only controller #1 is running @@ -151,10 +151,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_node()->get_current_state().id()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - abstract_test_controller2.c->get_state().id()); + abstract_test_controller2.c->get_lifecycle_node()->get_current_state().id()); } { // Test stopping both controllers when only controller #1 is running @@ -172,10 +172,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_node()->get_current_state().id()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - abstract_test_controller2.c->get_state().id()); + abstract_test_controller2.c->get_lifecycle_node()->get_current_state().id()); } { // Test starting both controllers at the same time @@ -193,9 +193,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_node()->get_current_state().id()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - abstract_test_controller2.c->get_state().id()); + abstract_test_controller2.c->get_lifecycle_node()->get_current_state().id()); } } diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index a29a68b045..11e87d5475 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -98,17 +98,17 @@ 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_node()->get_current_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); // Try to spawn again, it should fail because already active EXPECT_NE(call_spawner("ctrl_1 -c test_controller_manager"), 0) << "Cannot configure from active"; - ctrl_1.c->deactivate(); + ctrl_1.c->get_lifecycle_node()->deactivate(); // We should be able to reconfigure and start a configured controller EXPECT_EQ(call_spawner("ctrl_1 -c test_controller_manager"), 0); - ctrl_1.c->cleanup(); + ctrl_1.c->get_lifecycle_node()->cleanup(); // We should be able to reconfigure and start am unconfigured loaded controller EXPECT_EQ(call_spawner("ctrl_1 -c test_controller_manager"), 0); - ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + ASSERT_EQ(ctrl_1.c->get_lifecycle_node()->get_current_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); // Unload and reload EXPECT_EQ(call_unspawner("ctrl_1 -c test_controller_manager"), 0); @@ -116,7 +116,7 @@ TEST_F(TestLoadController, spawner_test_type_in_param) EXPECT_EQ(call_spawner("ctrl_1 -c test_controller_manager"), 0); ASSERT_EQ(cm_->get_loaded_controllers().size(), 1ul) << "Controller should have been loaded"; ctrl_1 = cm_->get_loaded_controllers()[0]; - ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + ASSERT_EQ(ctrl_1.c->get_lifecycle_node()->get_current_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); } TEST_F(TestLoadController, spawner_test_type_in_arg) @@ -132,7 +132,7 @@ TEST_F(TestLoadController, spawner_test_type_in_arg) auto ctrl_2 = cm_->get_loaded_controllers()[0]; ASSERT_EQ(ctrl_2.info.name, "ctrl_2"); ASSERT_EQ(ctrl_2.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); - ASSERT_EQ(ctrl_2.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + ASSERT_EQ(ctrl_2.c->get_lifecycle_node()->get_current_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); } TEST_F(TestLoadController, unload_on_kill) From 2e1997749b49efb8a8c6baf423a687ef8aa7b861 Mon Sep 17 00:00:00 2001 From: Vatan Aksoy Tezer Date: Tue, 21 Sep 2021 03:40:08 +0300 Subject: [PATCH 02/16] pre-commit fixes --- .../controller_interface.hpp | 3 +++ .../src/controller_interface.cpp | 17 ++++++++++------- .../test/test_controller_with_options.hpp | 3 ++- controller_manager/src/controller_manager.cpp | 17 +++++++++-------- .../test/test_controller_manager.cpp | 8 ++++++-- .../test/test_controller_manager_srvs.cpp | 15 +++++++++++---- .../test/test_spawner_unspawner.cpp | 16 ++++++++++++---- 7 files changed, 53 insertions(+), 26 deletions(-) diff --git a/controller_interface/include/controller_interface/controller_interface.hpp b/controller_interface/include/controller_interface/controller_interface.hpp index 5d4ad745c5..5f3f87255f 100644 --- a/controller_interface/include/controller_interface/controller_interface.hpp +++ b/controller_interface/include/controller_interface/controller_interface.hpp @@ -93,6 +93,9 @@ class ControllerInterface : public rclcpp_lifecycle::node_interfaces::LifecycleN CONTROLLER_INTERFACE_PUBLIC unsigned int get_update_rate() const; + CONTROLLER_INTERFACE_PUBLIC + const rclcpp_lifecycle::State & get_state() const; + /// Declare and initialize a parameter with a type. /** * diff --git a/controller_interface/src/controller_interface.cpp b/controller_interface/src/controller_interface.cpp index 10e2d32659..8ae7b48a97 100644 --- a/controller_interface/src/controller_interface.cpp +++ b/controller_interface/src/controller_interface.cpp @@ -26,9 +26,9 @@ namespace controller_interface return_type ControllerInterface::init(const std::string & controller_name) { lifecycle_node_ = std::make_shared( - controller_name, - rclcpp::NodeOptions().allow_undeclared_parameters(true). - automatically_declare_parameters_from_overrides(true)); + controller_name, rclcpp::NodeOptions() + .allow_undeclared_parameters(true) + .automatically_declare_parameters_from_overrides(true)); return_type result = return_type::OK; @@ -82,17 +82,20 @@ void ControllerInterface::release_interfaces() state_interfaces_.clear(); } -std::shared_ptr -ControllerInterface::get_lifecycle_node() +std::shared_ptr ControllerInterface::get_lifecycle_node() { - - if(!lifecycle_node_.get()) + if (!lifecycle_node_.get()) { throw std::runtime_error("Lifecycle node hasn't been initialized yet!"); } return lifecycle_node_; } +const rclcpp_lifecycle::State & ControllerInterface::get_state() const +{ + return lifecycle_node_->get_current_state(); +} + unsigned int ControllerInterface::get_update_rate() const { return update_rate_; } } // namespace controller_interface diff --git a/controller_interface/test/test_controller_with_options.hpp b/controller_interface/test/test_controller_with_options.hpp index 17d77119b3..c7966a73c2 100644 --- a/controller_interface/test/test_controller_with_options.hpp +++ b/controller_interface/test/test_controller_with_options.hpp @@ -56,7 +56,8 @@ class ControllerWithOptions : public controller_interface::ControllerInterface } if (lifecycle_node_->get_parameters("parameter_list", params)) { - RCLCPP_INFO_STREAM(lifecycle_node_->get_logger(), "I found " << params.size() << " parameters."); + RCLCPP_INFO_STREAM( + lifecycle_node_->get_logger(), "I found " << params.size() << " parameters."); return controller_interface::return_type::OK; } else diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 7ece7ed649..2637250589 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -47,7 +47,7 @@ static const rmw_qos_profile_t rmw_qos_profile_services_hist_keep_all = { inline bool is_controller_inactive(const controller_interface::ControllerInterface & controller) { - return controller.get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE; + return controller.get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE; } inline bool is_controller_inactive( @@ -781,9 +781,11 @@ void ControllerManager::stop_controllers() continue; } auto controller = found_it->c; - if (is_controller_active(*controller)) { + if (is_controller_active(*controller)) + { const auto new_state = controller->get_lifecycle_node()->deactivate(); - if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { + if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + { RCLCPP_ERROR( get_logger(), "After deactivating, controller '%s' is in state '%s', expected Inactive", request.c_str(), new_state.label().c_str()); @@ -892,12 +894,11 @@ void ControllerManager::start_controllers() controller->assign_interfaces(std::move(command_loans), std::move(state_loans)); const auto new_state = controller->get_lifecycle_node()->activate(); - if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { + if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { RCLCPP_ERROR( - get_logger(), - "After activating, controller %s is in state %s, expected Active", - controller->get_lifecycle_node()->get_name(), - new_state.label().c_str()); + get_logger(), "After activating, controller %s is in state %s, expected Active", + controller->get_lifecycle_node()->get_name(), new_state.label().c_str()); } } // All controllers started, switching done diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index 5238e3f778..679576c2df 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -71,7 +71,9 @@ TEST_P(TestControllerManager, 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_lifecycle_node()->get_current_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller->get_lifecycle_node()->get_current_state().id()); // Start controller, will take effect at the end of the update function std::vector start_controllers = {"fake_controller", TEST_CONTROLLER2_NAME}; @@ -112,7 +114,9 @@ TEST_P(TestControllerManager, 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_lifecycle_node()->get_current_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller->get_lifecycle_node()->get_current_state().id()); EXPECT_EQ( controller_interface::return_type::OK, diff --git a/controller_manager/test/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp index 1945c2b0c3..6d413cda37 100644 --- a/controller_manager/test/test_controller_manager_srvs.cpp +++ b/controller_manager/test/test_controller_manager_srvs.cpp @@ -210,7 +210,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_lifecycle_node()->get_current_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller->get_lifecycle_node()->get_current_state().id()); ASSERT_GT(test_controller.use_count(), 1) << "Controller manager should have have a copy of this shared ptr"; @@ -232,7 +233,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_lifecycle_node()->get_current_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller->get_lifecycle_node()->get_current_state().id()); ASSERT_GT(test_controller.use_count(), 1) << "Controller manager should have have a copy of this shared ptr"; @@ -256,13 +259,17 @@ 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_lifecycle_node()->get_current_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller->get_lifecycle_node()->get_current_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_lifecycle_node()->get_current_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller->get_lifecycle_node()->get_current_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"; diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index 11e87d5475..e596f5bcea 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -98,7 +98,9 @@ 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_lifecycle_node()->get_current_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + ASSERT_EQ( + ctrl_1.c->get_lifecycle_node()->get_current_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); // Try to spawn again, it should fail because already active EXPECT_NE(call_spawner("ctrl_1 -c test_controller_manager"), 0) << "Cannot configure from active"; @@ -108,7 +110,9 @@ TEST_F(TestLoadController, spawner_test_type_in_param) ctrl_1.c->get_lifecycle_node()->cleanup(); // We should be able to reconfigure and start am unconfigured loaded controller EXPECT_EQ(call_spawner("ctrl_1 -c test_controller_manager"), 0); - ASSERT_EQ(ctrl_1.c->get_lifecycle_node()->get_current_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + ASSERT_EQ( + ctrl_1.c->get_lifecycle_node()->get_current_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); // Unload and reload EXPECT_EQ(call_unspawner("ctrl_1 -c test_controller_manager"), 0); @@ -116,7 +120,9 @@ TEST_F(TestLoadController, spawner_test_type_in_param) EXPECT_EQ(call_spawner("ctrl_1 -c test_controller_manager"), 0); ASSERT_EQ(cm_->get_loaded_controllers().size(), 1ul) << "Controller should have been loaded"; ctrl_1 = cm_->get_loaded_controllers()[0]; - ASSERT_EQ(ctrl_1.c->get_lifecycle_node()->get_current_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + ASSERT_EQ( + ctrl_1.c->get_lifecycle_node()->get_current_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); } TEST_F(TestLoadController, spawner_test_type_in_arg) @@ -132,7 +138,9 @@ TEST_F(TestLoadController, spawner_test_type_in_arg) auto ctrl_2 = cm_->get_loaded_controllers()[0]; ASSERT_EQ(ctrl_2.info.name, "ctrl_2"); ASSERT_EQ(ctrl_2.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); - ASSERT_EQ(ctrl_2.c->get_lifecycle_node()->get_current_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + ASSERT_EQ( + ctrl_2.c->get_lifecycle_node()->get_current_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); } TEST_F(TestLoadController, unload_on_kill) From afb4dafdccb37a2185f2d94ff97116598d0d64ef Mon Sep 17 00:00:00 2001 From: Vatan Aksoy Tezer Date: Tue, 21 Sep 2021 04:27:17 +0300 Subject: [PATCH 03/16] fix state bug --- controller_manager/src/controller_manager.cpp | 2 +- .../test/test_controller_manager_srvs.cpp | 38 +++++++++++-------- 2 files changed, 23 insertions(+), 17 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 2637250589..f416fdd5bc 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -724,7 +724,6 @@ controller_interface::ControllerInterfaceSharedPtr ControllerManager::add_contro controller.info.name.c_str()); controller.c->get_lifecycle_node()->set_parameter(use_sim_time); } - controller.c->get_lifecycle_node()->configure(); executor_->add_node(controller.c->get_lifecycle_node()->get_node_base_interface()); to.emplace_back(controller); @@ -784,6 +783,7 @@ void ControllerManager::stop_controllers() if (is_controller_active(*controller)) { const auto new_state = controller->get_lifecycle_node()->deactivate(); + controller->release_interfaces(); if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { RCLCPP_ERROR( diff --git a/controller_manager/test/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp index 6d413cda37..a0ca947237 100644 --- a/controller_manager/test/test_controller_manager_srvs.cpp +++ b/controller_manager/test/test_controller_manager_srvs.cpp @@ -195,15 +195,21 @@ TEST_F(TestControllerManagerSrvs, reload_controller_libraries_srv) auto request = std::make_shared(); + // Create a lambda to store the cleanup state change + bool cleanup_called = false; + auto set_cleanup_called = [&](const rclcpp_lifecycle::State &) { + cleanup_called = true; + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + }; + // Reload with no controllers running request->force_kill = false; auto result = call_service_and_wait(*client, request, srv_executor); ASSERT_TRUE(result->ok); // Add a controller, but unconfigured - std::shared_ptr test_controller = - std::dynamic_pointer_cast(cm_->load_controller( - test_controller::TEST_CONTROLLER_NAME, test_controller::TEST_CONTROLLER_CLASS_NAME)); + auto test_controller = cm_->load_controller( + test_controller::TEST_CONTROLLER_NAME, test_controller::TEST_CONTROLLER_CLASS_NAME); // weak_ptr so the only controller shared_ptr instance is owned by the controller_manager and // can be completely destroyed before reloading the library @@ -215,21 +221,21 @@ TEST_F(TestControllerManagerSrvs, reload_controller_libraries_srv) ASSERT_GT(test_controller.use_count(), 1) << "Controller manager should have have a copy of this shared ptr"; - size_t cleanup_calls = 0; - test_controller->cleanup_calls = &cleanup_calls; + cleanup_called = false; + test_controller->get_lifecycle_node()->register_on_cleanup(set_cleanup_called); test_controller.reset(); // destroy our copy of the controller request->force_kill = false; result = call_service_and_wait(*client, request, srv_executor, true); ASSERT_TRUE(result->ok); - ASSERT_EQ(cleanup_calls, 1u); + ASSERT_TRUE(cleanup_called); ASSERT_EQ(test_controller.use_count(), 0) << "No more references to the controller after reloading."; test_controller.reset(); // Add a controller, but inactive - test_controller = std::dynamic_pointer_cast(cm_->load_controller( - test_controller::TEST_CONTROLLER_NAME, test_controller::TEST_CONTROLLER_CLASS_NAME)); + test_controller = cm_->load_controller( + test_controller::TEST_CONTROLLER_NAME, test_controller::TEST_CONTROLLER_CLASS_NAME); test_controller_weak = test_controller; cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME); @@ -239,20 +245,20 @@ TEST_F(TestControllerManagerSrvs, reload_controller_libraries_srv) ASSERT_GT(test_controller.use_count(), 1) << "Controller manager should have have a copy of this shared ptr"; - cleanup_calls = 0; - test_controller->cleanup_calls = &cleanup_calls; + cleanup_called = false; + test_controller->get_lifecycle_node()->register_on_cleanup(set_cleanup_called); test_controller.reset(); // destroy our copy of the controller request->force_kill = false; result = call_service_and_wait(*client, request, srv_executor, true); ASSERT_TRUE(result->ok); - ASSERT_EQ(cleanup_calls, 1u); + ASSERT_TRUE(cleanup_called); ASSERT_EQ(test_controller.use_count(), 0) << "No more references to the controller after reloading."; test_controller.reset(); - test_controller = std::dynamic_pointer_cast(cm_->load_controller( - test_controller::TEST_CONTROLLER_NAME, test_controller::TEST_CONTROLLER_CLASS_NAME)); + test_controller = cm_->load_controller( + test_controller::TEST_CONTROLLER_NAME, test_controller::TEST_CONTROLLER_CLASS_NAME); test_controller_weak = test_controller; cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME); // Start Controller @@ -274,8 +280,8 @@ TEST_F(TestControllerManagerSrvs, reload_controller_libraries_srv) << "Controller manager should still have have a copy of " "this shared ptr, no unloading was performed"; - cleanup_calls = 0; - test_controller->cleanup_calls = &cleanup_calls; + cleanup_called = false; + test_controller->get_lifecycle_node()->register_on_cleanup(set_cleanup_called); test_controller.reset(); // destroy our copy of the controller // Force stop active controller @@ -285,7 +291,7 @@ TEST_F(TestControllerManagerSrvs, reload_controller_libraries_srv) ASSERT_EQ(test_controller_weak.use_count(), 0) << "No more references to the controller after reloading."; - ASSERT_EQ(cleanup_calls, 1u) + ASSERT_TRUE(cleanup_called) << "Controller should have been stopped and cleaned up with force_kill = true"; } From 32bdaacf6f8bcb0a58768523fce195e38f0c1dd1 Mon Sep 17 00:00:00 2001 From: Vatan Aksoy Tezer Date: Tue, 21 Sep 2021 16:10:13 +0300 Subject: [PATCH 04/16] Add configure and cleanup methods --- .../test/test_controller/test_controller.cpp | 21 +++++++++++++++++++ .../test/test_controller/test_controller.hpp | 8 +++++++ 2 files changed, 29 insertions(+) diff --git a/controller_manager/test/test_controller/test_controller.cpp b/controller_manager/test/test_controller/test_controller.cpp index 2cb094c6df..6b922775d2 100644 --- a/controller_manager/test/test_controller/test_controller.cpp +++ b/controller_manager/test/test_controller/test_controller.cpp @@ -85,6 +85,27 @@ CallbackReturn TestController::on_cleanup(const rclcpp_lifecycle::State & /*prev return CallbackReturn::SUCCESS; } +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +TestController::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) +{ + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +TestController::on_cleanup(const rclcpp_lifecycle::State & /*previous_state*/) +{ + if (simulate_cleanup_failure) + { + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; + } + + if (cleanup_calls) + { + (*cleanup_calls)++; + } + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + void TestController::set_command_interface_configuration( const controller_interface::InterfaceConfiguration & cfg) { diff --git a/controller_manager/test/test_controller/test_controller.hpp b/controller_manager/test/test_controller/test_controller.hpp index 4e4ec44d17..11d1e213df 100644 --- a/controller_manager/test/test_controller/test_controller.hpp +++ b/controller_manager/test/test_controller/test_controller.hpp @@ -57,6 +57,14 @@ class TestController : public controller_interface::ControllerInterface CONTROLLER_MANAGER_PUBLIC CallbackReturn on_cleanup(const rclcpp_lifecycle::State & previous_state) override; + CONTROLLER_MANAGER_PUBLIC + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + CONTROLLER_MANAGER_PUBLIC + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_cleanup( + const rclcpp_lifecycle::State & previous_state) override; + CONTROLLER_MANAGER_PUBLIC void set_command_interface_configuration( const controller_interface::InterfaceConfiguration & cfg); From 964e2a7f817fdce3a8b1d9bea24d3487d0a57a64 Mon Sep 17 00:00:00 2001 From: Vatan Aksoy Tezer Date: Tue, 21 Sep 2021 16:23:17 +0300 Subject: [PATCH 05/16] address names --- .../controller_interface.hpp | 14 +++---- .../src/controller_interface.cpp | 41 +++++++++---------- .../test/test_controller_with_options.cpp | 4 +- .../test/test_controller_with_options.hpp | 7 ++-- controller_manager/src/controller_manager.cpp | 22 +++++----- .../test/test_controller_manager.cpp | 18 +++----- .../test/test_controller_manager_srvs.cpp | 31 ++++++-------- .../test/test_release_interfaces.cpp | 32 +++++++-------- .../test/test_spawner_unspawner.cpp | 20 +++------ 9 files changed, 81 insertions(+), 108 deletions(-) diff --git a/controller_interface/include/controller_interface/controller_interface.hpp b/controller_interface/include/controller_interface/controller_interface.hpp index 5f3f87255f..9f7a42a7be 100644 --- a/controller_interface/include/controller_interface/controller_interface.hpp +++ b/controller_interface/include/controller_interface/controller_interface.hpp @@ -88,13 +88,13 @@ class ControllerInterface : public rclcpp_lifecycle::node_interfaces::LifecycleN virtual return_type update(const rclcpp::Time & time, const rclcpp::Duration & period) = 0; CONTROLLER_INTERFACE_PUBLIC - std::shared_ptr get_lifecycle_node(); + std::shared_ptr get_node(); CONTROLLER_INTERFACE_PUBLIC - unsigned int get_update_rate() const; + const rclcpp_lifecycle::State & get_state() const; CONTROLLER_INTERFACE_PUBLIC - const rclcpp_lifecycle::State & get_state() const; + unsigned int get_update_rate() const; /// Declare and initialize a parameter with a type. /** @@ -106,20 +106,20 @@ class ControllerInterface : public rclcpp_lifecycle::node_interfaces::LifecycleN template auto auto_declare(const std::string & name, const ParameterT & default_value) { - if (!lifecycle_node_->has_parameter(name)) + if (!node_->has_parameter(name)) { - return lifecycle_node_->declare_parameter(name, default_value); + return node_->declare_parameter(name, default_value); } else { - return lifecycle_node_->get_parameter(name).get_value(); + return node_->get_parameter(name).get_value(); } } protected: std::vector command_interfaces_; std::vector state_interfaces_; - std::shared_ptr lifecycle_node_; + std::shared_ptr node_; unsigned int update_rate_ = 0; }; diff --git a/controller_interface/src/controller_interface.cpp b/controller_interface/src/controller_interface.cpp index 8ae7b48a97..dd6d4894fe 100644 --- a/controller_interface/src/controller_interface.cpp +++ b/controller_interface/src/controller_interface.cpp @@ -25,47 +25,44 @@ namespace controller_interface { return_type ControllerInterface::init(const std::string & controller_name) { - lifecycle_node_ = std::make_shared( + node_ = std::make_shared( controller_name, rclcpp::NodeOptions() .allow_undeclared_parameters(true) .automatically_declare_parameters_from_overrides(true)); - - return_type result = return_type::OK; switch (on_init()) { case LifecycleNodeInterface::CallbackReturn::SUCCESS: break; case LifecycleNodeInterface::CallbackReturn::ERROR: case LifecycleNodeInterface::CallbackReturn::FAILURE: - result = return_type::ERROR; + return return_type::ERROR; break; } - lifecycle_node_->register_on_configure( + node_->register_on_configure( std::bind(&ControllerInterface::on_configure, this, std::placeholders::_1)); - lifecycle_node_->register_on_cleanup( + node_->register_on_cleanup( std::bind(&ControllerInterface::on_cleanup, this, std::placeholders::_1)); - lifecycle_node_->register_on_activate( + node_->register_on_activate( std::bind(&ControllerInterface::on_activate, this, std::placeholders::_1)); - lifecycle_node_->register_on_deactivate( + node_->register_on_deactivate( std::bind(&ControllerInterface::on_deactivate, this, std::placeholders::_1)); - lifecycle_node_->register_on_shutdown( + node_->register_on_shutdown( std::bind(&ControllerInterface::on_shutdown, this, std::placeholders::_1)); - lifecycle_node_->register_on_error( - std::bind(&ControllerInterface::on_error, this, std::placeholders::_1)); + node_->register_on_error(std::bind(&ControllerInterface::on_error, this, std::placeholders::_1)); - if (lifecycle_node_->has_parameter("update_rate")) + if (node_->has_parameter("update_rate")) { - update_rate_ = lifecycle_node_->get_parameter("update_rate").as_int(); + update_rate_ = node_->get_parameter("update_rate").as_int(); } - return result; + return return_type::OK; } void ControllerInterface::assign_interfaces( @@ -82,18 +79,18 @@ void ControllerInterface::release_interfaces() state_interfaces_.clear(); } -std::shared_ptr ControllerInterface::get_lifecycle_node() +const rclcpp_lifecycle::State & ControllerInterface::get_state() const { - if (!lifecycle_node_.get()) - { - throw std::runtime_error("Lifecycle node hasn't been initialized yet!"); - } - return lifecycle_node_; + return node_->get_current_state(); } -const rclcpp_lifecycle::State & ControllerInterface::get_state() const +std::shared_ptr ControllerInterface::get_node() { - return lifecycle_node_->get_current_state(); + if (!node_.get()) + { + throw std::runtime_error("Lifecycle node hasn't been initialized yet!"); + } + return node_; } unsigned int ControllerInterface::get_update_rate() const { return update_rate_; } diff --git a/controller_interface/test/test_controller_with_options.cpp b/controller_interface/test/test_controller_with_options.cpp index 40480ed899..1426c4e19c 100644 --- a/controller_interface/test/test_controller_with_options.cpp +++ b/controller_interface/test/test_controller_with_options.cpp @@ -42,7 +42,7 @@ TEST(ControllerWithOption, init_with_overrides) FriendControllerWithOptions controller; EXPECT_EQ(controller.init("controller_name"), controller_interface::return_type::OK); // checks that the node options have been updated - const auto & node_options = controller.lifecycle_node_->get_node_options(); + const auto & node_options = controller.node_->get_node_options(); EXPECT_TRUE(node_options.allow_undeclared_parameters()); EXPECT_TRUE(node_options.automatically_declare_parameters_from_overrides()); // checks that the parameters have been correctly processed @@ -63,7 +63,7 @@ TEST(ControllerWithOption, init_without_overrides) FriendControllerWithOptions controller; EXPECT_EQ(controller.init("controller_name"), controller_interface::return_type::ERROR); // checks that the node options have been updated - const auto & node_options = controller.lifecycle_node_->get_node_options(); + const auto & node_options = controller.node_->get_node_options(); EXPECT_TRUE(node_options.allow_undeclared_parameters()); EXPECT_TRUE(node_options.automatically_declare_parameters_from_overrides()); // checks that no parameter has been declared from overrides diff --git a/controller_interface/test/test_controller_with_options.hpp b/controller_interface/test/test_controller_with_options.hpp index c7966a73c2..c025492d19 100644 --- a/controller_interface/test/test_controller_with_options.hpp +++ b/controller_interface/test/test_controller_with_options.hpp @@ -44,7 +44,7 @@ class ControllerWithOptions : public controller_interface::ControllerInterface { rclcpp::NodeOptions options; options.allow_undeclared_parameters(true).automatically_declare_parameters_from_overrides(true); - lifecycle_node_ = std::make_shared(controller_name, options); + node_ = std::make_shared(controller_name, options); switch (on_init()) { @@ -54,10 +54,9 @@ class ControllerWithOptions : public controller_interface::ControllerInterface case LifecycleNodeInterface::CallbackReturn::FAILURE: return controller_interface::return_type::ERROR; } - if (lifecycle_node_->get_parameters("parameter_list", params)) + if (node_->get_parameters("parameter_list", params)) { - RCLCPP_INFO_STREAM( - lifecycle_node_->get_logger(), "I found " << params.size() << " parameters."); + RCLCPP_INFO_STREAM(node_->get_logger(), "I found " << params.size() << " parameters."); return controller_interface::return_type::OK; } else diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index f416fdd5bc..48112f1dbe 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -312,8 +312,8 @@ controller_interface::return_type ControllerManager::unload_controller( } RCLCPP_DEBUG(get_logger(), "Cleanup controller"); - controller.c->get_lifecycle_node()->cleanup(); - executor_->remove_node(controller.c->get_lifecycle_node()->get_node_base_interface()); + controller.c->get_node()->cleanup(); + executor_->remove_node(controller.c->get_node()->get_node_base_interface()); to.erase(found_it); // Destroys the old controllers list when the realtime thread is finished with it. @@ -355,7 +355,7 @@ controller_interface::return_type ControllerManager::configure_controller( } auto controller = found_it->c; - auto state = controller->get_lifecycle_node()->get_current_state(); + auto state = controller->get_state(); if ( state.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE || state.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) @@ -366,12 +366,12 @@ controller_interface::return_type ControllerManager::configure_controller( return controller_interface::return_type::ERROR; } - auto new_state = controller->get_lifecycle_node()->get_current_state(); + auto new_state = controller->get_state(); if (state.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { RCLCPP_DEBUG( get_logger(), "Controller '%s' is cleaned-up before configuring", controller_name.c_str()); - new_state = controller->get_lifecycle_node()->cleanup(); + new_state = controller->get_node()->cleanup(); if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) { RCLCPP_ERROR( @@ -381,7 +381,7 @@ controller_interface::return_type ControllerManager::configure_controller( } } - new_state = controller->get_lifecycle_node()->configure(); + new_state = controller->get_node()->configure(); if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { RCLCPP_ERROR( @@ -722,9 +722,9 @@ controller_interface::ControllerInterfaceSharedPtr ControllerManager::add_contro "Setting use_sim_time=True for %s to match controller manager " "(see ros2_control#325 for details)", controller.info.name.c_str()); - controller.c->get_lifecycle_node()->set_parameter(use_sim_time); + controller.c->get_node()->set_parameter(use_sim_time); } - executor_->add_node(controller.c->get_lifecycle_node()->get_node_base_interface()); + executor_->add_node(controller.c->get_node()->get_node_base_interface()); to.emplace_back(controller); // Destroys the old controllers list when the realtime thread is finished with it. @@ -782,7 +782,7 @@ void ControllerManager::stop_controllers() auto controller = found_it->c; if (is_controller_active(*controller)) { - const auto new_state = controller->get_lifecycle_node()->deactivate(); + const auto new_state = controller->get_node()->deactivate(); controller->release_interfaces(); if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { @@ -893,12 +893,12 @@ void ControllerManager::start_controllers() } controller->assign_interfaces(std::move(command_loans), std::move(state_loans)); - const auto new_state = controller->get_lifecycle_node()->activate(); + const auto new_state = controller->get_node()->activate(); if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { RCLCPP_ERROR( get_logger(), "After activating, controller %s is in state %s, expected Active", - controller->get_lifecycle_node()->get_name(), new_state.label().c_str()); + controller->get_node()->get_name(), new_state.label().c_str()); } } // All controllers started, switching done diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index 679576c2df..ec73b315b2 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -59,8 +59,7 @@ TEST_P(TestControllerManager, controller_lifecycle) << "Update should not reach an unconfigured controller"; EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, - test_controller->get_lifecycle_node()->get_current_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id()); // configure controller cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME); @@ -71,9 +70,7 @@ TEST_P(TestControllerManager, 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_lifecycle_node()->get_current_state().id()); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_state().id()); // Start controller, will take effect at the end of the update function std::vector start_controllers = {"fake_controller", TEST_CONTROLLER2_NAME}; @@ -114,9 +111,7 @@ TEST_P(TestControllerManager, 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_lifecycle_node()->get_current_state().id()); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_state().id()); EXPECT_EQ( controller_interface::return_type::OK, @@ -144,9 +139,7 @@ TEST_P(TestControllerManager, controller_lifecycle) EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); } - EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - test_controller->get_lifecycle_node()->get_current_state().id()); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_state().id()); auto unload_future = std::async( std::launch::async, &controller_manager::ControllerManager::unload_controller, cm_, test_controller::TEST_CONTROLLER_NAME); @@ -157,8 +150,7 @@ TEST_P(TestControllerManager, controller_lifecycle) EXPECT_EQ(controller_interface::return_type::OK, unload_future.get()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, - test_controller->get_lifecycle_node()->get_current_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id()); EXPECT_EQ(1, test_controller.use_count()); } diff --git a/controller_manager/test/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp index a0ca947237..6187044254 100644 --- a/controller_manager/test/test_controller_manager_srvs.cpp +++ b/controller_manager/test/test_controller_manager_srvs.cpp @@ -216,13 +216,12 @@ 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_lifecycle_node()->get_current_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id()); ASSERT_GT(test_controller.use_count(), 1) << "Controller manager should have have a copy of this shared ptr"; cleanup_called = false; - test_controller->get_lifecycle_node()->register_on_cleanup(set_cleanup_called); + test_controller->get_node()->register_on_cleanup(set_cleanup_called); test_controller.reset(); // destroy our copy of the controller request->force_kill = false; @@ -239,14 +238,12 @@ 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_lifecycle_node()->get_current_state().id()); + ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_state().id()); ASSERT_GT(test_controller.use_count(), 1) << "Controller manager should have have a copy of this shared ptr"; cleanup_called = false; - test_controller->get_lifecycle_node()->register_on_cleanup(set_cleanup_called); + test_controller->get_node()->register_on_cleanup(set_cleanup_called); test_controller.reset(); // destroy our copy of the controller request->force_kill = false; @@ -265,23 +262,19 @@ 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_lifecycle_node()->get_current_state().id()); + ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_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_lifecycle_node()->get_current_state().id()); + ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_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"; cleanup_called = false; - test_controller->get_lifecycle_node()->register_on_cleanup(set_cleanup_called); + test_controller->get_node()->register_on_cleanup(set_cleanup_called); test_controller.reset(); // destroy our copy of the controller // Force stop active controller @@ -317,7 +310,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_lifecycle_node()->get_current_state().id()); + cm_->get_loaded_controllers()[0].c->get_state().id()); } TEST_F(TestControllerManagerSrvs, unload_controller_srv) @@ -370,7 +363,7 @@ 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_lifecycle_node()->get_current_state().id()); + cm_->get_loaded_controllers()[0].c->get_state().id()); } TEST_F(TestControllerManagerSrvs, load_configure_controller_srv) @@ -396,7 +389,7 @@ TEST_F(TestControllerManagerSrvs, load_configure_controller_srv) EXPECT_EQ(test_controller::TEST_CONTROLLER_NAME, cm_->get_loaded_controllers()[0].info.name); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - cm_->get_loaded_controllers()[0].c->get_lifecycle_node()->get_current_state().id()); + cm_->get_loaded_controllers()[0].c->get_state().id()); } TEST_F(TestControllerManagerSrvs, load_start_controller_srv) @@ -422,7 +415,7 @@ TEST_F(TestControllerManagerSrvs, load_start_controller_srv) EXPECT_EQ(test_controller::TEST_CONTROLLER_NAME, cm_->get_loaded_controllers()[0].info.name); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - cm_->get_loaded_controllers()[0].c->get_lifecycle_node()->get_current_state().id()); + cm_->get_loaded_controllers()[0].c->get_state().id()); } TEST_F(TestControllerManagerSrvs, configure_start_controller_srv) @@ -451,5 +444,5 @@ TEST_F(TestControllerManagerSrvs, configure_start_controller_srv) EXPECT_EQ(1u, cm_->get_loaded_controllers().size()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - cm_->get_loaded_controllers()[0].c->get_lifecycle_node()->get_current_state().id()); + cm_->get_loaded_controllers()[0].c->get_state().id()); } diff --git a/controller_manager/test/test_release_interfaces.cpp b/controller_manager/test/test_release_interfaces.cpp index 5f283ab0c7..3fddb8bbe4 100644 --- a/controller_manager/test/test_release_interfaces.cpp +++ b/controller_manager/test/test_release_interfaces.cpp @@ -51,10 +51,10 @@ TEST_F(TestReleaseInterfaces, switch_controllers_same_interface) ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - abstract_test_controller1.c->get_lifecycle_node()->get_current_state().id()); + abstract_test_controller1.c->get_state().id()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - abstract_test_controller2.c->get_lifecycle_node()->get_current_state().id()); + abstract_test_controller2.c->get_state().id()); { // Test starting the first controller RCLCPP_INFO(cm_->get_logger(), "Starting controller #1"); @@ -69,10 +69,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_lifecycle_node()->get_current_state().id()); + abstract_test_controller1.c->get_state().id()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - abstract_test_controller2.c->get_lifecycle_node()->get_current_state().id()); + abstract_test_controller2.c->get_state().id()); } { // Test starting the second controller when the first is running @@ -89,10 +89,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_lifecycle_node()->get_current_state().id()); + abstract_test_controller1.c->get_state().id()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - abstract_test_controller2.c->get_lifecycle_node()->get_current_state().id()); + abstract_test_controller2.c->get_state().id()); } { // Test stopping controller #1 and starting controller #2 @@ -108,10 +108,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_lifecycle_node()->get_current_state().id()); + abstract_test_controller1.c->get_state().id()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - abstract_test_controller2.c->get_lifecycle_node()->get_current_state().id()); + abstract_test_controller2.c->get_state().id()); } { // Test stopping controller #2 and starting controller #1 @@ -127,10 +127,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_lifecycle_node()->get_current_state().id()); + abstract_test_controller1.c->get_state().id()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - abstract_test_controller2.c->get_lifecycle_node()->get_current_state().id()); + abstract_test_controller2.c->get_state().id()); } { // Test stopping both controllers when only controller #1 is running @@ -151,10 +151,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_lifecycle_node()->get_current_state().id()); + abstract_test_controller1.c->get_state().id()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - abstract_test_controller2.c->get_lifecycle_node()->get_current_state().id()); + abstract_test_controller2.c->get_state().id()); } { // Test stopping both controllers when only controller #1 is running @@ -172,10 +172,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_lifecycle_node()->get_current_state().id()); + abstract_test_controller1.c->get_state().id()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - abstract_test_controller2.c->get_lifecycle_node()->get_current_state().id()); + abstract_test_controller2.c->get_state().id()); } { // Test starting both controllers at the same time @@ -193,9 +193,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_lifecycle_node()->get_current_state().id()); + abstract_test_controller1.c->get_state().id()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - abstract_test_controller2.c->get_lifecycle_node()->get_current_state().id()); + abstract_test_controller2.c->get_state().id()); } } diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index e596f5bcea..5f4305ef31 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -98,21 +98,17 @@ 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_lifecycle_node()->get_current_state().id(), - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); // Try to spawn again, it should fail because already active EXPECT_NE(call_spawner("ctrl_1 -c test_controller_manager"), 0) << "Cannot configure from active"; - ctrl_1.c->get_lifecycle_node()->deactivate(); + ctrl_1.c->get_node()->deactivate(); // We should be able to reconfigure and start a configured controller EXPECT_EQ(call_spawner("ctrl_1 -c test_controller_manager"), 0); - ctrl_1.c->get_lifecycle_node()->cleanup(); + ctrl_1.c->get_node()->cleanup(); // We should be able to reconfigure and start am unconfigured loaded controller EXPECT_EQ(call_spawner("ctrl_1 -c test_controller_manager"), 0); - ASSERT_EQ( - ctrl_1.c->get_lifecycle_node()->get_current_state().id(), - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); // Unload and reload EXPECT_EQ(call_unspawner("ctrl_1 -c test_controller_manager"), 0); @@ -120,9 +116,7 @@ TEST_F(TestLoadController, spawner_test_type_in_param) EXPECT_EQ(call_spawner("ctrl_1 -c test_controller_manager"), 0); ASSERT_EQ(cm_->get_loaded_controllers().size(), 1ul) << "Controller should have been loaded"; ctrl_1 = cm_->get_loaded_controllers()[0]; - ASSERT_EQ( - ctrl_1.c->get_lifecycle_node()->get_current_state().id(), - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); } TEST_F(TestLoadController, spawner_test_type_in_arg) @@ -138,9 +132,7 @@ TEST_F(TestLoadController, spawner_test_type_in_arg) auto ctrl_2 = cm_->get_loaded_controllers()[0]; ASSERT_EQ(ctrl_2.info.name, "ctrl_2"); ASSERT_EQ(ctrl_2.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); - ASSERT_EQ( - ctrl_2.c->get_lifecycle_node()->get_current_state().id(), - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + ASSERT_EQ(ctrl_2.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); } TEST_F(TestLoadController, unload_on_kill) From 0b265dde349d129ccf33de742f98eae8e90be598 Mon Sep 17 00:00:00 2001 From: Vatan Aksoy Tezer Date: Tue, 21 Sep 2021 16:46:58 +0300 Subject: [PATCH 06/16] Consts are back! --- controller_manager/src/controller_manager.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 48112f1dbe..aff0debaae 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -56,7 +56,7 @@ inline bool is_controller_inactive( return is_controller_inactive(*controller); } -inline bool is_controller_active(controller_interface::ControllerInterface & controller) +inline bool is_controller_active(const controller_interface::ControllerInterface & controller) { return controller.get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE; } From 5fee7fc7d1eda22c7fe99e4163349d47e8078f87 Mon Sep 17 00:00:00 2001 From: Vatan Aksoy Tezer Date: Tue, 21 Sep 2021 16:48:27 +0300 Subject: [PATCH 07/16] Minor formatting --- controller_manager/src/controller_manager.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index aff0debaae..14dbc2713a 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -897,7 +897,7 @@ void ControllerManager::start_controllers() if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { RCLCPP_ERROR( - get_logger(), "After activating, controller %s is in state %s, expected Active", + get_logger(), "After activating, controller '%s' is in state '%s', expected Active", controller->get_node()->get_name(), new_state.label().c_str()); } } From c7231ba2b10abc60e630936147982cf64f2e7582 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Thu, 24 Feb 2022 22:31:47 +0100 Subject: [PATCH 08/16] Fixes of controller tests. --- .../test/test_controller/test_controller.cpp | 29 +++---------------- .../test/test_load_controller.cpp | 4 ++- 2 files changed, 7 insertions(+), 26 deletions(-) diff --git a/controller_manager/test/test_controller/test_controller.cpp b/controller_manager/test/test_controller/test_controller.cpp index 6b922775d2..2b8b8bc262 100644 --- a/controller_manager/test/test_controller/test_controller.cpp +++ b/controller_manager/test/test_controller/test_controller.cpp @@ -30,8 +30,8 @@ TestController::TestController() controller_interface::InterfaceConfiguration TestController::command_interface_configuration() const { if ( - lifecycle_state_.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || - lifecycle_state_.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || + get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { return cmd_iface_cfg_; } @@ -45,8 +45,8 @@ controller_interface::InterfaceConfiguration TestController::command_interface_c controller_interface::InterfaceConfiguration TestController::state_interface_configuration() const { if ( - lifecycle_state_.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || - lifecycle_state_.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || + get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { return state_iface_cfg_; } @@ -85,27 +85,6 @@ CallbackReturn TestController::on_cleanup(const rclcpp_lifecycle::State & /*prev return CallbackReturn::SUCCESS; } -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -TestController::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) -{ - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; -} - -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -TestController::on_cleanup(const rclcpp_lifecycle::State & /*previous_state*/) -{ - if (simulate_cleanup_failure) - { - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; - } - - if (cleanup_calls) - { - (*cleanup_calls)++; - } - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; -} - void TestController::set_command_interface_configuration( const controller_interface::InterfaceConfiguration & cfg) { diff --git a/controller_manager/test/test_load_controller.cpp b/controller_manager/test/test_load_controller.cpp index f714bd1d01..b97f2e2b0e 100644 --- a/controller_manager/test/test_load_controller.cpp +++ b/controller_manager/test/test_load_controller.cpp @@ -173,7 +173,9 @@ TEST_F(TestLoadedController, can_not_start_finalized_controller) lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if->get_state().id()); // Shutdown controller on purpose for testing - ASSERT_EQ(controller_if->shutdown().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED); + ASSERT_EQ( + controller_if->get_node()->shutdown().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED); // Start controller start_test_controller(std::future_status::ready, controller_interface::return_type::ERROR); From c6b567cf18a60b49db275c1eb1038bf4aef87c37 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Thu, 24 Feb 2022 22:44:38 +0100 Subject: [PATCH 09/16] Add custom 'configure' to controller interface to get 'update_rate' parameter. --- .../controller_interface.hpp | 12 ++++++++-- .../src/controller_interface.cpp | 22 ++++++++++++++----- controller_manager/src/controller_manager.cpp | 4 ++-- 3 files changed, 29 insertions(+), 9 deletions(-) diff --git a/controller_interface/include/controller_interface/controller_interface.hpp b/controller_interface/include/controller_interface/controller_interface.hpp index 9f7a42a7be..7c5e5ee91d 100644 --- a/controller_interface/include/controller_interface/controller_interface.hpp +++ b/controller_interface/include/controller_interface/controller_interface.hpp @@ -79,10 +79,18 @@ class ControllerInterface : public rclcpp_lifecycle::node_interfaces::LifecycleN void release_interfaces(); CONTROLLER_INTERFACE_PUBLIC - virtual LifecycleNodeInterface::CallbackReturn on_init() = 0; + virtual return_type init(const std::string & controller_name); + /// Custom configure method to read additional parameters for controller-nodes + /* + * Override default implementation for configure of LifecycleNode to get 'update_rate' parameter. + */ CONTROLLER_INTERFACE_PUBLIC - virtual return_type init(const std::string & controller_name); + const rclcpp_lifecycle::State & configure(); + + /// Extending interface with initialization method which is individual for each controller + CONTROLLER_INTERFACE_PUBLIC + virtual LifecycleNodeInterface::CallbackReturn on_init() = 0; CONTROLLER_INTERFACE_PUBLIC virtual return_type update(const rclcpp::Time & time, const rclcpp::Duration & period) = 0; diff --git a/controller_interface/src/controller_interface.cpp b/controller_interface/src/controller_interface.cpp index dd6d4894fe..347f49bfb3 100644 --- a/controller_interface/src/controller_interface.cpp +++ b/controller_interface/src/controller_interface.cpp @@ -30,6 +30,16 @@ return_type ControllerInterface::init(const std::string & controller_name) .allow_undeclared_parameters(true) .automatically_declare_parameters_from_overrides(true)); + try + { + node_->declare_parameter("update_rate", 0); + } + catch (const std::exception & e) + { + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return return_type::ERROR; + } + switch (on_init()) { case LifecycleNodeInterface::CallbackReturn::SUCCESS: @@ -57,14 +67,16 @@ return_type ControllerInterface::init(const std::string & controller_name) node_->register_on_error(std::bind(&ControllerInterface::on_error, this, std::placeholders::_1)); - if (node_->has_parameter("update_rate")) - { - update_rate_ = node_->get_parameter("update_rate").as_int(); - } - return return_type::OK; } +const rclcpp_lifecycle::State & ControllerInterface::configure() +{ + update_rate_ = node_->get_parameter("update_rate").as_int(); + + return node_->configure(); +} + void ControllerInterface::assign_interfaces( std::vector && command_interfaces, std::vector && state_interfaces) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 14dbc2713a..12757a1746 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -381,7 +381,7 @@ controller_interface::return_type ControllerManager::configure_controller( } } - new_state = controller->get_node()->configure(); + new_state = controller->configure(); if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { RCLCPP_ERROR( @@ -1355,7 +1355,7 @@ controller_interface::return_type ControllerManager::update( // https://github.com/ros-controls/ros2_control/issues/153 if (is_controller_active(*loaded_controller.c)) { - auto controller_update_rate = loaded_controller.c->get_update_rate(); + const auto controller_update_rate = loaded_controller.c->get_update_rate(); bool controller_go = controller_update_rate == 0 || ((update_loop_counter_ % controller_update_rate) == 0); From d3ec3ef5d7ebccd1c44ec008b1c3de0a11b566b9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Thu, 24 Feb 2022 23:01:37 +0100 Subject: [PATCH 10/16] Revert controller reload (cleanup calls test). --- .../test/test_controller_manager_srvs.cpp | 38 ++++++++----------- 1 file changed, 16 insertions(+), 22 deletions(-) diff --git a/controller_manager/test/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp index 6187044254..7cd73ab2e3 100644 --- a/controller_manager/test/test_controller_manager_srvs.cpp +++ b/controller_manager/test/test_controller_manager_srvs.cpp @@ -195,21 +195,15 @@ TEST_F(TestControllerManagerSrvs, reload_controller_libraries_srv) auto request = std::make_shared(); - // Create a lambda to store the cleanup state change - bool cleanup_called = false; - auto set_cleanup_called = [&](const rclcpp_lifecycle::State &) { - cleanup_called = true; - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; - }; - // Reload with no controllers running request->force_kill = false; auto result = call_service_and_wait(*client, request, srv_executor); ASSERT_TRUE(result->ok); // Add a controller, but unconfigured - auto test_controller = cm_->load_controller( - test_controller::TEST_CONTROLLER_NAME, test_controller::TEST_CONTROLLER_CLASS_NAME); + std::shared_ptr test_controller = + std::dynamic_pointer_cast(cm_->load_controller( + test_controller::TEST_CONTROLLER_NAME, test_controller::TEST_CONTROLLER_CLASS_NAME)); // weak_ptr so the only controller shared_ptr instance is owned by the controller_manager and // can be completely destroyed before reloading the library @@ -220,21 +214,21 @@ TEST_F(TestControllerManagerSrvs, reload_controller_libraries_srv) ASSERT_GT(test_controller.use_count(), 1) << "Controller manager should have have a copy of this shared ptr"; - cleanup_called = false; - test_controller->get_node()->register_on_cleanup(set_cleanup_called); + size_t cleanup_calls = 0; + test_controller->cleanup_calls = &cleanup_calls; test_controller.reset(); // destroy our copy of the controller request->force_kill = false; result = call_service_and_wait(*client, request, srv_executor, true); ASSERT_TRUE(result->ok); - ASSERT_TRUE(cleanup_called); + ASSERT_EQ(cleanup_calls, 1u); ASSERT_EQ(test_controller.use_count(), 0) << "No more references to the controller after reloading."; test_controller.reset(); // Add a controller, but inactive - test_controller = cm_->load_controller( - test_controller::TEST_CONTROLLER_NAME, test_controller::TEST_CONTROLLER_CLASS_NAME); + test_controller = std::dynamic_pointer_cast(cm_->load_controller( + test_controller::TEST_CONTROLLER_NAME, test_controller::TEST_CONTROLLER_CLASS_NAME)); test_controller_weak = test_controller; cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME); @@ -242,20 +236,20 @@ TEST_F(TestControllerManagerSrvs, reload_controller_libraries_srv) ASSERT_GT(test_controller.use_count(), 1) << "Controller manager should have have a copy of this shared ptr"; - cleanup_called = false; - test_controller->get_node()->register_on_cleanup(set_cleanup_called); + cleanup_calls = 0; + test_controller->cleanup_calls = &cleanup_calls; test_controller.reset(); // destroy our copy of the controller request->force_kill = false; result = call_service_and_wait(*client, request, srv_executor, true); ASSERT_TRUE(result->ok); - ASSERT_TRUE(cleanup_called); + ASSERT_EQ(cleanup_calls, 1u); ASSERT_EQ(test_controller.use_count(), 0) << "No more references to the controller after reloading."; test_controller.reset(); - test_controller = cm_->load_controller( - test_controller::TEST_CONTROLLER_NAME, test_controller::TEST_CONTROLLER_CLASS_NAME); + test_controller = std::dynamic_pointer_cast(cm_->load_controller( + test_controller::TEST_CONTROLLER_NAME, test_controller::TEST_CONTROLLER_CLASS_NAME)); test_controller_weak = test_controller; cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME); // Start Controller @@ -273,8 +267,8 @@ TEST_F(TestControllerManagerSrvs, reload_controller_libraries_srv) << "Controller manager should still have have a copy of " "this shared ptr, no unloading was performed"; - cleanup_called = false; - test_controller->get_node()->register_on_cleanup(set_cleanup_called); + cleanup_calls = 0; + test_controller->cleanup_calls = &cleanup_calls; test_controller.reset(); // destroy our copy of the controller // Force stop active controller @@ -284,7 +278,7 @@ TEST_F(TestControllerManagerSrvs, reload_controller_libraries_srv) ASSERT_EQ(test_controller_weak.use_count(), 0) << "No more references to the controller after reloading."; - ASSERT_TRUE(cleanup_called) + ASSERT_EQ(cleanup_calls, 1u) << "Controller should have been stopped and cleaned up with force_kill = true"; } From ef245ab807bff1eacdcd129c59dbe3bddcdeef09 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Thu, 24 Feb 2022 23:06:12 +0100 Subject: [PATCH 11/16] Cleanup is actually not called if in unconfigured state. --- controller_manager/test/test_controller_manager_srvs.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/controller_manager/test/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp index 7cd73ab2e3..9f54b50c7c 100644 --- a/controller_manager/test/test_controller_manager_srvs.cpp +++ b/controller_manager/test/test_controller_manager_srvs.cpp @@ -221,7 +221,8 @@ TEST_F(TestControllerManagerSrvs, reload_controller_libraries_srv) request->force_kill = false; result = call_service_and_wait(*client, request, srv_executor, true); ASSERT_TRUE(result->ok); - ASSERT_EQ(cleanup_calls, 1u); + // Cleanup is not called from UNCONFIGURED: https://design.ros2.org/articles/node_lifecycle.html + ASSERT_EQ(cleanup_calls, 0u); ASSERT_EQ(test_controller.use_count(), 0) << "No more references to the controller after reloading."; test_controller.reset(); From bb6d25bd1d44a428b13d4c0f0feb19fa6b0a8b66 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Thu, 24 Feb 2022 23:14:36 +0100 Subject: [PATCH 12/16] Can not call 'cleanup' from 'active' state. --- controller_manager/test/test_spawner_unspawner.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index 5f4305ef31..40164dcc5f 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -103,10 +103,11 @@ TEST_F(TestLoadController, spawner_test_type_in_param) // Try to spawn again, it should fail because already active EXPECT_NE(call_spawner("ctrl_1 -c test_controller_manager"), 0) << "Cannot configure from active"; ctrl_1.c->get_node()->deactivate(); - // We should be able to reconfigure and start a configured controller + // We should be able to reconfigure and activate a configured controller EXPECT_EQ(call_spawner("ctrl_1 -c test_controller_manager"), 0); + ctrl_1.c->get_node()->deactivate(); ctrl_1.c->get_node()->cleanup(); - // We should be able to reconfigure and start am unconfigured loaded controller + // We should be able to reconfigure and activate am unconfigured loaded controller EXPECT_EQ(call_spawner("ctrl_1 -c test_controller_manager"), 0); ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); From 02b6cb182449a766daeda63b9baff1318d7e671c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Fri, 25 Feb 2022 08:58:56 +0100 Subject: [PATCH 13/16] Disalbe external interfaces of LifecycleNode. --- controller_interface/src/controller_interface.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/controller_interface/src/controller_interface.cpp b/controller_interface/src/controller_interface.cpp index 347f49bfb3..a3cd59bded 100644 --- a/controller_interface/src/controller_interface.cpp +++ b/controller_interface/src/controller_interface.cpp @@ -26,9 +26,11 @@ namespace controller_interface return_type ControllerInterface::init(const std::string & controller_name) { node_ = std::make_shared( - controller_name, rclcpp::NodeOptions() - .allow_undeclared_parameters(true) - .automatically_declare_parameters_from_overrides(true)); + controller_name, + rclcpp::NodeOptions() + .allow_undeclared_parameters(true) + .automatically_declare_parameters_from_overrides(true), + false); try { From dc0c59afa8fa8691f975a7f54a2a5b9ad4fb9613 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Sun, 6 Mar 2022 14:18:15 +0100 Subject: [PATCH 14/16] Use 'auto_declare' for 'update_rate' parameter. --- controller_interface/src/controller_interface.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/controller_interface/src/controller_interface.cpp b/controller_interface/src/controller_interface.cpp index a3cd59bded..c9433ce538 100644 --- a/controller_interface/src/controller_interface.cpp +++ b/controller_interface/src/controller_interface.cpp @@ -34,7 +34,7 @@ return_type ControllerInterface::init(const std::string & controller_name) try { - node_->declare_parameter("update_rate", 0); + auto_declare("update_rate", 0); } catch (const std::exception & e) { @@ -49,7 +49,6 @@ return_type ControllerInterface::init(const std::string & controller_name) case LifecycleNodeInterface::CallbackReturn::ERROR: case LifecycleNodeInterface::CallbackReturn::FAILURE: return return_type::ERROR; - break; } node_->register_on_configure( From b5d0c8f2b113f0788cb96d2042fa1005f2023c9e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Mon, 7 Mar 2022 13:25:38 +0100 Subject: [PATCH 15/16] Fix error when rebasing. --- .../test/test_controller/test_controller.hpp | 8 -------- 1 file changed, 8 deletions(-) diff --git a/controller_manager/test/test_controller/test_controller.hpp b/controller_manager/test/test_controller/test_controller.hpp index 11d1e213df..4e4ec44d17 100644 --- a/controller_manager/test/test_controller/test_controller.hpp +++ b/controller_manager/test/test_controller/test_controller.hpp @@ -57,14 +57,6 @@ class TestController : public controller_interface::ControllerInterface CONTROLLER_MANAGER_PUBLIC CallbackReturn on_cleanup(const rclcpp_lifecycle::State & previous_state) override; - CONTROLLER_MANAGER_PUBLIC - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure( - const rclcpp_lifecycle::State & previous_state) override; - - CONTROLLER_MANAGER_PUBLIC - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_cleanup( - const rclcpp_lifecycle::State & previous_state) override; - CONTROLLER_MANAGER_PUBLIC void set_command_interface_configuration( const controller_interface::InterfaceConfiguration & cfg); From 2015f4ae8f389686b871025d63a4df5df11be734 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Fri, 11 Mar 2022 08:58:12 +0000 Subject: [PATCH 16/16] Apply suggestions from code review MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Denis Štogl --- .../include/controller_interface/controller_interface.hpp | 2 +- controller_interface/src/controller_interface.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/controller_interface/include/controller_interface/controller_interface.hpp b/controller_interface/include/controller_interface/controller_interface.hpp index 7c5e5ee91d..d88075c62c 100644 --- a/controller_interface/include/controller_interface/controller_interface.hpp +++ b/controller_interface/include/controller_interface/controller_interface.hpp @@ -83,7 +83,7 @@ class ControllerInterface : public rclcpp_lifecycle::node_interfaces::LifecycleN /// Custom configure method to read additional parameters for controller-nodes /* - * Override default implementation for configure of LifecycleNode to get 'update_rate' parameter. + * Override default implementation for configure of LifecycleNode to get parameters. */ CONTROLLER_INTERFACE_PUBLIC const rclcpp_lifecycle::State & configure(); diff --git a/controller_interface/src/controller_interface.cpp b/controller_interface/src/controller_interface.cpp index c9433ce538..d336b9e1a2 100644 --- a/controller_interface/src/controller_interface.cpp +++ b/controller_interface/src/controller_interface.cpp @@ -30,7 +30,7 @@ return_type ControllerInterface::init(const std::string & controller_name) rclcpp::NodeOptions() .allow_undeclared_parameters(true) .automatically_declare_parameters_from_overrides(true), - false); + false); // disable LifecycleNode service interfaces try {