diff --git a/controller_interface/include/controller_interface/controller_interface.hpp b/controller_interface/include/controller_interface/controller_interface.hpp index 0d58da4d0d..d88075c62c 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 { @@ -80,16 +79,30 @@ 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 parameters. + */ 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; CONTROLLER_INTERFACE_PUBLIC - std::shared_ptr get_node(); + std::shared_ptr get_node(); + + CONTROLLER_INTERFACE_PUBLIC + const rclcpp_lifecycle::State & get_state() const; + + CONTROLLER_INTERFACE_PUBLIC + unsigned int get_update_rate() const; /// Declare and initialize a parameter with a type. /** @@ -111,42 +124,10 @@ class ControllerInterface : public rclcpp_lifecycle::node_interfaces::LifecycleN } } - /** - * 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 node_; unsigned int update_rate_ = 0; }; diff --git a/controller_interface/src/controller_interface.cpp b/controller_interface/src/controller_interface.cpp index 703ef33c82..d336b9e1a2 100644 --- a/controller_interface/src/controller_interface.cpp +++ b/controller_interface/src/controller_interface.cpp @@ -20,148 +20,63 @@ #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)); + node_ = std::make_shared( + controller_name, + rclcpp::NodeOptions() + .allow_undeclared_parameters(true) + .automatically_declare_parameters_from_overrides(true), + false); // disable LifecycleNode service interfaces - return_type result = return_type::OK; - switch (on_init()) + try { - 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; + auto_declare("update_rate", 0); } - return result; -} - -const rclcpp_lifecycle::State & ControllerInterface::configure() -{ - if (lifecycle_state_.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) + catch (const std::exception & e) { - 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(); - } + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return return_type::ERROR; } - return lifecycle_state_; -} -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_)) + switch (on_init()) { 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 return_type::ERROR; } - 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_; + + node_->register_on_configure( + std::bind(&ControllerInterface::on_configure, this, std::placeholders::_1)); + + node_->register_on_cleanup( + std::bind(&ControllerInterface::on_cleanup, this, std::placeholders::_1)); + + node_->register_on_activate( + std::bind(&ControllerInterface::on_activate, this, std::placeholders::_1)); + + node_->register_on_deactivate( + std::bind(&ControllerInterface::on_deactivate, this, std::placeholders::_1)); + + node_->register_on_shutdown( + std::bind(&ControllerInterface::on_shutdown, this, std::placeholders::_1)); + + node_->register_on_error(std::bind(&ControllerInterface::on_error, this, std::placeholders::_1)); + + return return_type::OK; } -const rclcpp_lifecycle::State & ControllerInterface::shutdown() +const rclcpp_lifecycle::State & ControllerInterface::configure() { - switch (on_shutdown(lifecycle_state_)) - { - 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; - } - return lifecycle_state_; -} + update_rate_ = node_->get_parameter("update_rate").as_int(); -const rclcpp_lifecycle::State & ControllerInterface::get_state() const { return lifecycle_state_; } + return node_->configure(); +} void ControllerInterface::assign_interfaces( std::vector && command_interfaces, @@ -177,11 +92,16 @@ void ControllerInterface::release_interfaces() state_interfaces_.clear(); } -std::shared_ptr ControllerInterface::get_node() +const rclcpp_lifecycle::State & ControllerInterface::get_state() const +{ + return node_->get_current_state(); +} + +std::shared_ptr ControllerInterface::get_node() { if (!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_; } diff --git a/controller_interface/test/test_controller_with_options.hpp b/controller_interface/test/test_controller_with_options.hpp index 5ce95453d2..c025492d19 100644 --- a/controller_interface/test/test_controller_with_options.hpp +++ b/controller_interface/test/test_controller_with_options.hpp @@ -44,14 +44,11 @@ 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); + 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: diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index d42aef00f0..12757a1746 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; } @@ -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_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. @@ -371,7 +371,7 @@ controller_interface::return_type ControllerManager::configure_controller( { RCLCPP_DEBUG( get_logger(), "Controller '%s' is cleaned-up before configuring", controller_name.c_str()); - new_state = controller->cleanup(); + new_state = controller->get_node()->cleanup(); if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) { RCLCPP_ERROR( @@ -724,7 +724,7 @@ controller_interface::ControllerInterfaceSharedPtr ControllerManager::add_contro controller.info.name.c_str()); controller.c->get_node()->set_parameter(use_sim_time); } - executor_->add_node(controller.c->get_node()); + 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->deactivate(); + const auto new_state = controller->get_node()->deactivate(); controller->release_interfaces(); if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { @@ -893,7 +893,7 @@ void ControllerManager::start_controllers() } controller->assign_interfaces(std::move(command_loans), std::move(state_loans)); - const auto new_state = controller->activate(); + const auto new_state = controller->get_node()->activate(); if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { 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); diff --git a/controller_manager/test/test_controller/test_controller.cpp b/controller_manager/test/test_controller/test_controller.cpp index 2cb094c6df..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_; } 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(); 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); diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index a29a68b045..40164dcc5f 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -102,11 +102,12 @@ 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->deactivate(); - // We should be able to reconfigure and start a configured controller + ctrl_1.c->get_node()->deactivate(); + // 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->cleanup(); - // We should be able to reconfigure and start am unconfigured loaded controller + ctrl_1.c->get_node()->deactivate(); + ctrl_1.c->get_node()->cleanup(); + // 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);