From cc75a74d93cc729afac594850abb94db4dbf1a66 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Thu, 23 May 2024 15:55:13 +0200 Subject: [PATCH 01/13] Fix controllers parameters of controller manager Signed-off-by: Jakub Delicat --- controller_manager/controller_manager/spawner.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index 2778bddebd..52e30c4284 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/controller_manager/controller_manager/spawner.py @@ -262,7 +262,7 @@ def main(args=None): ) if controller_type: parameter = Parameter() - parameter.name = prefixed_controller_name + ".type" + parameter.name = controller_name + ".type" parameter.value = get_parameter_value(string_value=controller_type) response = call_set_parameters( @@ -294,7 +294,7 @@ def main(args=None): if param_file: parameter = Parameter() - parameter.name = prefixed_controller_name + ".params_file" + parameter.name = controller_name + ".params_file" parameter.value = get_parameter_value(string_value=param_file) response = call_set_parameters( From 19cf6ff6c8c66404de449b45455659b5380904d2 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 24 Jul 2024 19:55:57 +0200 Subject: [PATCH 02/13] fix the fallback controllers controller name --- controller_manager/controller_manager/spawner.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index 52e30c4284..ca100ccc3f 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/controller_manager/controller_manager/spawner.py @@ -331,7 +331,7 @@ def main(args=None): if fallback_controllers: parameter = Parameter() - parameter.name = prefixed_controller_name + ".fallback_controllers" + parameter.name = controller_name + ".fallback_controllers" parameter.value = get_parameter_value(string_value=str(fallback_controllers)) response = call_set_parameters( From 7edce1acea76676214c91831597f3bfa79542fd3 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 24 Jul 2024 19:56:13 +0200 Subject: [PATCH 03/13] use non prefixed controller name to check if the controller is loaded or not --- controller_manager/controller_manager/spawner.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index ca100ccc3f..c97695dfbe 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/controller_manager/controller_manager/spawner.py @@ -248,7 +248,7 @@ def main(args=None): if controller_namespace: prefixed_controller_name = controller_namespace + "/" + controller_name - if is_controller_loaded(node, controller_manager_name, prefixed_controller_name): + if is_controller_loaded(node, controller_manager_name, controller_name): node.get_logger().warn( bcolors.WARNING + "Controller already loaded, skipping load_controller" From 5e1023a34ef11f28fc23c05f8a00383aceb7fa12 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 24 Jul 2024 19:56:41 +0200 Subject: [PATCH 04/13] fix the controller manager namespacing in spawner --- controller_manager/controller_manager/spawner.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index c97695dfbe..8b0400e5a2 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/controller_manager/controller_manager/spawner.py @@ -227,9 +227,9 @@ def main(args=None): node = Node("spawner_" + controller_names[0]) if not controller_manager_name.startswith("/"): - spawner_namespace = node.get_namespace() - if spawner_namespace != "/": - controller_manager_name = f"{spawner_namespace}/{controller_manager_name}" + spawner_namespace = args.namespace + if spawner_namespace != "/" and spawner_namespace != "": + controller_manager_name = f"/{spawner_namespace}/{controller_manager_name}" else: controller_manager_name = f"/{controller_manager_name}" From 8ccaae1c3b22d89acf1a13f6cae4ed89280e9b73 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 24 Jul 2024 19:52:30 +0200 Subject: [PATCH 05/13] Added test to test with the namespaced controller manager --- .../test/controller_manager_test_common.hpp | 5 +- .../test/test_spawner_unspawner.cpp | 137 ++++++++++++++++++ 2 files changed, 140 insertions(+), 2 deletions(-) diff --git a/controller_manager/test/controller_manager_test_common.hpp b/controller_manager/test/controller_manager_test_common.hpp index 72326e8c6c..d5a60e6aac 100644 --- a/controller_manager/test/controller_manager_test_common.hpp +++ b/controller_manager/test/controller_manager_test_common.hpp @@ -61,14 +61,15 @@ class ControllerManagerFixture : public ::testing::Test { public: explicit ControllerManagerFixture( - const std::string & robot_description = ros2_control_test_assets::minimal_robot_urdf) + const std::string & robot_description = ros2_control_test_assets::minimal_robot_urdf, + const std::string & cm_namespace = "") : robot_description_(robot_description) { executor_ = std::make_shared(); cm_ = std::make_shared( std::make_unique( rm_node_->get_node_clock_interface(), rm_node_->get_node_logging_interface()), - executor_, TEST_CM_NAME); + executor_, TEST_CM_NAME, cm_namespace); // We want to be able to not pass robot description immediately if (!robot_description_.empty()) { diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index a7c2432959..23bbacff14 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -445,3 +445,140 @@ TEST_F( ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); } } + +class TestLoadControllerWithNamespacedCM +: public ControllerManagerFixture +{ +public: + TestLoadControllerWithNamespacedCM() + : ControllerManagerFixture( + ros2_control_test_assets::minimal_robot_urdf, "foo_namespace") + { + } + + void SetUp() override + { + ControllerManagerFixture::SetUp(); + + update_timer_ = cm_->create_wall_timer( + std::chrono::milliseconds(10), + [&]() + { + cm_->read(time_, PERIOD); + cm_->update(time_, PERIOD); + cm_->write(time_, PERIOD); + }); + + update_executor_ = + std::make_shared(rclcpp::ExecutorOptions(), 2); + + update_executor_->add_node(cm_); + update_executor_spin_future_ = + std::async(std::launch::async, [this]() -> void { update_executor_->spin(); }); + + // This sleep is needed to prevent a too fast test from ending before the + // executor has began to spin, which causes it to hang + std::this_thread::sleep_for(50ms); + } + + void TearDown() override { update_executor_->cancel(); } + +protected: + rclcpp::TimerBase::SharedPtr update_timer_; + + // Using a MultiThreadedExecutor so we can call update on a separate thread from service callbacks + std::shared_ptr update_executor_; + std::future update_executor_spin_future_; +}; + +TEST_F(TestLoadControllerWithNamespacedCM, multi_ctrls_test_type_in_param) +{ + 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 ctrl_2 -c test_controller_manager -n foo_namespace"), 0); + EXPECT_EQ(call_spawner("ctrl_1 ctrl_2 -c test_controller_manager"), 256) + << "Should fail without defining the namespace"; + + auto validate_loaded_controllers = [&]() + { + auto loaded_controllers = cm_->get_loaded_controllers(); + for (size_t i = 0; i < loaded_controllers.size(); i++) + { + auto ctrl = loaded_controllers[i]; + const std::string controller_name = "ctrl_" + std::to_string(i + 1); + + RCLCPP_ERROR(rclcpp::get_logger("test_controller_manager"), "%s", controller_name.c_str()); + auto it = std::find_if( + loaded_controllers.begin(), loaded_controllers.end(), + [&](const auto & controller) { return controller.info.name == controller_name; }); + ASSERT_TRUE(it != loaded_controllers.end()); + ASSERT_EQ(ctrl.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ(ctrl.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + } + }; + + ASSERT_EQ(cm_->get_loaded_controllers().size(), 2ul); + { + validate_loaded_controllers(); + } + + // Try to spawn again multiple but one of them is already active, it should fail because already + // active + EXPECT_NE(call_spawner("ctrl_1 ctrl_3 -c /foo_namespace/test_controller_manager"), 0) + << "Cannot configure from active"; + + std::vector start_controllers = {}; + std::vector stop_controllers = {"ctrl_1"}; + cm_->switch_controller( + start_controllers, stop_controllers, + controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0)); + + // We should be able to reconfigure and activate a configured controller + EXPECT_EQ(call_spawner("ctrl_1 ctrl_3 -c foo_namespace/test_controller_manager"), 0); + + ASSERT_EQ(cm_->get_loaded_controllers().size(), 3ul); + { + validate_loaded_controllers(); + } + + stop_controllers = {"ctrl_1", "ctrl_2"}; + cm_->switch_controller( + start_controllers, stop_controllers, + controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0)); + for (auto ctrl : stop_controllers) + { + cm_->unload_controller(ctrl); + cm_->load_controller(ctrl); + } + + // We should be able to reconfigure and activate am unconfigured loaded controller + EXPECT_EQ(call_spawner("ctrl_1 ctrl_2 -c test_controller_manager -n foo_namespace"), 0); + ASSERT_EQ(cm_->get_loaded_controllers().size(), 3ul); + { + validate_loaded_controllers(); + } + + // Unload and reload + EXPECT_EQ(call_unspawner("ctrl_1 ctrl_3 -c foo_namespace/test_controller_manager"), 0); + ASSERT_EQ(cm_->get_loaded_controllers().size(), 1ul) << "Controller should have been unloaded"; + EXPECT_EQ(call_spawner("ctrl_1 ctrl_3 -c test_controller_manager -n foo_namespace"), 0); + ASSERT_EQ(cm_->get_loaded_controllers().size(), 3ul) << "Controller should have been loaded"; + { + validate_loaded_controllers(); + } + + // Now unload everything and load them as a group in a single operation + EXPECT_EQ(call_unspawner("ctrl_1 ctrl_2 ctrl_3 -c /foo_namespace/test_controller_manager"), 0); + ASSERT_EQ(cm_->get_loaded_controllers().size(), 0ul) << "Controller should have been unloaded"; + EXPECT_EQ( + call_spawner( + "ctrl_1 ctrl_2 ctrl_3 -c test_controller_manager --activate-as-group -n foo_namespace"), + 0); + ASSERT_EQ(cm_->get_loaded_controllers().size(), 3ul) << "Controller should have been loaded"; + { + validate_loaded_controllers(); + } +} From bc039859376ca7ae6d369ef9c23fea1ee7f0caea Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sat, 27 Jul 2024 09:32:37 +0200 Subject: [PATCH 06/13] remove the `--namespace` option and prefixed_controller_name internal variable --- .../controller_manager/spawner.py | 56 +++++++++---------- 1 file changed, 25 insertions(+), 31 deletions(-) diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index 8b0400e5a2..8968fed5bb 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/controller_manager/controller_manager/spawner.py @@ -136,17 +136,20 @@ def is_controller_loaded(node, controller_manager, controller_name): return any(c.name == controller_name for c in controllers) -def get_parameter_from_param_file(controller_name, parameter_file, parameter_name): +def get_parameter_from_param_file(controller_name, namespace, parameter_file, parameter_name): with open(parameter_file) as f: + namespaced_controller = ( + controller_name if namespace == "/" else f"{namespace}/{controller_name}" + ) parameters = yaml.safe_load(f) - if controller_name in parameters: - value = parameters[controller_name] + if namespaced_controller in parameters: + value = parameters[namespaced_controller] if not isinstance(value, dict) or "ros__parameters" not in value: raise RuntimeError( - f"YAML file : {parameter_file} is not a valid ROS parameter file for controller : {controller_name}" + f"YAML file : {parameter_file} is not a valid ROS parameter file for controller : {namespaced_controller}" ) - if parameter_name in parameters[controller_name]["ros__parameters"]: - return parameters[controller_name]["ros__parameters"][parameter_name] + if parameter_name in parameters[namespaced_controller]["ros__parameters"]: + return parameters[namespaced_controller]["ros__parameters"][parameter_name] else: return None @@ -169,9 +172,7 @@ def main(args=None): default=None, required=False, ) - parser.add_argument( - "-n", "--namespace", help="Namespace for the controller", default="", required=False - ) + parser.add_argument( "--load-only", help="Only load the controller and leave unconfigured.", @@ -217,7 +218,6 @@ def main(args=None): args = parser.parse_args(command_line_args) controller_names = args.controller_names controller_manager_name = args.controller_manager - controller_namespace = args.namespace param_file = args.param_file controller_manager_timeout = args.controller_manager_timeout @@ -227,9 +227,8 @@ def main(args=None): node = Node("spawner_" + controller_names[0]) if not controller_manager_name.startswith("/"): - spawner_namespace = args.namespace - if spawner_namespace != "/" and spawner_namespace != "": - controller_manager_name = f"/{spawner_namespace}/{controller_manager_name}" + if node.get_namespace() != "/": + controller_manager_name = f"{node.get_namespace()}/{controller_manager_name}" else: controller_manager_name = f"/{controller_manager_name}" @@ -244,9 +243,6 @@ def main(args=None): for controller_name in controller_names: fallback_controllers = args.fallback_controllers - prefixed_controller_name = controller_name - if controller_namespace: - prefixed_controller_name = controller_namespace + "/" + controller_name if is_controller_loaded(node, controller_manager_name, controller_name): node.get_logger().warn( @@ -258,7 +254,9 @@ def main(args=None): controller_type = ( None if param_file is None - else get_parameter_from_param_file(controller_name, param_file, "type") + else get_parameter_from_param_file( + controller_name, node.get_namespace(), param_file, "type" + ) ) if controller_type: parameter = Parameter() @@ -277,7 +275,7 @@ def main(args=None): + controller_type + '" for ' + bcolors.BOLD - + prefixed_controller_name + + controller_name + bcolors.ENDC ) else: @@ -287,7 +285,7 @@ def main(args=None): + controller_type + '" for ' + bcolors.BOLD - + prefixed_controller_name + + controller_name + bcolors.ENDC ) return 1 @@ -309,7 +307,7 @@ def main(args=None): + param_file + '" for ' + bcolors.BOLD - + prefixed_controller_name + + controller_name + bcolors.ENDC ) else: @@ -319,14 +317,14 @@ def main(args=None): + param_file + '" for ' + bcolors.BOLD - + prefixed_controller_name + + controller_name + bcolors.ENDC ) return 1 if not fallback_controllers and param_file: fallback_controllers = get_parameter_from_param_file( - controller_name, param_file, "fallback_controllers" + controller_name, node.get_namespace(), param_file, "fallback_controllers" ) if fallback_controllers: @@ -346,7 +344,7 @@ def main(args=None): + ",".join(fallback_controllers) + '"] for ' + bcolors.BOLD - + prefixed_controller_name + + controller_name + bcolors.ENDC ) else: @@ -356,7 +354,7 @@ def main(args=None): + ",".join(fallback_controllers) + '"] for ' + bcolors.BOLD - + prefixed_controller_name + + controller_name + bcolors.ENDC ) return 1 @@ -367,16 +365,12 @@ def main(args=None): bcolors.FAIL + "Failed loading controller " + bcolors.BOLD - + prefixed_controller_name + + controller_name + bcolors.ENDC ) return 1 node.get_logger().info( - bcolors.OKBLUE - + "Loaded " - + bcolors.BOLD - + prefixed_controller_name - + bcolors.ENDC + bcolors.OKBLUE + "Loaded " + bcolors.BOLD + controller_name + bcolors.ENDC ) if not args.load_only: @@ -401,7 +395,7 @@ def main(args=None): bcolors.OKGREEN + "Configured and activated " + bcolors.BOLD - + prefixed_controller_name + + controller_name + bcolors.ENDC ) From 78cdceb6d2b5f24e0a58c4a9ccae4db6b80315ee Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sat, 27 Jul 2024 09:47:36 +0200 Subject: [PATCH 07/13] Update the spawner tests with the ros namespacing --- .../test/test_spawner_unspawner.cpp | 19 ++++++++++++------- 1 file changed, 12 insertions(+), 7 deletions(-) diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index 23bbacff14..97fda582c9 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -498,9 +498,10 @@ TEST_F(TestLoadControllerWithNamespacedCM, multi_ctrls_test_type_in_param) cm_->set_parameter(rclcpp::Parameter("ctrl_3.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); ControllerManagerRunner cm_runner(this); - EXPECT_EQ(call_spawner("ctrl_1 ctrl_2 -c test_controller_manager -n foo_namespace"), 0); EXPECT_EQ(call_spawner("ctrl_1 ctrl_2 -c test_controller_manager"), 256) << "Should fail without defining the namespace"; + EXPECT_EQ( + call_spawner("ctrl_1 ctrl_2 -c test_controller_manager --ros-args -r __ns:=/foo_namespace"), 0); auto validate_loaded_controllers = [&]() { @@ -527,7 +528,8 @@ TEST_F(TestLoadControllerWithNamespacedCM, multi_ctrls_test_type_in_param) // Try to spawn again multiple but one of them is already active, it should fail because already // active - EXPECT_NE(call_spawner("ctrl_1 ctrl_3 -c /foo_namespace/test_controller_manager"), 0) + EXPECT_NE( + call_spawner("ctrl_1 ctrl_3 -c test_controller_manager --ros-args -r __ns:=/foo_namespace"), 0) << "Cannot configure from active"; std::vector start_controllers = {}; @@ -537,7 +539,8 @@ TEST_F(TestLoadControllerWithNamespacedCM, multi_ctrls_test_type_in_param) controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0)); // We should be able to reconfigure and activate a configured controller - EXPECT_EQ(call_spawner("ctrl_1 ctrl_3 -c foo_namespace/test_controller_manager"), 0); + EXPECT_EQ( + call_spawner("ctrl_1 ctrl_3 -c test_controller_manager --ros-args -r __ns:=/foo_namespace"), 0); ASSERT_EQ(cm_->get_loaded_controllers().size(), 3ul); { @@ -555,7 +558,8 @@ TEST_F(TestLoadControllerWithNamespacedCM, multi_ctrls_test_type_in_param) } // We should be able to reconfigure and activate am unconfigured loaded controller - EXPECT_EQ(call_spawner("ctrl_1 ctrl_2 -c test_controller_manager -n foo_namespace"), 0); + EXPECT_EQ( + call_spawner("ctrl_1 ctrl_2 -c test_controller_manager --ros-args -r __ns:=/foo_namespace"), 0); ASSERT_EQ(cm_->get_loaded_controllers().size(), 3ul); { validate_loaded_controllers(); @@ -564,7 +568,8 @@ TEST_F(TestLoadControllerWithNamespacedCM, multi_ctrls_test_type_in_param) // Unload and reload EXPECT_EQ(call_unspawner("ctrl_1 ctrl_3 -c foo_namespace/test_controller_manager"), 0); ASSERT_EQ(cm_->get_loaded_controllers().size(), 1ul) << "Controller should have been unloaded"; - EXPECT_EQ(call_spawner("ctrl_1 ctrl_3 -c test_controller_manager -n foo_namespace"), 0); + EXPECT_EQ( + call_spawner("ctrl_1 ctrl_3 -c test_controller_manager --ros-args -r __ns:=/foo_namespace"), 0); ASSERT_EQ(cm_->get_loaded_controllers().size(), 3ul) << "Controller should have been loaded"; { validate_loaded_controllers(); @@ -574,8 +579,8 @@ TEST_F(TestLoadControllerWithNamespacedCM, multi_ctrls_test_type_in_param) EXPECT_EQ(call_unspawner("ctrl_1 ctrl_2 ctrl_3 -c /foo_namespace/test_controller_manager"), 0); ASSERT_EQ(cm_->get_loaded_controllers().size(), 0ul) << "Controller should have been unloaded"; EXPECT_EQ( - call_spawner( - "ctrl_1 ctrl_2 ctrl_3 -c test_controller_manager --activate-as-group -n foo_namespace"), + call_spawner("ctrl_1 ctrl_2 ctrl_3 -c test_controller_manager --activate-as-group --ros-args " + "-r __ns:=/foo_namespace"), 0); ASSERT_EQ(cm_->get_loaded_controllers().size(), 3ul) << "Controller should have been loaded"; { From 49498c3d38718939a9bebbbff7b35f2bb5786f4c Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sat, 27 Jul 2024 10:09:22 +0200 Subject: [PATCH 08/13] Add tests parsing the controllers from the param file to a namespaced controller_manager --- controller_manager/CMakeLists.txt | 1 + .../test_controller_spawner_with_type.yaml | 14 +++++ .../test/test_spawner_unspawner.cpp | 59 +++++++++++++++++++ 3 files changed, 74 insertions(+) diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index c4ef0e7df0..5dea15c0d1 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -185,6 +185,7 @@ if(BUILD_TESTING) ament_add_gmock(test_spawner_unspawner test/test_spawner_unspawner.cpp + TIMEOUT 120 ) target_link_libraries(test_spawner_unspawner controller_manager diff --git a/controller_manager/test/test_controller_spawner_with_type.yaml b/controller_manager/test/test_controller_spawner_with_type.yaml index 95a6efba30..892427bab7 100644 --- a/controller_manager/test/test_controller_spawner_with_type.yaml +++ b/controller_manager/test/test_controller_spawner_with_type.yaml @@ -11,3 +11,17 @@ chainable_ctrl_with_parameters_and_type: ctrl_with_parameters_and_no_type: ros__parameters: joint_names: ["joint2"] + +/foo_namespace/ctrl_with_parameters_and_type: + ros__parameters: + type: "controller_manager/test_controller" + joint_names: ["joint1"] + +/foo_namespace/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: + ros__parameters: + joint_names: ["joint2"] diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index 97fda582c9..9d5ccd0e5d 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -587,3 +587,62 @@ TEST_F(TestLoadControllerWithNamespacedCM, multi_ctrls_test_type_in_param) validate_loaded_controllers(); } } + +TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_type_in_params_file) +{ + const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") + + "/test/test_controller_spawner_with_type.yaml"; + + // 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 " + "test_controller_manager -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 " + "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, "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_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, "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_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + + EXPECT_EQ( + call_spawner( + "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, "ctrl_with_parameters_and_type"); + 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_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.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ(ctrl_2.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); +} From 45a4eb9b2f8e98a2f70f96bdee405bdc9b02acdf Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sat, 27 Jul 2024 15:03:05 +0200 Subject: [PATCH 09/13] add `--namespace` as deprecated and also tests for the deprecated arg --- .../controller_manager/spawner.py | 33 +++++++- .../test/test_spawner_unspawner.cpp | 75 +++++++++++++++++++ 2 files changed, 104 insertions(+), 4 deletions(-) diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index 8968fed5bb..afd9500509 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/controller_manager/controller_manager/spawner.py @@ -172,7 +172,13 @@ def main(args=None): default=None, required=False, ) - + parser.add_argument( + "-n", + "--namespace", + help="Namespace for the controller_manager and the controller(s) (deprecated)", + default=None, + required=False, + ) parser.add_argument( "--load-only", help="Only load the controller and leave unconfigured.", @@ -226,9 +232,28 @@ def main(args=None): node = Node("spawner_" + controller_names[0]) + if node.get_namespace() != "/" and args.namespace: + raise RuntimeError( + f"Setting namespace through both '--namespace {args.namespace}' arg and the ROS 2 standard way " + f"'--ros-args -r __ns:={node.get_namespace()}' is not allowed!" + ) + + if args.namespace: + warnings.filterwarnings("always") + warnings.warn( + "The '--namespace' argument is deprecated and will be removed in future releases." + " Use the ROS 2 standard way of setting the node namespacing using --ros-args -r __ns:=", + DeprecationWarning, + ) + + spawner_namespace = args.namespace if args.namespace else node.get_namespace() + + if not spawner_namespace.startswith("/"): + spawner_namespace = f"/{spawner_namespace}" + if not controller_manager_name.startswith("/"): - if node.get_namespace() != "/": - controller_manager_name = f"{node.get_namespace()}/{controller_manager_name}" + if spawner_namespace and spawner_namespace != "/": + controller_manager_name = f"{spawner_namespace}/{controller_manager_name}" else: controller_manager_name = f"/{controller_manager_name}" @@ -255,7 +280,7 @@ def main(args=None): None if param_file is None else get_parameter_from_param_file( - controller_name, node.get_namespace(), param_file, "type" + controller_name, spawner_namespace, param_file, "type" ) ) if controller_type: diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index 9d5ccd0e5d..58321d173d 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -646,3 +646,78 @@ TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_type_in_params_file) ASSERT_EQ(ctrl_2.info.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_EQ(ctrl_2.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); } + +TEST_F( + TestLoadControllerWithNamespacedCM, spawner_test_type_in_params_file_deprecated_namespace_arg) +{ + const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") + + "/test/test_controller_spawner_with_type.yaml"; + + // 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 " + "test_controller_manager -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 " + "test_controller_manager --namespace foo_namespace -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 " + "test_controller_manager --namespace foo_namespace -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 " + "test_controller_manager --namespace foo_namespace -p " + + test_file_path), + 0) + << "Should work when parsed through the deprecated arg"; + + 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.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ( + ctrl_with_parameters_and_type.c->get_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, "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_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + + EXPECT_EQ( + call_spawner( + "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!"; + // 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, "ctrl_with_parameters_and_type"); + 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_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.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ(ctrl_2.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); +} From abc1e12d5147492ff098e3fb2edb79c3e91b7ee3 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 29 Jul 2024 10:05:26 +0200 Subject: [PATCH 10/13] use spawner namespace for the fallback controllers retrieval as well --- controller_manager/controller_manager/spawner.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index afd9500509..50f61e7a93 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/controller_manager/controller_manager/spawner.py @@ -349,7 +349,7 @@ def main(args=None): if not fallback_controllers and param_file: fallback_controllers = get_parameter_from_param_file( - controller_name, node.get_namespace(), param_file, "fallback_controllers" + controller_name, spawner_namespace, param_file, "fallback_controllers" ) if fallback_controllers: From caf22b862b8e1ff4242d1bb704b7d9f0776c360f Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 29 Jul 2024 10:48:30 +0200 Subject: [PATCH 11/13] Update the helper scripts documentation and update release notes --- controller_manager/doc/userdoc.rst | 10 ++++++---- doc/release_notes.rst | 1 + 2 files changed, 7 insertions(+), 4 deletions(-) diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index 02e532866d..26b3cc20bd 100644 --- a/controller_manager/doc/userdoc.rst +++ b/controller_manager/doc/userdoc.rst @@ -149,7 +149,7 @@ There are two scripts to interact with controller manager from launch files: controller_name positional arguments: - controller_name Name of the controller + controller_names List of controllers options: -h, --help show this help message and exit @@ -158,14 +158,16 @@ There are two scripts to interact with controller manager from launch files: -p PARAM_FILE, --param-file PARAM_FILE Controller param file to be loaded into controller node before configure -n NAMESPACE, --namespace NAMESPACE - Namespace for the controller + Namespace for the controller_manager and the controller(s) (deprecated) --load-only Only load the controller and leave unconfigured. --inactive Load and configure the controller, however do not activate them - -t CONTROLLER_TYPE, --controller-type CONTROLLER_TYPE - If not provided it should exist in the controller manager namespace -u, --unload-on-kill Wait until this application is interrupted and unload controller --controller-manager-timeout CONTROLLER_MANAGER_TIMEOUT Time to wait for the controller manager + --activate-as-group Activates all the parsed controllers list together instead of one by one. Useful for activating all chainable controllers altogether + --fallback_controllers FALLBACK_CONTROLLERS [FALLBACK_CONTROLLERS ...] + Fallback controllers list are activated as a fallback strategy when the spawned controllers fail. When the argument is provided, it takes precedence over the fallback_controllers list in the + param file ``unspawner`` diff --git a/doc/release_notes.rst b/doc/release_notes.rst index e2b22bf7a6..5f49dacefc 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -70,6 +70,7 @@ controller_manager The parameters within the ``ros2_control`` tag are not supported any more. * 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 `_). hardware_interface ****************** From 285e54784b58cf9e9eaabc8f4224054fb98e2ed1 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 29 Jul 2024 11:59:10 +0200 Subject: [PATCH 12/13] Update the userdoc for the deprecated --namespace option Co-authored-by: Bence Magyar --- controller_manager/doc/userdoc.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index 26b3cc20bd..fc4adc991a 100644 --- a/controller_manager/doc/userdoc.rst +++ b/controller_manager/doc/userdoc.rst @@ -158,7 +158,7 @@ There are two scripts to interact with controller manager from launch files: -p PARAM_FILE, --param-file PARAM_FILE Controller param file to be loaded into controller node before configure -n NAMESPACE, --namespace NAMESPACE - Namespace for the controller_manager and the controller(s) (deprecated) + DEPRECATED Namespace for the controller_manager and the controller(s) --load-only Only load the controller and leave unconfigured. --inactive Load and configure the controller, however do not activate them -u, --unload-on-kill Wait until this application is interrupted and unload controller From bb819286024227c7a27ac7bc5fa3255df4de8989 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Mon, 29 Jul 2024 18:09:52 +0100 Subject: [PATCH 13/13] DEPRECATED --- controller_manager/controller_manager/spawner.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index 50f61e7a93..bc8d218ea8 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/controller_manager/controller_manager/spawner.py @@ -175,7 +175,7 @@ def main(args=None): parser.add_argument( "-n", "--namespace", - help="Namespace for the controller_manager and the controller(s) (deprecated)", + help="DEPRECATED Namespace for the controller_manager and the controller(s)", default=None, required=False, )