From ebbd0362a0fd637d81772a3735461309906a05a1 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 19 Aug 2024 12:21:41 +0200 Subject: [PATCH 1/2] Refactor spawner to be able to reuse code for ros2controlcli (#1661) (cherry picked from commit 0631e3edecadd8b4a50ad2a3dc1374030167f71a) # Conflicts: # controller_manager/controller_manager/spawner.py # controller_manager/test/test_spawner_unspawner.cpp --- .../controller_manager/__init__.py | 8 ++ .../controller_manager_services.py | 117 ++++++++++++++++++ .../controller_manager/hardware_spawner.py | 14 +-- .../controller_manager/spawner.py | 55 +++----- .../test/test_spawner_unspawner.cpp | 51 ++++++++ .../ros2controlcli/verb/list_controllers.py | 3 +- .../verb/list_hardware_components.py | 3 +- .../verb/list_hardware_interfaces.py | 3 +- 8 files changed, 200 insertions(+), 54 deletions(-) diff --git a/controller_manager/controller_manager/__init__.py b/controller_manager/controller_manager/__init__.py index f49bed4d34..4a8d7daee5 100644 --- a/controller_manager/controller_manager/__init__.py +++ b/controller_manager/controller_manager/__init__.py @@ -23,6 +23,10 @@ set_hardware_component_state, switch_controllers, unload_controller, + get_parameter_from_param_file, + set_controller_parameters, + set_controller_parameters_from_param_file, + bcolors, ) __all__ = [ @@ -36,4 +40,8 @@ "set_hardware_component_state", "switch_controllers", "unload_controller", + "get_parameter_from_param_file", + "set_controller_parameters", + "set_controller_parameters_from_param_file", + "bcolors", ] diff --git a/controller_manager/controller_manager/controller_manager_services.py b/controller_manager/controller_manager/controller_manager_services.py index 3f5a820fae..7b0f726d68 100644 --- a/controller_manager/controller_manager/controller_manager_services.py +++ b/controller_manager/controller_manager/controller_manager_services.py @@ -26,6 +26,29 @@ ) import rclpy +import yaml +from rcl_interfaces.msg import Parameter + +# @note: The versions conditioning is added here to support the source-compatibility with Humble +# The `get_parameter_value` function is moved to `rclpy.parameter` module from `ros2param.api` module from version 3.6.0 +try: + from rclpy.parameter import get_parameter_value +except ImportError: + from ros2param.api import get_parameter_value +from ros2param.api import call_set_parameters + + +# from https://stackoverflow.com/a/287944 +class bcolors: + MAGENTA = "\033[95m" + OKBLUE = "\033[94m" + OKCYAN = "\033[96m" + OKGREEN = "\033[92m" + WARNING = "\033[93m" + FAIL = "\033[91m" + ENDC = "\033[0m" + BOLD = "\033[1m" + UNDERLINE = "\033[4m" class ServiceNotFoundError(Exception): @@ -220,3 +243,97 @@ def unload_controller(node, controller_manager_name, controller_name, service_ti request, service_timeout, ) + + +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 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 : {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 + + +def set_controller_parameters( + node, controller_manager_name, controller_name, parameter_name, parameter_value +): + parameter = Parameter() + parameter.name = controller_name + "." + parameter_name + parameter_string = str(parameter_value) + parameter.value = get_parameter_value(string_value=parameter_string) + + 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 + + 'Setting controller param "' + + parameter_name + + '" to "' + + parameter_string + + '" for ' + + bcolors.BOLD + + controller_name + + bcolors.ENDC + ) + else: + node.get_logger().fatal( + bcolors.FAIL + + 'Could not set controller param "' + + parameter_name + + '" to "' + + parameter_string + + '" for ' + + bcolors.BOLD + + controller_name + + bcolors.ENDC + ) + return False + return True + + +def set_controller_parameters_from_param_file( + node, controller_manager_name, controller_name, parameter_file, namespace=None +): + if parameter_file: + spawner_namespace = namespace if namespace else node.get_namespace() + set_controller_parameters( + node, controller_manager_name, controller_name, "param_file", parameter_file + ) + + controller_type = get_parameter_from_param_file( + controller_name, spawner_namespace, parameter_file, "type" + ) + if controller_type: + if not set_controller_parameters( + 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/hardware_spawner.py b/controller_manager/controller_manager/hardware_spawner.py index 3e3a487c6a..29c0b5e97c 100644 --- a/controller_manager/controller_manager/hardware_spawner.py +++ b/controller_manager/controller_manager/hardware_spawner.py @@ -19,6 +19,7 @@ from controller_manager import ( list_hardware_components, set_hardware_component_state, + bcolors, ) from controller_manager.controller_manager_services import ServiceNotFoundError @@ -28,19 +29,6 @@ from rclpy.signals import SignalHandlerOptions -# from https://stackoverflow.com/a/287944 -class bcolors: - HEADER = "\033[95m" - OKBLUE = "\033[94m" - OKCYAN = "\033[96m" - OKGREEN = "\033[92m" - WARNING = "\033[93m" - FAIL = "\033[91m" - ENDC = "\033[0m" - BOLD = "\033[1m" - UNDERLINE = "\033[4m" - - def first_match(iterable, predicate): return next((n for n in iterable if predicate(n)), None) diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index 112405cd0a..118a435f3b 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/controller_manager/controller_manager/spawner.py @@ -19,7 +19,6 @@ import sys import time import warnings -import yaml from controller_manager import ( configure_controller, @@ -27,29 +26,18 @@ load_controller, switch_controllers, unload_controller, + set_controller_parameters_from_param_file, + bcolors, ) from controller_manager.controller_manager_services import ServiceNotFoundError import rclpy -from rcl_interfaces.msg import Parameter 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 -from ros2param.api import call_set_parameters - -# from https://stackoverflow.com/a/287944 - - -class bcolors: - MAGENTA = "\033[95m" - OKBLUE = "\033[94m" - OKCYAN = "\033[96m" - OKGREEN = "\033[92m" - WARNING = "\033[93m" - FAIL = "\033[91m" - ENDC = "\033[0m" - BOLD = "\033[1m" - UNDERLINE = "\033[4m" def first_match(iterable, predicate): @@ -81,24 +69,6 @@ def is_controller_loaded(node, controller_manager, controller_name, service_time return any(c.name == controller_name for c in controllers) -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 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 : {namespaced_controller}" - ) - if parameter_name in parameters[namespaced_controller]["ros__parameters"]: - return parameters[namespaced_controller]["ros__parameters"][parameter_name] - else: - return None - - def main(args=None): rclpy.init(args=args, signal_handler_options=SignalHandlerOptions.NO) parser = argparse.ArgumentParser() @@ -190,6 +160,10 @@ 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 ): @@ -199,6 +173,7 @@ def main(args=None): + bcolors.ENDC ) else: +<<<<<<< HEAD controller_type = ( args.controller_type if param_file is None @@ -268,6 +243,16 @@ def main(args=None): + controller_name + bcolors.ENDC ) +======= + if param_file: + if not set_controller_parameters_from_param_file( + node, + controller_manager_name, + controller_name, + 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 06503dc7a7..180d76e6b2 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -283,6 +283,57 @@ 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; diff --git a/ros2controlcli/ros2controlcli/verb/list_controllers.py b/ros2controlcli/ros2controlcli/verb/list_controllers.py index a26a1168a1..3daf752b8c 100644 --- a/ros2controlcli/ros2controlcli/verb/list_controllers.py +++ b/ros2controlcli/ros2controlcli/verb/list_controllers.py @@ -12,8 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -from controller_manager import list_controllers -from controller_manager.spawner import bcolors +from controller_manager import list_controllers, bcolors from ros2cli.node.direct import add_arguments from ros2cli.node.strategy import NodeStrategy diff --git a/ros2controlcli/ros2controlcli/verb/list_hardware_components.py b/ros2controlcli/ros2controlcli/verb/list_hardware_components.py index e2b427b233..e3e2269920 100644 --- a/ros2controlcli/ros2controlcli/verb/list_hardware_components.py +++ b/ros2controlcli/ros2controlcli/verb/list_hardware_components.py @@ -12,8 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -from controller_manager import list_hardware_components -from controller_manager.spawner import bcolors +from controller_manager import list_hardware_components, bcolors from lifecycle_msgs.msg import State diff --git a/ros2controlcli/ros2controlcli/verb/list_hardware_interfaces.py b/ros2controlcli/ros2controlcli/verb/list_hardware_interfaces.py index 7aa850f3bc..4510998ad9 100644 --- a/ros2controlcli/ros2controlcli/verb/list_hardware_interfaces.py +++ b/ros2controlcli/ros2controlcli/verb/list_hardware_interfaces.py @@ -12,8 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -from controller_manager import list_hardware_interfaces -from controller_manager.spawner import bcolors +from controller_manager import list_hardware_interfaces, bcolors from ros2cli.node.direct import add_arguments from ros2cli.node.strategy import NodeStrategy From 12e080a37937601125fdc9f1d80474430d0d6c2a Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sat, 31 Aug 2024 23:06:28 +0200 Subject: [PATCH 2/2] [Iron] Fix conflicts of 1661 backport (#1736) * Fix conflicts * added controller_type parameter setting to the spawner --- .../controller_manager_services.py | 13 --- .../controller_manager/spawner.py | 88 ++----------------- .../test/test_spawner_unspawner.cpp | 51 ----------- 3 files changed, 9 insertions(+), 143 deletions(-) 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;