From c1fe929a773ac630dabaefb46cb7a99500c26506 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 27 Aug 2024 18:51:54 +0200 Subject: [PATCH 01/10] added logic to support the param files with wildcard entries --- .../controller_manager_services.py | 37 ++++++++++++++----- 1 file changed, 27 insertions(+), 10 deletions(-) diff --git a/controller_manager/controller_manager/controller_manager_services.py b/controller_manager/controller_manager/controller_manager_services.py index a09be5d14b..2921656b9e 100644 --- a/controller_manager/controller_manager/controller_manager_services.py +++ b/controller_manager/controller_manager/controller_manager_services.py @@ -250,19 +250,36 @@ def get_parameter_from_param_file(controller_name, namespace, parameter_file, pa namespaced_controller = ( controller_name if namespace == "/" else f"{namespace}/{controller_name}" ) + WILDCARD_KEY = "/**" + ROS_PARAMS_KEY = "ros__parameters" parameters = yaml.safe_load(f) - if namespaced_controller in parameters: - value = parameters[namespaced_controller] - if not isinstance(value, dict) or "ros__parameters" not in value: + controller_param_dict = None + # check for the parameter in 'controller_name' or 'namespaced_controller' or '/**/namespaced_controller' or '/**/controller_name' + for key in [ + controller_name, + namespaced_controller, + f"{WILDCARD_KEY}/{controller_name}", + f"{WILDCARD_KEY}/{namespaced_controller}", + ]: + if key in parameters: + controller_param_dict = parameters[key] + break + if WILDCARD_KEY in parameters and key in parameters[WILDCARD_KEY]: + controller_param_dict = parameters[WILDCARD_KEY][key] + break + + if controller_param_dict: + if ( + not isinstance(controller_param_dict, dict) + or ROS_PARAMS_KEY not in controller_param_dict + ): raise RuntimeError( - f"YAML file : {parameter_file} is not a valid ROS parameter file for controller : {namespaced_controller}" + f"YAML file : {parameter_file} is not a valid ROS parameter file for controller node : {namespaced_controller}" ) - if parameter_name in parameters[namespaced_controller]["ros__parameters"]: - return parameters[namespaced_controller]["ros__parameters"][parameter_name] - else: - return None - else: - return None + if parameter_name in controller_param_dict[ROS_PARAMS_KEY]: + return controller_param_dict[ROS_PARAMS_KEY][parameter_name] + + return None def set_controller_parameters( From 1346b53627806894873f32106ed7e78bc4cf6f99 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 27 Aug 2024 19:19:49 +0200 Subject: [PATCH 02/10] rename controllers within the same yaml file to be differentiable --- .../test_controller_spawner_with_type.yaml | 29 +++++++++++---- .../test/test_spawner_unspawner.cpp | 37 +++++++++++-------- 2 files changed, 43 insertions(+), 23 deletions(-) diff --git a/controller_manager/test/test_controller_spawner_with_type.yaml b/controller_manager/test/test_controller_spawner_with_type.yaml index 892427bab7..087994bd23 100644 --- a/controller_manager/test/test_controller_spawner_with_type.yaml +++ b/controller_manager/test/test_controller_spawner_with_type.yaml @@ -3,25 +3,40 @@ ctrl_with_parameters_and_type: type: "controller_manager/test_controller" joint_names: ["joint0"] -chainable_ctrl_with_parameters_and_type: - ros__parameters: - type: "controller_manager/test_chainable_controller" - joint_names: ["joint1"] +/**: + chainable_ctrl_with_parameters_and_type: + ros__parameters: + type: "controller_manager/test_chainable_controller" + joint_names: ["joint1"] + + wildcard_ctrl_with_parameters_and_type: + ros__parameters: + type: "controller_manager/test_controller" + joint_names: ["joint1"] ctrl_with_parameters_and_no_type: ros__parameters: joint_names: ["joint2"] -/foo_namespace/ctrl_with_parameters_and_type: +/foo_namespace/ns_ctrl_with_parameters_and_type: ros__parameters: type: "controller_manager/test_controller" joint_names: ["joint1"] -/foo_namespace/chainable_ctrl_with_parameters_and_type: +/foo_namespace/ns_chainable_ctrl_with_parameters_and_type: + ros__parameters: + type: "controller_manager/test_chainable_controller" + joint_names: ["joint1"] + +/foo_namespace/ns_ctrl_with_parameters_and_no_type: + ros__parameters: + joint_names: ["joint2"] + +/**/wildcard_chainable_ctrl_with_parameters_and_type: ros__parameters: type: "controller_manager/test_chainable_controller" joint_names: ["joint1"] -/foo_namespace/ctrl_with_parameters_and_no_type: +/**/wildcard_ctrl_with_parameters_and_no_type: ros__parameters: joint_names: ["joint2"] diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index 0812d7ffa4..593a7aa15b 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -660,14 +660,14 @@ TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_type_in_params_file) // Provide controller type via the parsed file EXPECT_EQ( call_spawner( - "ctrl_with_parameters_and_type chainable_ctrl_with_parameters_and_type --load-only -c " + "ns_ctrl_with_parameters_and_type ns_chainable_ctrl_with_parameters_and_type --load-only -c " "test_controller_manager --controller-manager-timeout 1.0 -p " + test_file_path), 256) << "Should fail without the namespacing it"; EXPECT_EQ( call_spawner( - "ctrl_with_parameters_and_type chainable_ctrl_with_parameters_and_type --load-only -c " + "ns_ctrl_with_parameters_and_type ns_chainable_ctrl_with_parameters_and_type --load-only -c " "test_controller_manager -p " + test_file_path + " --ros-args -r __ns:=/foo_namespace"), 0); @@ -675,7 +675,7 @@ TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_type_in_params_file) ASSERT_EQ(cm_->get_loaded_controllers().size(), 2ul); auto ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[0]; - ASSERT_EQ(ctrl_with_parameters_and_type.info.name, "ctrl_with_parameters_and_type"); + ASSERT_EQ(ctrl_with_parameters_and_type.info.name, "ns_ctrl_with_parameters_and_type"); ASSERT_EQ(ctrl_with_parameters_and_type.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_EQ( ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), @@ -683,7 +683,7 @@ TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_type_in_params_file) auto chain_ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[1]; ASSERT_EQ( - chain_ctrl_with_parameters_and_type.info.name, "chainable_ctrl_with_parameters_and_type"); + chain_ctrl_with_parameters_and_type.info.name, "ns_chainable_ctrl_with_parameters_and_type"); ASSERT_EQ( chain_ctrl_with_parameters_and_type.info.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); @@ -693,7 +693,7 @@ TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_type_in_params_file) EXPECT_EQ( call_spawner( - "ctrl_with_parameters_and_no_type -c test_controller_manager -p " + test_file_path + + "ns_ctrl_with_parameters_and_no_type -c test_controller_manager -p " + test_file_path + " --ros-args -r __ns:=/foo_namespace"), 256) << "Should fail as no type is defined!"; @@ -701,13 +701,13 @@ TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_type_in_params_file) ASSERT_EQ(cm_->get_loaded_controllers().size(), 2ul); auto ctrl_1 = cm_->get_loaded_controllers()[0]; - ASSERT_EQ(ctrl_1.info.name, "ctrl_with_parameters_and_type"); + ASSERT_EQ(ctrl_1.info.name, "ns_ctrl_with_parameters_and_type"); ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_EQ( ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); auto ctrl_2 = cm_->get_loaded_controllers()[1]; - ASSERT_EQ(ctrl_2.info.name, "chainable_ctrl_with_parameters_and_type"); + ASSERT_EQ(ctrl_2.info.name, "ns_chainable_ctrl_with_parameters_and_type"); ASSERT_EQ(ctrl_2.info.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_EQ( ctrl_2.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); @@ -723,28 +723,28 @@ TEST_F( // Provide controller type via the parsed file EXPECT_EQ( call_spawner( - "ctrl_with_parameters_and_type chainable_ctrl_with_parameters_and_type --load-only -c " + "ns_ctrl_with_parameters_and_type ns_chainable_ctrl_with_parameters_and_type --load-only -c " "test_controller_manager --controller-manager-timeout 1.0 -p " + test_file_path), 256) << "Should fail without the namespacing it"; EXPECT_EQ( call_spawner( - "ctrl_with_parameters_and_type chainable_ctrl_with_parameters_and_type --load-only -c " + "ns_ctrl_with_parameters_and_type ns_chainable_ctrl_with_parameters_and_type --load-only -c " "test_controller_manager --namespace foo_namespace --controller-manager-timeout 1.0 -p " + test_file_path + " --ros-args -r __ns:=/random_namespace"), 256) << "Should fail when parsed namespace through both way with different namespaces"; EXPECT_EQ( call_spawner( - "ctrl_with_parameters_and_type chainable_ctrl_with_parameters_and_type --load-only -c " + "ns_ctrl_with_parameters_and_type ns_chainable_ctrl_with_parameters_and_type --load-only -c " "test_controller_manager --namespace foo_namespace --controller-manager-timeout 1.0 -p" + test_file_path + " --ros-args -r __ns:=/foo_namespace"), 256) << "Should fail when parsed namespace through both ways even with same namespacing name"; EXPECT_EQ( call_spawner( - "ctrl_with_parameters_and_type chainable_ctrl_with_parameters_and_type --load-only -c " + "ns_ctrl_with_parameters_and_type ns_chainable_ctrl_with_parameters_and_type --load-only -c " "test_controller_manager --namespace foo_namespace -p " + test_file_path), 0) @@ -753,7 +753,7 @@ TEST_F( ASSERT_EQ(cm_->get_loaded_controllers().size(), 2ul); auto ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[0]; - ASSERT_EQ(ctrl_with_parameters_and_type.info.name, "ctrl_with_parameters_and_type"); + ASSERT_EQ(ctrl_with_parameters_and_type.info.name, "ns_ctrl_with_parameters_and_type"); ASSERT_EQ(ctrl_with_parameters_and_type.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_EQ( ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), @@ -761,7 +761,7 @@ TEST_F( auto chain_ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[1]; ASSERT_EQ( - chain_ctrl_with_parameters_and_type.info.name, "chainable_ctrl_with_parameters_and_type"); + chain_ctrl_with_parameters_and_type.info.name, "ns_chainable_ctrl_with_parameters_and_type"); ASSERT_EQ( chain_ctrl_with_parameters_and_type.info.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); @@ -771,7 +771,8 @@ TEST_F( EXPECT_EQ( call_spawner( - "ctrl_with_parameters_and_no_type -c test_controller_manager --namespace foo_namespace -p " + + "ns_ctrl_with_parameters_and_no_type -c test_controller_manager --namespace foo_namespace " + "-p " + test_file_path), 256) << "Should fail as no type is defined!"; @@ -779,13 +780,17 @@ TEST_F( ASSERT_EQ(cm_->get_loaded_controllers().size(), 2ul); auto ctrl_1 = cm_->get_loaded_controllers()[0]; - ASSERT_EQ(ctrl_1.info.name, "ctrl_with_parameters_and_type"); + ASSERT_EQ(ctrl_1.info.name, "ns_ctrl_with_parameters_and_type"); ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_EQ( ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); auto ctrl_2 = cm_->get_loaded_controllers()[1]; - ASSERT_EQ(ctrl_2.info.name, "chainable_ctrl_with_parameters_and_type"); + ASSERT_EQ(ctrl_2.info.name, "ns_chainable_ctrl_with_parameters_and_type"); + ASSERT_EQ(ctrl_2.info.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ( + ctrl_2.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); +} ASSERT_EQ(ctrl_2.info.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_EQ( ctrl_2.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); From 0491a02f9b315e42cc9217b323e409c79e095361 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 27 Aug 2024 19:20:16 +0200 Subject: [PATCH 03/10] Add test case for controllers with wildcard entries in the params_file --- .../test/test_spawner_unspawner.cpp | 61 +++++++++++++++++++ 1 file changed, 61 insertions(+) diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index 593a7aa15b..127ec513df 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -791,6 +791,67 @@ TEST_F( ASSERT_EQ( ctrl_2.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); } + +TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_with_wildcard_entries_in_params_file) +{ + const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") + + "/test/test_controller_spawner_with_type.yaml"; + + ControllerManagerRunner cm_runner(this); + // Provide controller type via the parsed file + EXPECT_EQ( + call_spawner( + "wildcard_ctrl_with_parameters_and_type wildcard_chainable_ctrl_with_parameters_and_type " + "--load-only -c " + "test_controller_manager --controller-manager-timeout 1.0 -p " + + test_file_path), + 256) + << "Should fail without the namespacing it due to timeout but can find the parameters"; + EXPECT_EQ( + call_spawner( + "wildcard_ctrl_with_parameters_and_type wildcard_chainable_ctrl_with_parameters_and_type " + "--load-only -c " + "test_controller_manager -p " + + test_file_path + " --ros-args -r __ns:=/foo_namespace"), + 0); + + ASSERT_EQ(cm_->get_loaded_controllers().size(), 2ul); + + auto ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[0]; + ASSERT_EQ(ctrl_with_parameters_and_type.info.name, "wildcard_ctrl_with_parameters_and_type"); + ASSERT_EQ(ctrl_with_parameters_and_type.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ( + ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + + auto chain_ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[1]; + ASSERT_EQ( + chain_ctrl_with_parameters_and_type.info.name, + "wildcard_chainable_ctrl_with_parameters_and_type"); + ASSERT_EQ( + chain_ctrl_with_parameters_and_type.info.type, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ( + chain_ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + + EXPECT_EQ( + call_spawner( + "wildcard_ctrl_with_parameters_and_no_type -c test_controller_manager -p " + test_file_path + + " --ros-args -r __ns:=/foo_namespace"), + 256) + << "Should fail as no type is defined!"; + // Will still be same as the current call will fail + ASSERT_EQ(cm_->get_loaded_controllers().size(), 2ul); + + auto ctrl_1 = cm_->get_loaded_controllers()[0]; + ASSERT_EQ(ctrl_1.info.name, "wildcard_ctrl_with_parameters_and_type"); + ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ( + ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + + auto ctrl_2 = cm_->get_loaded_controllers()[1]; + ASSERT_EQ(ctrl_2.info.name, "wildcard_chainable_ctrl_with_parameters_and_type"); ASSERT_EQ(ctrl_2.info.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_EQ( ctrl_2.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); From 8a9ae264f69d3a7eeba249bb9a11836c342010fb Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 27 Aug 2024 19:51:23 +0200 Subject: [PATCH 04/10] throw errors when trying to access controller info from a non namespaced or non wildcard entry param file --- .../controller_manager_services.py | 4 ++ .../test/test_spawner_unspawner.cpp | 42 +++++++++++++++++++ 2 files changed, 46 insertions(+) diff --git a/controller_manager/controller_manager/controller_manager_services.py b/controller_manager/controller_manager/controller_manager_services.py index 2921656b9e..b0c0d1a52b 100644 --- a/controller_manager/controller_manager/controller_manager_services.py +++ b/controller_manager/controller_manager/controller_manager_services.py @@ -262,6 +262,10 @@ def get_parameter_from_param_file(controller_name, namespace, parameter_file, pa f"{WILDCARD_KEY}/{namespaced_controller}", ]: if key in parameters: + if key == controller_name and controller_name != namespaced_controller: + raise RuntimeError( + f"YAML file : {parameter_file} is not a valid ROS parameter file for controller node : {namespaced_controller}" + ) controller_param_dict = parameters[key] break if WILDCARD_KEY in parameters and key in parameters[WILDCARD_KEY]: diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index 127ec513df..6b0aef999b 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -856,3 +856,45 @@ TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_with_wildcard_entries_in ASSERT_EQ( ctrl_2.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); } + +TEST_F( + TestLoadControllerWithNamespacedCM, + spawner_test_fail_namespaced_controllers_with_non_wildcard_entries) +{ + const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") + + "/test/test_controller_spawner_with_type.yaml"; + + ControllerManagerRunner cm_runner(this); + // Provide controller type via the parsed file + EXPECT_EQ( + call_spawner( + "ctrl_with_parameters_and_type --load-only -c " + "test_controller_manager --controller-manager-timeout 1.0 -p " + + test_file_path), + 256) + << "Should fail without the namespacing it"; + EXPECT_EQ( + call_spawner( + "ctrl_with_parameters_and_type --load-only -c " + "test_controller_manager --namespace foo_namespace -p " + + test_file_path), + 256) + << "Should fail even namespacing it as ctrl_with_parameters_and_type is not a wildcard entry"; + EXPECT_EQ( + call_spawner( + "chainable_ctrl_with_parameters_and_type --load-only -c " + "test_controller_manager --namespace foo_namespace -p " + + test_file_path), + 0) + << "Should work as chainable_ctrl_with_parameters_and_type is a wildcard entry"; + + ASSERT_EQ(cm_->get_loaded_controllers().size(), 1ul); + + auto ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[0]; + ASSERT_EQ(ctrl_with_parameters_and_type.info.name, "chainable_ctrl_with_parameters_and_type"); + ASSERT_EQ( + ctrl_with_parameters_and_type.info.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ( + ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); +} From 021281293616a698267531530592718845279d0f Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 27 Aug 2024 23:48:28 +0200 Subject: [PATCH 05/10] add better error message logging and cleanup --- .../controller_manager_services.py | 36 +++++++++++-------- 1 file changed, 21 insertions(+), 15 deletions(-) diff --git a/controller_manager/controller_manager/controller_manager_services.py b/controller_manager/controller_manager/controller_manager_services.py index b0c0d1a52b..fbaa97f634 100644 --- a/controller_manager/controller_manager/controller_manager_services.py +++ b/controller_manager/controller_manager/controller_manager_services.py @@ -245,7 +245,9 @@ def unload_controller(node, controller_manager_name, controller_name, service_ti ) -def get_parameter_from_param_file(controller_name, namespace, parameter_file, parameter_name): +def get_parameter_from_param_file( + node, controller_name, namespace, parameter_file, parameter_name +): with open(parameter_file) as f: namespaced_controller = ( controller_name if namespace == "/" else f"{namespace}/{controller_name}" @@ -263,25 +265,29 @@ def get_parameter_from_param_file(controller_name, namespace, parameter_file, pa ]: if key in parameters: if key == controller_name and controller_name != namespaced_controller: - raise RuntimeError( - f"YAML file : {parameter_file} is not a valid ROS parameter file for controller node : {namespaced_controller}" + node.get_logger().fatal( + f"{bcolors.FAIL}Missing namespace : {namespace} or wildcard in parameter file for controller : {controller_name}{bcolors.ENDC}" ) + break controller_param_dict = parameters[key] break if WILDCARD_KEY in parameters and key in parameters[WILDCARD_KEY]: controller_param_dict = parameters[WILDCARD_KEY][key] break - if controller_param_dict: - if ( - not isinstance(controller_param_dict, dict) - or ROS_PARAMS_KEY not in controller_param_dict - ): - raise RuntimeError( - f"YAML file : {parameter_file} is not a valid ROS parameter file for controller node : {namespaced_controller}" - ) - if parameter_name in controller_param_dict[ROS_PARAMS_KEY]: - return controller_param_dict[ROS_PARAMS_KEY][parameter_name] + if controller_param_dict is None: + node.get_logger().fatal( + f"{bcolors.FAIL}Controller : {namespaced_controller} parameters not found in parameter file : {parameter_file}{bcolors.ENDC}" + ) + if ( + not isinstance(controller_param_dict, dict) + or ROS_PARAMS_KEY not in controller_param_dict + ): + raise RuntimeError( + f"YAML file : {parameter_file} is not a valid ROS parameter file for controller node : {namespaced_controller}" + ) + if parameter_name in controller_param_dict[ROS_PARAMS_KEY]: + return controller_param_dict[ROS_PARAMS_KEY][parameter_name] return None @@ -337,7 +343,7 @@ def set_controller_parameters_from_param_file( ) controller_type = get_parameter_from_param_file( - controller_name, spawner_namespace, parameter_file, "type" + node, controller_name, spawner_namespace, parameter_file, "type" ) if controller_type: if not set_controller_parameters( @@ -346,7 +352,7 @@ def set_controller_parameters_from_param_file( return False fallback_controllers = get_parameter_from_param_file( - controller_name, spawner_namespace, parameter_file, "fallback_controllers" + node, controller_name, spawner_namespace, parameter_file, "fallback_controllers" ) if fallback_controllers: if not set_controller_parameters( From 90b353de4274ea364293ecfe622c26aaa2f540c1 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 2 Sep 2024 18:53:05 +0200 Subject: [PATCH 06/10] Minor fix to also find the parameters from global parameter namespace --- .../controller_manager/controller_manager_services.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/controller_manager/controller_manager/controller_manager_services.py b/controller_manager/controller_manager/controller_manager_services.py index fbaa97f634..2aa44da3ab 100644 --- a/controller_manager/controller_manager/controller_manager_services.py +++ b/controller_manager/controller_manager/controller_manager_services.py @@ -250,7 +250,7 @@ def get_parameter_from_param_file( ): with open(parameter_file) as f: namespaced_controller = ( - controller_name if namespace == "/" else f"{namespace}/{controller_name}" + f"/{controller_name}" if namespace == "/" else f"{namespace}/{controller_name}" ) WILDCARD_KEY = "/**" ROS_PARAMS_KEY = "ros__parameters" @@ -261,10 +261,10 @@ def get_parameter_from_param_file( controller_name, namespaced_controller, f"{WILDCARD_KEY}/{controller_name}", - f"{WILDCARD_KEY}/{namespaced_controller}", + f"{WILDCARD_KEY}{namespaced_controller}", ]: if key in parameters: - if key == controller_name and controller_name != namespaced_controller: + if key == controller_name and namespace != "/": node.get_logger().fatal( f"{bcolors.FAIL}Missing namespace : {namespace} or wildcard in parameter file for controller : {controller_name}{bcolors.ENDC}" ) From 21872b77f90075d31efa941cff9e344ce2022050 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 24 Sep 2024 23:52:43 +0200 Subject: [PATCH 07/10] fix tests after rebasing --- .../test/test_spawner_unspawner.cpp | 24 ++++++++----------- 1 file changed, 10 insertions(+), 14 deletions(-) diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index 212be1d6b8..74e1efeeed 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -695,7 +695,8 @@ TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_type_in_params_file) ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); ASSERT_EQ( - cm_->get_parameter("ctrl_with_parameters_and_type.params_file").as_string(), test_file_path); + cm_->get_parameter(ctrl_with_parameters_and_type.info.name + ".params_file").as_string(), + test_file_path); auto chain_ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[1]; ASSERT_EQ( @@ -707,7 +708,7 @@ TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_type_in_params_file) chain_ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); ASSERT_EQ( - cm_->get_parameter("chainable_ctrl_with_parameters_and_type.params_file").as_string(), + cm_->get_parameter(chain_ctrl_with_parameters_and_type.info.name + ".params_file").as_string(), test_file_path); EXPECT_EQ( @@ -724,17 +725,14 @@ TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_type_in_params_file) ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_EQ( ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); - ASSERT_EQ( - cm_->get_parameter("ctrl_with_parameters_and_type.params_file").as_string(), test_file_path); + ASSERT_EQ(cm_->get_parameter(ctrl_1.info.name + ".params_file").as_string(), test_file_path); auto ctrl_2 = cm_->get_loaded_controllers()[1]; ASSERT_EQ(ctrl_2.info.name, "ns_chainable_ctrl_with_parameters_and_type"); ASSERT_EQ(ctrl_2.info.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_EQ( ctrl_2.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); - ASSERT_EQ( - cm_->get_parameter("chainable_ctrl_with_parameters_and_type.params_file").as_string(), - test_file_path); + ASSERT_EQ(cm_->get_parameter(ctrl_2.info.name + ".params_file").as_string(), test_file_path); } TEST_F( @@ -783,7 +781,8 @@ TEST_F( ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); ASSERT_EQ( - cm_->get_parameter("ctrl_with_parameters_and_type.params_file").as_string(), test_file_path); + cm_->get_parameter(ctrl_with_parameters_and_type.info.name + ".params_file").as_string(), + test_file_path); auto chain_ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[1]; ASSERT_EQ( @@ -795,7 +794,7 @@ TEST_F( chain_ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); ASSERT_EQ( - cm_->get_parameter("chainable_ctrl_with_parameters_and_type.params_file").as_string(), + cm_->get_parameter(chain_ctrl_with_parameters_and_type.info.name + ".params_file").as_string(), test_file_path); EXPECT_EQ( @@ -813,17 +812,14 @@ TEST_F( ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_EQ( ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); - ASSERT_EQ( - cm_->get_parameter("ctrl_with_parameters_and_type.params_file").as_string(), test_file_path); + ASSERT_EQ(cm_->get_parameter(ctrl_1.info.name + ".params_file").as_string(), test_file_path); auto ctrl_2 = cm_->get_loaded_controllers()[1]; ASSERT_EQ(ctrl_2.info.name, "ns_chainable_ctrl_with_parameters_and_type"); ASSERT_EQ(ctrl_2.info.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_EQ( ctrl_2.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); - ASSERT_EQ( - cm_->get_parameter("chainable_ctrl_with_parameters_and_type.params_file").as_string(), - test_file_path); + ASSERT_EQ(cm_->get_parameter(ctrl_2.info.name + ".params_file").as_string(), test_file_path); } TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_with_wildcard_entries_in_params_file) From 1b524a2b4ee7b1182bf3d362817eada94deabd4c Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 9 Oct 2024 19:47:13 +0200 Subject: [PATCH 08/10] add documentation and release_notes --- controller_manager/doc/userdoc.rst | 54 ++++++++++++++++++++++++++++++ doc/release_notes.rst | 1 + 2 files changed, 55 insertions(+) diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index bc9f75384e..db507b79bb 100644 --- a/controller_manager/doc/userdoc.rst +++ b/controller_manager/doc/userdoc.rst @@ -171,6 +171,60 @@ There are two scripts to interact with controller manager from launch files: param file +The parsed controller config file can follow the same conventions as the typical ROS 2 parameter file format. Now, the spawner can handle config files with wildcard entries and also the controller name in the absolute namespace. See the following examples on the config files: + + .. code-block:: yaml + + /**/position_trajectory_controller: + ros__parameters: + type: joint_trajectory_controller/JointTrajectoryController + joints: + - joint1 + - joint2 + + command_interfaces: + - position + ..... + + .. code-block:: yaml + + /position_trajectory_controller: + ros__parameters: + type: joint_trajectory_controller/JointTrajectoryController + joints: + - joint1 + - joint2 + + command_interfaces: + - position + ..... + + .. code-block:: yaml + + position_trajectory_controller: + ros__parameters: + type: joint_trajectory_controller/JointTrajectoryController + joints: + - joint1 + - joint2 + + command_interfaces: + - position + ..... + + .. code-block:: yaml + + /rrbot_1/position_trajectory_controller: + ros__parameters: + type: joint_trajectory_controller/JointTrajectoryController + joints: + - joint1 + - joint2 + + command_interfaces: + - position + ..... + ``unspawner`` ^^^^^^^^^^^^^^^^ diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 8320a81705..75bb8632fe 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -73,6 +73,7 @@ controller_manager * The support for the ``description`` parameter for loading the URDF was removed (`#1358 `_). * The ``--controller-type`` or ``-t`` spawner arg is removed. Now the controller type is defined in the controller configuration file with ``type`` field (`#1639 `_). * The ``--namespace`` or ``-n`` spawner arg is deprecated. Now the spawner namespace can be defined using the ROS 2 standard way (`#1640 `_). +* Added support for the wildcard entries for the controller configuration files (`#1724 `_). hardware_interface ****************** From 5b1fe443ce1b64b760461f85b7453910c9236d2a Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 9 Oct 2024 19:49:59 +0200 Subject: [PATCH 09/10] fix typo --- doc/release_notes.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 5f775c24b8..25965aa753 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -74,7 +74,7 @@ controller_manager * The support for the ``description`` parameter for loading the URDF was removed (`#1358 `_). * The ``--controller-type`` or ``-t`` spawner arg is removed. Now the controller type is defined in the controller configuration file with ``type`` field (`#1639 `_). * The ``--namespace`` or ``-n`` spawner arg is deprecated. Now the spawner namespace can be defined using the ROS 2 standard way (`#1640 `_). -* Added support for the wildcard entries for the controller configuration files (`#1724 `_). +* Added support for the wildcard entries for the controller configuration files (`#1724 `_). hardware_interface ****************** From 9ff8450e999b446b6f81203853d20b1007e2badc Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 10 Oct 2024 10:45:36 +0200 Subject: [PATCH 10/10] update the logic to work for overriding parameters as well --- .../controller_manager_services.py | 22 ++++++++++++------- 1 file changed, 14 insertions(+), 8 deletions(-) diff --git a/controller_manager/controller_manager/controller_manager_services.py b/controller_manager/controller_manager/controller_manager_services.py index 29e07e3d2f..102531f8e8 100644 --- a/controller_manager/controller_manager/controller_manager_services.py +++ b/controller_manager/controller_manager/controller_manager_services.py @@ -278,22 +278,28 @@ def get_parameter_from_param_file( ) break controller_param_dict = parameters[key] - break + if WILDCARD_KEY in parameters and key in parameters[WILDCARD_KEY]: controller_param_dict = parameters[WILDCARD_KEY][key] + + if controller_param_dict and ( + not isinstance(controller_param_dict, dict) + or ROS_PARAMS_KEY not in controller_param_dict + ): + raise RuntimeError( + f"YAML file : {parameter_file} is not a valid ROS parameter file for controller node : {namespaced_controller}" + ) + if ( + controller_param_dict + and ROS_PARAMS_KEY in controller_param_dict + and parameter_name in controller_param_dict[ROS_PARAMS_KEY] + ): break if controller_param_dict is None: node.get_logger().fatal( f"{bcolors.FAIL}Controller : {namespaced_controller} parameters not found in parameter file : {parameter_file}{bcolors.ENDC}" ) - if ( - not isinstance(controller_param_dict, dict) - or ROS_PARAMS_KEY not in controller_param_dict - ): - raise RuntimeError( - f"YAML file : {parameter_file} is not a valid ROS parameter file for controller node : {namespaced_controller}" - ) if parameter_name in controller_param_dict[ROS_PARAMS_KEY]: return controller_param_dict[ROS_PARAMS_KEY][parameter_name]