diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index 28c11c6290..7084e249f8 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/controller_manager/controller_manager/spawner.py @@ -64,13 +64,15 @@ def has_service_names(node, node_name, node_namespace, service_names): return all(service in client_names for service in service_names) -def is_controller_loaded( +def get_loaded_controller_state( node, controller_manager, controller_name, service_timeout=0.0, call_timeout=10.0 ): + """Return the lifecycle state of the controller, or None if it is not loaded.""" controllers = list_controllers( node, controller_manager, service_timeout, call_timeout ).controller - return any(c.name == controller_name for c in controllers) + match = first_match(controllers, lambda c: c.name == controller_name) + return match.state if match else None def parse_args_advanced(args): @@ -448,13 +450,14 @@ def _on_shutdown_signal(signum, frame): for controller in controllers: controller_name = controller["name"] - if is_controller_loaded( + loaded_state = get_loaded_controller_state( node, controller_manager_name, controller_name, controller_manager_timeout, service_call_timeout, - ): + ) + if loaded_state is not None: logger.warning( bcolors.WARNING + "Controller already loaded, skipping load_controller" @@ -503,18 +506,23 @@ def _on_shutdown_signal(signum, frame): logger.info( bcolors.OKBLUE + "Loaded " + bcolors.BOLD + controller_name + bcolors.ENDC ) + loaded_state = "unconfigured" if not controller["load_only"]: - ret = configure_controller( - node, - controller_manager_name, - controller_name, - controller_manager_timeout, - service_call_timeout, - ) - if not ret.ok: - logger.error(bcolors.FAIL + "Failed to configure controller" + bcolors.ENDC) - return 1 + # configure_controller only accepts the unconfigured state + if loaded_state == "unconfigured": + ret = configure_controller( + node, + controller_manager_name, + controller_name, + controller_manager_timeout, + service_call_timeout, + ) + if not ret.ok: + logger.error( + bcolors.FAIL + "Failed to configure controller" + bcolors.ENDC + ) + return 1 if not controller["inactive"]: if activate_as_group: diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 32d3e5dfbf..d8c8e46d7e 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -1614,27 +1614,15 @@ controller_interface::return_type ControllerManager::configure_controller( } auto controller = found_it->c; - const auto & state = controller->get_lifecycle_state(); - if ( - state.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE || - state.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) + if (!is_controller_unconfigured(*controller)) { RCLCPP_ERROR( get_logger(), "Controller '%s' can not be configured from '%s' state.", - controller_name.c_str(), state.label().c_str()); + controller_name.c_str(), controller->get_lifecycle_state().label().c_str()); return controller_interface::return_type::ERROR; } - 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()); - if (cleanup_controller(*found_it) != controller_interface::return_type::OK) - { - return controller_interface::return_type::ERROR; - } - } - // For cases, when the controller ends up in the unconfigured state from any other state + // Remove any stale exported interfaces from a prior configure cycle before re-configuring. cleanup_controller_exported_interfaces(*found_it); try diff --git a/controller_manager/test/test_controller/test_controller.cpp b/controller_manager/test/test_controller/test_controller.cpp index c2dbbc54f6..f8fbaea806 100644 --- a/controller_manager/test/test_controller/test_controller.cpp +++ b/controller_manager/test/test_controller/test_controller.cpp @@ -211,11 +211,6 @@ CallbackReturn TestController::on_activate(const rclcpp_lifecycle::State & /*pre CallbackReturn TestController::on_cleanup(const rclcpp_lifecycle::State & /*previous_state*/) { verify_internal_lifecycle_id(get_lifecycle_id(), get_lifecycle_state().id()); - if (simulate_cleanup_failure) - { - return CallbackReturn::FAILURE; - } - if (cleanup_calls) { (*cleanup_calls)++; diff --git a/controller_manager/test/test_controller/test_controller.hpp b/controller_manager/test/test_controller/test_controller.hpp index f259021b54..c22fbe1791 100644 --- a/controller_manager/test/test_controller/test_controller.hpp +++ b/controller_manager/test/test_controller/test_controller.hpp @@ -70,7 +70,6 @@ class TestController : public controller_interface::ControllerInterface rclcpp::Service::SharedPtr service_; unsigned int internal_counter = 0; double activation_processing_time = 0.0; - bool simulate_cleanup_failure = false; // Variable where we store when shutdown was called, pointer because the controller // is usually destroyed after shutdown size_t * cleanup_calls = nullptr; diff --git a/controller_manager/test/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp index 7bd49797f9..6f1154a84e 100644 --- a/controller_manager/test/test_controller_manager_srvs.cpp +++ b/controller_manager/test/test_controller_manager_srvs.cpp @@ -629,6 +629,9 @@ TEST_F(TestControllerManagerSrvs, configure_controller_srv) rclcpp::Client::SharedPtr unload_client = srv_node->create_client( "test_controller_manager/unload_controller"); + rclcpp::Client::SharedPtr cleanup_client = + srv_node->create_client( + "test_controller_manager/cleanup_controller"); auto request = std::make_shared(); request->name = test_controller::TEST_CONTROLLER_NAME; @@ -651,9 +654,9 @@ TEST_F(TestControllerManagerSrvs, configure_controller_srv) lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_lifecycle_state().id()); - // call configure again and check the state + it shouldn't throw any exception + // configure_controller must reject any state other than UNCONFIGURED result = call_service_and_wait(*client, request, srv_executor, true); - ASSERT_TRUE(result->ok); + ASSERT_FALSE(result->ok) << "Controller configured from inactive state: " << request->name; EXPECT_EQ(1u, cm_->get_loaded_controllers().size()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, @@ -701,9 +704,9 @@ TEST_F(TestControllerManagerSrvs, configure_controller_srv) lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_chainable_controller->get_lifecycle_state().id()); - // call configure again and check the state + it shouldn't throw any exception + // configure_controller must reject any state other than UNCONFIGURED result = call_service_and_wait(*client, request, srv_executor, true); - ASSERT_TRUE(result->ok); + ASSERT_FALSE(result->ok) << "Controller configured from inactive state: " << request->name; EXPECT_EQ(2u, cm_->get_loaded_controllers().size()); EXPECT_EQ( test_chainable_controller::TEST_CONTROLLER_NAME, @@ -715,6 +718,15 @@ TEST_F(TestControllerManagerSrvs, configure_controller_srv) lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_chainable_controller->get_lifecycle_state().id()); + // Cleanup to UNCONFIGURED so the duplicate-interface tests below can attempt configure + auto cleanup_request = + std::make_shared(); + cleanup_request->name = test_chainable_controller::TEST_CONTROLLER_NAME; + ASSERT_TRUE(call_service_and_wait(*cleanup_client, cleanup_request, srv_executor, true)->ok); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_chainable_controller->get_lifecycle_state().id()); + // Now try to configure the chainable controller with duplicated command interfaces test_chainable_controller->set_command_interface_configuration(duplicated_chained_cmd_cfg); result = call_service_and_wait(*client, request, srv_executor, true); diff --git a/controller_manager/test/test_load_controller.cpp b/controller_manager/test/test_load_controller.cpp index b30e2e0e46..e7a4fe6b0e 100644 --- a/controller_manager/test/test_load_controller.cpp +++ b/controller_manager/test/test_load_controller.cpp @@ -221,56 +221,21 @@ TEST_P(TestLoadedControllerParametrized, can_not_start_finalized_controller) lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, controller_if->get_lifecycle_state().id()); } -TEST_P(TestLoadedControllerParametrized, inactive_controller_cannot_be_cleaned_up) +TEST_P(TestLoadedControllerParametrized, configure_controller_requires_unconfigured_state) { const auto test_param = GetParam(); EXPECT_EQ(cm_->configure_controller(CONTROLLER_NAME_1), controller_interface::return_type::OK); start_test_controller(test_param.strictness); - stop_test_controller(test_param.strictness); - ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_lifecycle_state().id()); - std::shared_ptr test_controller = - std::dynamic_pointer_cast(controller_if); - size_t cleanup_calls = 0; - test_controller->cleanup_calls = &cleanup_calls; - // Configure from inactive state: controller can no be cleaned-up - test_controller->simulate_cleanup_failure = true; + // configure_controller must reject any state other than UNCONFIGURED EXPECT_EQ(cm_->configure_controller(CONTROLLER_NAME_1), controller_interface::return_type::ERROR); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_lifecycle_state().id()); - EXPECT_EQ(0u, cleanup_calls); -} - -TEST_P(TestLoadedControllerParametrized, inactive_controller_cannot_be_configured) -{ - const auto test_param = GetParam(); - - EXPECT_EQ(cm_->configure_controller(CONTROLLER_NAME_1), controller_interface::return_type::OK); - - start_test_controller(test_param.strictness); - - stop_test_controller(test_param.strictness); - ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_lifecycle_state().id()); - - std::shared_ptr test_controller = - std::dynamic_pointer_cast(controller_if); - size_t cleanup_calls = 0; - test_controller->cleanup_calls = &cleanup_calls; - // Configure from inactive state - test_controller->simulate_cleanup_failure = false; - { - ControllerManagerRunner cm_runner(this); - EXPECT_EQ(cm_->configure_controller(CONTROLLER_NAME_1), controller_interface::return_type::OK); - } - ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_lifecycle_state().id()); - EXPECT_EQ(1u, cleanup_calls); } INSTANTIATE_TEST_SUITE_P( diff --git a/doc/migration.rst b/doc/migration.rst index 19dd80b3d7..ba0eb7e7a1 100644 --- a/doc/migration.rst +++ b/doc/migration.rst @@ -127,6 +127,13 @@ ChainableControllerInterface controller_manager ****************** +* ``configure_controller`` now performs a single-step lifecycle transition and + only accepts controllers in the ``unconfigured`` state (`#3196 + `__). Previously an + ``inactive`` controller was implicitly cleaned up and then reconfigured. To + reconfigure an ``inactive`` controller, call ``cleanup_controller`` first and + then ``configure_controller``. + hardware_interface ****************** diff --git a/doc/release_notes.rst b/doc/release_notes.rst index a180ab9156..8244b32568 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -24,6 +24,7 @@ controller_manager * Added new ``cleanup_controller`` service to the controller manager to allow cleaning up controllers from external clients. (`#2414 `__) * Removed forwarding of the controller manager's ros arguments to the controllers via NodeOptions. (`#3016 `__) * The ``spawner`` now forwards all the parameter files parsed to the spawner node to the spawned controllers. This would support ``allow_substs`` approach. (`#3136 `__) +* ``configure_controller`` no longer implicitly cleans up an ``inactive`` controller before configuring; it now strictly requires the ``unconfigured`` state and returns an error otherwise. (`#3196 `__) hardware_interface ****************** diff --git a/ros2controlcli/ros2controlcli/verb/load_controller.py b/ros2controlcli/ros2controlcli/verb/load_controller.py index 5e29058a31..d0a65625a8 100644 --- a/ros2controlcli/ros2controlcli/verb/load_controller.py +++ b/ros2controlcli/ros2controlcli/verb/load_controller.py @@ -54,7 +54,9 @@ def add_arguments(self, parser, cli_name): def main(self, *, args): with NodeStrategy(args).direct_node as node: controllers = list_controllers(node, args.controller_manager, 20.0).controller - if any(c.name == args.controller_name for c in controllers): + matched = next((c for c in controllers if c.name == args.controller_name), None) + loaded_state = matched.state if matched else None + if loaded_state is not None: print( f"{bcolors.WARNING}Controller : {args.controller_name} already loaded, skipping load_controller!{bcolors.ENDC}" ) @@ -86,18 +88,20 @@ def main(self, *, args): print( f"{bcolors.OKBLUE}Successfully loaded controller {args.controller_name}{bcolors.ENDC}" ) + loaded_state = "unconfigured" if args.set_state: - # we in any case configure the controller - response = configure_controller( - node, args.controller_manager, args.controller_name - ) - if not response.ok: - print( - f"{bcolors.FAIL}Error configuring controller : {args.controller_name}{bcolors.ENDC}" + # configure_controller only accepts the unconfigured state + if loaded_state == "unconfigured": + response = configure_controller( + node, args.controller_manager, args.controller_name ) - return 1 + if not response.ok: + print( + f"{bcolors.FAIL}Error configuring controller : {args.controller_name}{bcolors.ENDC}" + ) + return 1 if args.set_state == "active": response = switch_controllers(