diff --git a/controller_manager/controller_manager/controller_manager_services.py b/controller_manager/controller_manager/controller_manager_services.py index 7b0f726d68..dadb4d83ef 100644 --- a/controller_manager/controller_manager/controller_manager_services.py +++ b/controller_manager/controller_manager/controller_manager_services.py @@ -323,17 +323,4 @@ def set_controller_parameters_from_param_file( node, controller_manager_name, controller_name, "type", controller_type ): return False - - fallback_controllers = get_parameter_from_param_file( - controller_name, spawner_namespace, parameter_file, "fallback_controllers" - ) - if fallback_controllers: - if not set_controller_parameters( - node, - controller_manager_name, - controller_name, - "fallback_controllers", - fallback_controllers, - ): - return False return True diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index 118a435f3b..e40d52aa4d 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/controller_manager/controller_manager/spawner.py @@ -26,6 +26,7 @@ load_controller, switch_controllers, unload_controller, + set_controller_parameters, set_controller_parameters_from_param_file, bcolors, ) @@ -33,10 +34,6 @@ import rclpy from rclpy.node import Node -<<<<<<< HEAD -from rclpy.parameter import get_parameter_value -======= ->>>>>>> 0631e3e (Refactor spawner to be able to reuse code for ros2controlcli (#1661)) from rclpy.signals import SignalHandlerOptions @@ -160,10 +157,6 @@ def main(args=None): try: for controller_name in controller_names: -<<<<<<< HEAD -======= - ->>>>>>> 0631e3e (Refactor spawner to be able to reuse code for ros2controlcli (#1661)) if is_controller_loaded( node, controller_manager_name, controller_name, controller_manager_timeout ): @@ -173,77 +166,15 @@ def main(args=None): + bcolors.ENDC ) else: -<<<<<<< HEAD - controller_type = ( - args.controller_type - if param_file is None - else get_parameter_from_param_file( - controller_name, spawner_namespace, param_file, "type" - ) - ) - if controller_type: - parameter = Parameter() - parameter.name = controller_name + ".type" - parameter.value = get_parameter_value(string_value=controller_type) - - response = call_set_parameters( - node=node, node_name=controller_manager_name, parameters=[parameter] - ) - assert len(response.results) == 1 - result = response.results[0] - if result.successful: - node.get_logger().info( - bcolors.OKCYAN - + 'Set controller type to "' - + controller_type - + '" for ' - + bcolors.BOLD - + controller_name - + bcolors.ENDC - ) - else: - node.get_logger().fatal( - bcolors.FAIL - + 'Could not set controller type to "' - + controller_type - + '" for ' - + bcolors.BOLD - + controller_name - + bcolors.ENDC - ) + if args.controller_type: + if not set_controller_parameters( + node, + controller_manager_name, + controller_name, + "type", + args.controller_type, + ): return 1 - - if param_file: - parameter = Parameter() - parameter.name = controller_name + ".params_file" - parameter.value = get_parameter_value(string_value=param_file) - - response = call_set_parameters( - node=node, node_name=controller_manager_name, parameters=[parameter] - ) - assert len(response.results) == 1 - result = response.results[0] - if result.successful: - node.get_logger().info( - bcolors.OKCYAN - + 'Set controller params file to "' - + param_file - + '" for ' - + bcolors.BOLD - + controller_name - + bcolors.ENDC - ) - else: - node.get_logger().fatal( - bcolors.FAIL - + 'Could not set controller params file to "' - + param_file - + '" for ' - + bcolors.BOLD - + controller_name - + bcolors.ENDC - ) -======= if param_file: if not set_controller_parameters_from_param_file( node, @@ -252,7 +183,6 @@ def main(args=None): param_file, spawner_namespace, ): ->>>>>>> 0631e3e (Refactor spawner to be able to reuse code for ros2controlcli (#1661)) return 1 ret = load_controller(node, controller_manager_name, controller_name) diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index 180d76e6b2..06503dc7a7 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -283,57 +283,6 @@ TEST_F(TestLoadController, unload_on_kill) ASSERT_EQ(cm_->get_loaded_controllers().size(), 0ul); } -<<<<<<< HEAD -======= -TEST_F(TestLoadController, spawner_test_fallback_controllers) -{ - const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") + - "/test/test_controller_spawner_with_fallback_controllers.yaml"; - - cm_->set_parameter(rclcpp::Parameter("ctrl_1.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); - cm_->set_parameter(rclcpp::Parameter("ctrl_2.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); - cm_->set_parameter(rclcpp::Parameter("ctrl_3.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); - - ControllerManagerRunner cm_runner(this); - EXPECT_EQ(call_spawner("ctrl_1 -c test_controller_manager --load-only -p " + test_file_path), 0); - - ASSERT_EQ(cm_->get_loaded_controllers().size(), 1ul); - { - 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_TRUE(ctrl_1.info.fallback_controllers_names.empty()); - ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); - } - - // Try to spawn now the controller with fallback controllers inside the yaml - EXPECT_EQ( - call_spawner("ctrl_2 ctrl_3 -c test_controller_manager --load-only -p " + test_file_path), 0); - - ASSERT_EQ(cm_->get_loaded_controllers().size(), 3ul); - { - 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_TRUE(ctrl_1.info.fallback_controllers_names.empty()); - ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); - - auto ctrl_2 = cm_->get_loaded_controllers()[1]; - ASSERT_EQ(ctrl_2.info.name, "ctrl_2"); - ASSERT_EQ(ctrl_2.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); - ASSERT_THAT( - ctrl_2.info.fallback_controllers_names, testing::ElementsAre("ctrl_6", "ctrl_7", "ctrl_8")); - ASSERT_EQ(ctrl_2.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); - - auto ctrl_3 = cm_->get_loaded_controllers()[2]; - ASSERT_EQ(ctrl_3.info.name, "ctrl_3"); - ASSERT_EQ(ctrl_3.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); - ASSERT_THAT(ctrl_3.info.fallback_controllers_names, testing::ElementsAre("ctrl_9")); - ASSERT_EQ(ctrl_3.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); - } -} - ->>>>>>> 0631e3e (Refactor spawner to be able to reuse code for ros2controlcli (#1661)) TEST_F(TestLoadController, spawner_with_many_controllers) { std::stringstream ss;