From 04873796a2a95ea0b58c088ed11f2ca78e0cac1e Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Wed, 14 Aug 2024 16:24:23 +0100 Subject: [PATCH 1/9] Handle waiting in Spawner and align Hardware Spawner functionality (#1562) (cherry picked from commit af4b48f75a0ff4c57f30df3477f9fa2786e07965) # Conflicts: # controller_manager/controller_manager/controller_manager_services.py # controller_manager/controller_manager/hardware_spawner.py # controller_manager/controller_manager/spawner.py # controller_manager/doc/userdoc.rst # controller_manager/test/test_spawner_unspawner.cpp --- .../controller_manager_services.py | 41 +- .../controller_manager/hardware_spawner.py | 69 ++- .../controller_manager/spawner.py | 65 +-- .../controller_manager/unspawner.py | 6 + controller_manager/doc/userdoc.rst | 38 ++ .../test/test_spawner_unspawner.cpp | 461 +++++++++++++++++- 6 files changed, 579 insertions(+), 101 deletions(-) diff --git a/controller_manager/controller_manager/controller_manager_services.py b/controller_manager/controller_manager/controller_manager_services.py index cbc40541b6..63115c5f2e 100644 --- a/controller_manager/controller_manager/controller_manager_services.py +++ b/controller_manager/controller_manager/controller_manager_services.py @@ -28,15 +28,20 @@ import rclpy -def service_caller(node, service_name, service_type, request, service_timeout=10.0): +class ServiceNotFoundError(Exception): + pass + + +def service_caller(node, service_name, service_type, request, service_timeout=0.0): cli = node.create_client(service_type, service_name) - if not cli.service_is_ready(): - node.get_logger().debug( - f"waiting {service_timeout} seconds for service {service_name} to become available..." - ) - if not cli.wait_for_service(service_timeout): - raise RuntimeError(f"Could not contact service {service_name}") + while not cli.service_is_ready(): + node.get_logger().info(f"waiting for service {service_name} to become available...") + if service_timeout: + if not cli.wait_for_service(service_timeout): + raise ServiceNotFoundError(f"Could not contact service {service_name}") + elif not cli.wait_for_service(10.0): + node.get_logger().warn(f"Could not contact service {service_name}") node.get_logger().debug(f"requester: making request: {request}\n") future = cli.call_async(request) @@ -47,7 +52,7 @@ def service_caller(node, service_name, service_type, request, service_timeout=10 raise RuntimeError(f"Exception while calling service: {future.exception()}") -def configure_controller(node, controller_manager_name, controller_name, service_timeout=10.0): +def configure_controller(node, controller_manager_name, controller_name, service_timeout=0.0): request = ConfigureController.Request() request.name = controller_name return service_caller( @@ -59,7 +64,7 @@ def configure_controller(node, controller_manager_name, controller_name, service ) -def list_controllers(node, controller_manager_name, service_timeout=10.0): +def list_controllers(node, controller_manager_name, service_timeout=0.0): request = ListControllers.Request() return service_caller( node, @@ -70,7 +75,7 @@ def list_controllers(node, controller_manager_name, service_timeout=10.0): ) -def list_controller_types(node, controller_manager_name, service_timeout=10.0): +def list_controller_types(node, controller_manager_name, service_timeout=0.0): request = ListControllerTypes.Request() return service_caller( node, @@ -81,7 +86,7 @@ def list_controller_types(node, controller_manager_name, service_timeout=10.0): ) -def list_hardware_components(node, controller_manager_name, service_timeout=10.0): +def list_hardware_components(node, controller_manager_name, service_timeout=0.0): request = ListHardwareComponents.Request() return service_caller( node, @@ -92,7 +97,7 @@ def list_hardware_components(node, controller_manager_name, service_timeout=10.0 ) -def list_hardware_interfaces(node, controller_manager_name, service_timeout=10.0): +def list_hardware_interfaces(node, controller_manager_name, service_timeout=0.0): request = ListHardwareInterfaces.Request() return service_caller( node, @@ -103,7 +108,7 @@ def list_hardware_interfaces(node, controller_manager_name, service_timeout=10.0 ) -def load_controller(node, controller_manager_name, controller_name, service_timeout=10.0): +def load_controller(node, controller_manager_name, controller_name, service_timeout=0.0): request = LoadController.Request() request.name = controller_name return service_caller( @@ -115,7 +120,7 @@ def load_controller(node, controller_manager_name, controller_name, service_time ) -def reload_controller_libraries(node, controller_manager_name, force_kill, service_timeout=10.0): +def reload_controller_libraries(node, controller_manager_name, force_kill, service_timeout=0.0): request = ReloadControllerLibraries.Request() request.force_kill = force_kill return service_caller( @@ -127,7 +132,13 @@ def reload_controller_libraries(node, controller_manager_name, force_kill, servi ) +<<<<<<< HEAD def set_hardware_component_state(node, controller_manager_name, component_name, lifecyle_state): +======= +def set_hardware_component_state( + node, controller_manager_name, component_name, lifecyle_state, service_timeout=0.0 +): +>>>>>>> af4b48f (Handle waiting in Spawner and align Hardware Spawner functionality (#1562)) request = SetHardwareComponentState.Request() request.name = component_name request.target_state = lifecyle_state @@ -162,7 +173,7 @@ def switch_controllers( ) -def unload_controller(node, controller_manager_name, controller_name, service_timeout=10.0): +def unload_controller(node, controller_manager_name, controller_name, service_timeout=0.0): request = UnloadController.Request() request.name = controller_name return service_caller( diff --git a/controller_manager/controller_manager/hardware_spawner.py b/controller_manager/controller_manager/hardware_spawner.py index 13c004082e..e4329cae0f 100644 --- a/controller_manager/controller_manager/hardware_spawner.py +++ b/controller_manager/controller_manager/hardware_spawner.py @@ -15,13 +15,15 @@ import argparse import sys -import time -from controller_manager import set_hardware_component_state +from controller_manager import ( + list_hardware_components, + set_hardware_component_state, +) +from controller_manager.controller_manager_services import ServiceNotFoundError from lifecycle_msgs.msg import State import rclpy -from rclpy.duration import Duration from rclpy.node import Node from rclpy.signals import SignalHandlerOptions @@ -43,6 +45,7 @@ def first_match(iterable, predicate): return next((n for n in iterable if predicate(n)), None) +<<<<<<< HEAD def wait_for_value_or(function, node, timeout, default, description): while node.get_clock().now() < timeout: result = function() @@ -55,6 +58,8 @@ def wait_for_value_or(function, node, timeout, default, description): return default +======= +>>>>>>> af4b48f (Handle waiting in Spawner and align Hardware Spawner functionality (#1562)) def combine_name_and_namespace(name_and_namespace): node_name, namespace = name_and_namespace return namespace + ("" if namespace.endswith("/") else "/") + node_name @@ -76,35 +81,11 @@ def has_service_names(node, node_name, node_namespace, service_names): return all(service in client_names for service in service_names) -def wait_for_controller_manager(node, controller_manager, timeout_duration): - # List of service names from controller_manager we wait for - service_names = ( - f"{controller_manager}/list_hardware_components", - f"{controller_manager}/set_hardware_component_state", - ) - - # Wait for controller_manager - timeout = node.get_clock().now() + Duration(seconds=timeout_duration) - node_and_namespace = wait_for_value_or( - lambda: find_node_and_namespace(node, controller_manager), - node, - timeout, - None, - f"'{controller_manager}' node to exist", - ) - - # Wait for the services if the node was found - if node_and_namespace: - node_name, namespace = node_and_namespace - return wait_for_value_or( - lambda: has_service_names(node, node_name, namespace, service_names), - node, - timeout, - False, - f"'{controller_manager}' services to be available", - ) - - return False +def is_hardware_component_loaded( + node, controller_manager, hardware_component, service_timeout=0.0 +): + components = list_hardware_components(node, hardware_component, service_timeout).component + return any(c.name == hardware_component for c in components) def handle_set_component_state_service_call( @@ -168,10 +149,9 @@ def main(args=None): "--controller-manager-timeout", help="Time to wait for the controller manager", required=False, - default=10, - type=int, + default=0, + type=float, ) - # add arguments which are mutually exclusive activate_or_confiigure_grp.add_argument( "--activate", @@ -203,13 +183,15 @@ def main(args=None): controller_manager_name = f"/{controller_manager_name}" try: - if not wait_for_controller_manager( - node, controller_manager_name, controller_manager_timeout + if not is_hardware_component_loaded( + node, controller_manager_name, hardware_component, controller_manager_timeout ): - node.get_logger().error("Controller manager not available") - return 1 - - if activate: + node.get_logger().warn( + bcolors.WARNING + + "Hardware Component is not loaded - state can not be changed." + + bcolors.ENDC + ) + elif activate: activate_components(node, controller_manager_name, hardware_component) elif configure: configure_components(node, controller_manager_name, hardware_component) @@ -219,6 +201,11 @@ def main(args=None): ) parser.print_help() return 0 + except KeyboardInterrupt: + pass + except ServiceNotFoundError as err: + node.get_logger().fatal(str(err)) + return 1 finally: rclpy.shutdown() diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index 6f4bb19892..7a2710cf45 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/controller_manager/controller_manager/spawner.py @@ -27,10 +27,10 @@ switch_controllers, unload_controller, ) +from controller_manager.controller_manager_services import ServiceNotFoundError import rclpy from rcl_interfaces.msg import Parameter -from rclpy.duration import Duration from rclpy.node import Node from rclpy.signals import SignalHandlerOptions from ros2param.api import call_set_parameters @@ -55,6 +55,7 @@ def first_match(iterable, predicate): return next((n for n in iterable if predicate(n)), None) +<<<<<<< HEAD def wait_for_value_or(function, node, timeout, default, description): while node.get_clock().now() < timeout: result = function() @@ -67,6 +68,8 @@ def wait_for_value_or(function, node, timeout, default, description): return default +======= +>>>>>>> af4b48f (Handle waiting in Spawner and align Hardware Spawner functionality (#1562)) def combine_name_and_namespace(name_and_namespace): node_name, namespace = name_and_namespace return namespace + ("" if namespace.endswith("/") else "/") + node_name @@ -87,46 +90,8 @@ def has_service_names(node, node_name, node_namespace, service_names): return all(service in client_names for service in service_names) -def wait_for_controller_manager(node, controller_manager, timeout_duration): - # List of service names from controller_manager we wait for - service_names = ( - f"{controller_manager}/configure_controller", - f"{controller_manager}/list_controllers", - f"{controller_manager}/list_controller_types", - f"{controller_manager}/list_hardware_components", - f"{controller_manager}/list_hardware_interfaces", - f"{controller_manager}/load_controller", - f"{controller_manager}/reload_controller_libraries", - f"{controller_manager}/switch_controller", - f"{controller_manager}/unload_controller", - ) - - # Wait for controller_manager - timeout = node.get_clock().now() + Duration(seconds=timeout_duration) - node_and_namespace = wait_for_value_or( - lambda: find_node_and_namespace(node, controller_manager), - node, - timeout, - None, - f"'{controller_manager}' node to exist", - ) - - # Wait for the services if the node was found - if node_and_namespace: - node_name, namespace = node_and_namespace - return wait_for_value_or( - lambda: has_service_names(node, node_name, namespace, service_names), - node, - timeout, - False, - f"'{controller_manager}' services to be available", - ) - - return False - - -def is_controller_loaded(node, controller_manager, controller_name): - controllers = list_controllers(node, controller_manager).controller +def is_controller_loaded(node, controller_manager, controller_name, service_timeout=0.0): + controllers = list_controllers(node, controller_manager, service_timeout).controller return any(c.name == controller_name for c in controllers) @@ -186,8 +151,8 @@ def main(args=None): "--controller-manager-timeout", help="Time to wait for the controller manager", required=False, - default=10, - type=int, + default=0, + type=float, ) parser.add_argument( "--activate-as-group", @@ -219,18 +184,27 @@ def main(args=None): controller_manager_name = f"/{controller_manager_name}" try: +<<<<<<< HEAD if not wait_for_controller_manager( node, controller_manager_name, controller_manager_timeout ): node.get_logger().error("Controller manager not available") return 1 +======= +>>>>>>> af4b48f (Handle waiting in Spawner and align Hardware Spawner functionality (#1562)) for controller_name in controller_names: prefixed_controller_name = controller_name if controller_namespace: prefixed_controller_name = controller_namespace + "/" + controller_name +<<<<<<< HEAD if is_controller_loaded(node, controller_manager_name, prefixed_controller_name): +======= + if is_controller_loaded( + node, controller_manager_name, controller_name, controller_manager_timeout + ): +>>>>>>> af4b48f (Handle waiting in Spawner and align Hardware Spawner functionality (#1562)) node.get_logger().warn( bcolors.WARNING + "Controller already loaded, skipping load_controller" @@ -393,6 +367,11 @@ def main(args=None): node.get_logger().info("Unloaded controller") return 0 + except KeyboardInterrupt: + pass + except ServiceNotFoundError as err: + node.get_logger().fatal(str(err)) + return 1 finally: rclpy.shutdown() diff --git a/controller_manager/controller_manager/unspawner.py b/controller_manager/controller_manager/unspawner.py index 28d94dfe58..d7cab49cbd 100644 --- a/controller_manager/controller_manager/unspawner.py +++ b/controller_manager/controller_manager/unspawner.py @@ -19,6 +19,7 @@ import warnings from controller_manager import switch_controllers, unload_controller +from controller_manager.controller_manager_services import ServiceNotFoundError import rclpy from rclpy.node import Node @@ -58,6 +59,11 @@ def main(args=None): node.get_logger().info("Unloaded controller") return 0 + except KeyboardInterrupt: + pass + except ServiceNotFoundError as err: + node.get_logger().fatal(str(err)) + return 1 finally: rclpy.shutdown() diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index b9c057f571..5aa3c819b7 100644 --- a/controller_manager/doc/userdoc.rst +++ b/controller_manager/doc/userdoc.rst @@ -81,6 +81,7 @@ There are two scripts to interact with controller manager from launch files: 1. ``spawner`` - loads, configures and start a controller on startup. 2. ``unspawner`` - stops and unloads a controller. + 3. ``hardware_spawner`` - activates and configures a hardware component. ``spawner`` @@ -127,6 +128,43 @@ There are two scripts to interact with controller manager from launch files: -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER Name of the controller manager ROS node +<<<<<<< HEAD +======= +``hardware_spawner`` +^^^^^^^^^^^^^^^^^^^^^^ + +.. code-block:: console + + $ ros2 run controller_manager hardware_spawner -h + usage: hardware_spawner [-h] [-c CONTROLLER_MANAGER] (--activate | --configure) hardware_component_name + + positional arguments: + hardware_component_name + The name of the hardware component which should be activated. + + options: + -h, --help show this help message and exit + -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER + Name of the controller manager ROS node + --activate Activates the given components. Note: Components are by default configured before activated. + --configure Configures the given components. + +rqt_controller_manager +---------------------- +A GUI tool to interact with the controller manager services to be able to switch the lifecycle states of the controllers as well as the hardware components. + +.. image:: images/rqt_controller_manager.png + +It can be launched independently using the following command or as rqt plugin: + +.. code-block:: console + + ros2 run rqt_controller_manager rqt_controller_manager + +* Double-click on a controller or hardware component to show the additional info. +* Right-click on a controller or hardware component to show a context menu with options for lifecycle management. + +>>>>>>> af4b48f (Handle waiting in Spawner and align Hardware Spawner functionality (#1562)) Using the Controller Manager in a Process ----------------------------------------- diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index 7c03a8862d..1eeb489663 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -82,9 +82,10 @@ TEST_F(TestLoadController, spawner_with_no_arguments_errors) EXPECT_NE(call_spawner(""), 0) << "Missing mandatory arguments"; } -TEST_F(TestLoadController, spawner_without_manager_errors) +TEST_F(TestLoadController, spawner_without_manager_errors_with_given_timeout) { - EXPECT_NE(call_spawner("ctrl_1"), 0) << "Wrong controller manager name"; + EXPECT_NE(call_spawner("ctrl_1 --controller-manager-timeout 1.0"), 0) + << "Wrong controller manager name"; } TEST_F(TestLoadController, spawner_without_type_parameter_or_arg_errors) @@ -221,11 +222,51 @@ TEST_F(TestLoadController, spawner_test_type_in_arg) std::string(test_controller::TEST_CONTROLLER_CLASS_NAME)), 0); +<<<<<<< HEAD ASSERT_EQ(cm_->get_loaded_controllers().size(), 1ul); auto ctrl_2 = cm_->get_loaded_controllers()[0]; ASSERT_EQ(ctrl_2.info.name, "ctrl_2"); ASSERT_EQ(ctrl_2.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_EQ(ctrl_2.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); +======= + 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 --controller-manager-timeout " + "1.0 -p " + + test_file_path), + 256); + // 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); +>>>>>>> af4b48f (Handle waiting in Spawner and align Hardware Spawner functionality (#1562)) } TEST_F(TestLoadController, unload_on_kill) @@ -243,3 +284,419 @@ 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 --fallback_controllers ctrl_3 ctrl_4 ctrl_5 " + "-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_THAT( + ctrl_1.info.fallback_controllers_names, testing::ElementsAre("ctrl_3", "ctrl_4", "ctrl_5")); + 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_THAT( + ctrl_1.info.fallback_controllers_names, testing::ElementsAre("ctrl_3", "ctrl_4", "ctrl_5")); + 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); + } +} + +class TestLoadControllerWithoutRobotDescription +: public ControllerManagerFixture +{ +public: + TestLoadControllerWithoutRobotDescription() + : ControllerManagerFixture("") + { + } + + 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(); } + + rclcpp::TimerBase::SharedPtr robot_description_sending_timer_; + +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(TestLoadControllerWithoutRobotDescription, when_no_robot_description_spawner_times_out) +{ + cm_->set_parameter(rclcpp::Parameter("ctrl_1.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); + + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(call_spawner("ctrl_1 -c test_controller_manager --controller-manager-timeout 1.0"), 256) + << "could not spawn controller because not robot description and not services for controller " + "manager are active"; +} + +TEST_F( + TestLoadControllerWithoutRobotDescription, + controller_starting_with_later_load_of_robot_description) +{ + cm_->set_parameter(rclcpp::Parameter("ctrl_1.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); + + // Delay sending robot description + robot_description_sending_timer_ = cm_->create_wall_timer( + std::chrono::milliseconds(2500), [&]() { pass_robot_description_to_cm_and_rm(); }); + + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(call_spawner("ctrl_1 -c test_controller_manager"), 0) + << "could not activate control because not robot description"; + } + + 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_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 --controller-manager-timeout 1.0"), 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 = [&]() + { + 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 test_controller_manager --ros-args -r __ns:=/foo_namespace"), 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 test_controller_manager --ros-args -r __ns:=/foo_namespace"), 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 --ros-args -r __ns:=/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 --ros-args -r __ns:=/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 --ros-args " + "-r __ns:=/foo_namespace"), + 0); + ASSERT_EQ(cm_->get_loaded_controllers().size(), 3ul) << "Controller should have been loaded"; + { + 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 --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 " + "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); +} + +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 --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 " + "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 " + "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 " + "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); +} +>>>>>>> af4b48f (Handle waiting in Spawner and align Hardware Spawner functionality (#1562)) From b32b363cba4641ce0343729052404acbd5f11173 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Wed, 14 Aug 2024 17:46:00 +0200 Subject: [PATCH 2/9] Update hardware_spawner.py --- .../controller_manager/hardware_spawner.py | 15 --------------- 1 file changed, 15 deletions(-) diff --git a/controller_manager/controller_manager/hardware_spawner.py b/controller_manager/controller_manager/hardware_spawner.py index e4329cae0f..3e3a487c6a 100644 --- a/controller_manager/controller_manager/hardware_spawner.py +++ b/controller_manager/controller_manager/hardware_spawner.py @@ -45,21 +45,6 @@ def first_match(iterable, predicate): return next((n for n in iterable if predicate(n)), None) -<<<<<<< HEAD -def wait_for_value_or(function, node, timeout, default, description): - while node.get_clock().now() < timeout: - result = function() - if result: - return result - node.get_logger().info( - f"Waiting for {description}", throttle_duration_sec=2, skip_first=True - ) - time.sleep(0.2) - return default - - -======= ->>>>>>> af4b48f (Handle waiting in Spawner and align Hardware Spawner functionality (#1562)) def combine_name_and_namespace(name_and_namespace): node_name, namespace = name_and_namespace return namespace + ("" if namespace.endswith("/") else "/") + node_name From 90f8feff11ace055e9ed90a28dac721f5385d103 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Wed, 14 Aug 2024 17:47:08 +0200 Subject: [PATCH 3/9] Update spawner.py --- .../controller_manager/spawner.py | 29 ------------------- 1 file changed, 29 deletions(-) diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index 7a2710cf45..6496a821d2 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/controller_manager/controller_manager/spawner.py @@ -54,22 +54,6 @@ class bcolors: def first_match(iterable, predicate): return next((n for n in iterable if predicate(n)), None) - -<<<<<<< HEAD -def wait_for_value_or(function, node, timeout, default, description): - while node.get_clock().now() < timeout: - result = function() - if result: - return result - node.get_logger().info( - f"Waiting for {description}", throttle_duration_sec=2, skip_first=True - ) - time.sleep(0.2) - return default - - -======= ->>>>>>> af4b48f (Handle waiting in Spawner and align Hardware Spawner functionality (#1562)) def combine_name_and_namespace(name_and_namespace): node_name, namespace = name_and_namespace return namespace + ("" if namespace.endswith("/") else "/") + node_name @@ -184,27 +168,14 @@ def main(args=None): controller_manager_name = f"/{controller_manager_name}" try: -<<<<<<< HEAD - if not wait_for_controller_manager( - node, controller_manager_name, controller_manager_timeout - ): - node.get_logger().error("Controller manager not available") - return 1 - -======= ->>>>>>> af4b48f (Handle waiting in Spawner and align Hardware Spawner functionality (#1562)) for controller_name in controller_names: prefixed_controller_name = controller_name if controller_namespace: prefixed_controller_name = controller_namespace + "/" + controller_name -<<<<<<< HEAD - if is_controller_loaded(node, controller_manager_name, prefixed_controller_name): -======= if is_controller_loaded( node, controller_manager_name, controller_name, controller_manager_timeout ): ->>>>>>> af4b48f (Handle waiting in Spawner and align Hardware Spawner functionality (#1562)) node.get_logger().warn( bcolors.WARNING + "Controller already loaded, skipping load_controller" From 68d6ea38fd19e92b2bbdcf5bff06995533a46f72 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Wed, 14 Aug 2024 17:48:39 +0200 Subject: [PATCH 4/9] Update userdoc.rst --- controller_manager/doc/userdoc.rst | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index 5aa3c819b7..b13c5b11c7 100644 --- a/controller_manager/doc/userdoc.rst +++ b/controller_manager/doc/userdoc.rst @@ -128,8 +128,6 @@ There are two scripts to interact with controller manager from launch files: -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER Name of the controller manager ROS node -<<<<<<< HEAD -======= ``hardware_spawner`` ^^^^^^^^^^^^^^^^^^^^^^ @@ -164,7 +162,7 @@ It can be launched independently using the following command or as rqt plugin: * Double-click on a controller or hardware component to show the additional info. * Right-click on a controller or hardware component to show a context menu with options for lifecycle management. ->>>>>>> af4b48f (Handle waiting in Spawner and align Hardware Spawner functionality (#1562)) + Using the Controller Manager in a Process ----------------------------------------- From bc75c66ed6623c61e3f8be4ea4274a41fd3f6551 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Wed, 14 Aug 2024 17:50:06 +0200 Subject: [PATCH 5/9] Update controller_manager_services.py --- .../controller_manager/controller_manager_services.py | 4 ---- 1 file changed, 4 deletions(-) diff --git a/controller_manager/controller_manager/controller_manager_services.py b/controller_manager/controller_manager/controller_manager_services.py index 63115c5f2e..e65413e37c 100644 --- a/controller_manager/controller_manager/controller_manager_services.py +++ b/controller_manager/controller_manager/controller_manager_services.py @@ -132,13 +132,9 @@ def reload_controller_libraries(node, controller_manager_name, force_kill, servi ) -<<<<<<< HEAD -def set_hardware_component_state(node, controller_manager_name, component_name, lifecyle_state): -======= def set_hardware_component_state( node, controller_manager_name, component_name, lifecyle_state, service_timeout=0.0 ): ->>>>>>> af4b48f (Handle waiting in Spawner and align Hardware Spawner functionality (#1562)) request = SetHardwareComponentState.Request() request.name = component_name request.target_state = lifecyle_state From 9b96aa2cbba71dad9d557a1a3105966141bdc2e6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Thu, 15 Aug 2024 19:33:26 +0200 Subject: [PATCH 6/9] Test fixes. --- controller_manager/CMakeLists.txt | 4 + .../controller_manager/spawner.py | 27 ++++- .../test/controller_manager_test_common.hpp | 27 +++-- .../test_controller_spawner_with_type.yaml | 27 +++++ .../test/test_spawner_unspawner.cpp | 101 +----------------- 5 files changed, 79 insertions(+), 107 deletions(-) create mode 100644 controller_manager/test/test_controller_spawner_with_type.yaml diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index 29d489ad81..543daeae29 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -169,11 +169,15 @@ if(BUILD_TESTING) ament_add_gmock( test_spawner_unspawner test/test_spawner_unspawner.cpp + TIMEOUT 120 ) target_include_directories(test_spawner_unspawner PRIVATE include) target_link_libraries(test_spawner_unspawner ${PROJECT_NAME} test_controller) ament_target_dependencies(test_spawner_unspawner ros2_control_test_assets) + install(FILES test/test_controller_spawner_with_type.yaml + DESTINATION test) + ament_add_gmock( test_hardware_management_srvs test/test_hardware_management_srvs.cpp diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index 6496a821d2..12574fe29a 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/controller_manager/controller_manager/spawner.py @@ -19,6 +19,7 @@ import sys import time import warnings +import yaml from controller_manager import ( configure_controller, @@ -79,6 +80,24 @@ 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) @@ -152,7 +171,6 @@ def main(args=None): controller_manager_name = args.controller_manager controller_namespace = args.namespace param_file = args.param_file - controller_type = args.controller_type controller_manager_timeout = args.controller_manager_timeout if param_file and not os.path.isfile(param_file): @@ -182,6 +200,13 @@ def main(args=None): + bcolors.ENDC ) else: + controller_type = ( + None + 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 = prefixed_controller_name + ".type" diff --git a/controller_manager/test/controller_manager_test_common.hpp b/controller_manager/test/controller_manager_test_common.hpp index 5e6e02029f..570190e81c 100644 --- a/controller_manager/test/controller_manager_test_common.hpp +++ b/controller_manager/test/controller_manager_test_common.hpp @@ -67,7 +67,8 @@ class ControllerManagerFixture : public ::testing::Test public: explicit ControllerManagerFixture( const std::string & robot_description = ros2_control_test_assets::minimal_robot_urdf, - const bool & pass_urdf_as_parameter = false) + const bool & pass_urdf_as_parameter = false, + const std::string & cm_namespace = "") : robot_description_(robot_description), pass_urdf_as_parameter_(pass_urdf_as_parameter) { executor_ = std::make_shared(); @@ -75,7 +76,8 @@ class ControllerManagerFixture : public ::testing::Test if (robot_description_.empty()) { cm_ = std::make_shared( - std::make_unique(), executor_, TEST_CM_NAME); + std::make_unique(), executor_, TEST_CM_NAME, + cm_namespace); } else { @@ -85,7 +87,7 @@ class ControllerManagerFixture : public ::testing::Test { cm_ = std::make_shared( std::make_unique(robot_description_, true, true), - executor_, TEST_CM_NAME); + executor_, TEST_CM_NAME, cm_namespace); } else { @@ -94,11 +96,10 @@ class ControllerManagerFixture : public ::testing::Test // this is just a workaround to skip passing cm_ = std::make_shared( - std::make_unique(), executor_, TEST_CM_NAME); + std::make_unique(), executor_, TEST_CM_NAME, + cm_namespace); // mimic topic call - auto msg = std_msgs::msg::String(); - msg.data = robot_description_; - cm_->robot_description_callback(msg); + pass_robot_description_to_cm_and_rm(robot_description_); } } } @@ -134,6 +135,17 @@ class ControllerManagerFixture : public ::testing::Test } } + void pass_robot_description_to_cm_and_rm( + const std::string & robot_description = ros2_control_test_assets::minimal_robot_urdf) + { + // TODO(Manuel) : passing via topic not working in test setup, tested cm does + // not receive msg. Have to check this... + // this is just a workaround to skip passing - mimic topic call + auto msg = std_msgs::msg::String(); + msg.data = robot_description; + cm_->robot_description_callback(msg); + } + void switch_test_controllers( const std::vector & start_controllers, const std::vector & stop_controllers, const int strictness, @@ -157,6 +169,7 @@ class ControllerManagerFixture : public ::testing::Test std::thread updater_; bool run_updater_; const std::string robot_description_; + rclcpp::Time time_; const bool pass_urdf_as_parameter_; }; diff --git a/controller_manager/test/test_controller_spawner_with_type.yaml b/controller_manager/test/test_controller_spawner_with_type.yaml new file mode 100644 index 0000000000..892427bab7 --- /dev/null +++ b/controller_manager/test/test_controller_spawner_with_type.yaml @@ -0,0 +1,27 @@ +ctrl_with_parameters_and_type: + ros__parameters: + 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"] + +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 1eeb489663..017e98937c 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -23,6 +23,7 @@ #include "controller_manager/controller_manager.hpp" #include "controller_manager_test_common.hpp" #include "lifecycle_msgs/msg/state.hpp" +#include "test_chainable_controller/test_chainable_controller.hpp" #include "test_controller/test_controller.hpp" using ::testing::_; @@ -222,51 +223,11 @@ TEST_F(TestLoadController, spawner_test_type_in_arg) std::string(test_controller::TEST_CONTROLLER_CLASS_NAME)), 0); -<<<<<<< HEAD ASSERT_EQ(cm_->get_loaded_controllers().size(), 1ul); auto ctrl_2 = cm_->get_loaded_controllers()[0]; ASSERT_EQ(ctrl_2.info.name, "ctrl_2"); ASSERT_EQ(ctrl_2.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_EQ(ctrl_2.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); -======= - 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 --controller-manager-timeout " - "1.0 -p " + - test_file_path), - 256); - // 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); ->>>>>>> af4b48f (Handle waiting in Spawner and align Hardware Spawner functionality (#1562)) } TEST_F(TestLoadController, unload_on_kill) @@ -284,63 +245,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 --fallback_controllers ctrl_3 ctrl_4 ctrl_5 " - "-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_THAT( - ctrl_1.info.fallback_controllers_names, testing::ElementsAre("ctrl_3", "ctrl_4", "ctrl_5")); - 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_THAT( - ctrl_1.info.fallback_controllers_names, testing::ElementsAre("ctrl_3", "ctrl_4", "ctrl_5")); - 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); - } -} class TestLoadControllerWithoutRobotDescription : public ControllerManagerFixture @@ -429,7 +333,7 @@ class TestLoadControllerWithNamespacedCM public: TestLoadControllerWithNamespacedCM() : ControllerManagerFixture( - ros2_control_test_assets::minimal_robot_urdf, "foo_namespace") + ros2_control_test_assets::minimal_robot_urdf, false, "foo_namespace") { } @@ -699,4 +603,3 @@ TEST_F( 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); } ->>>>>>> af4b48f (Handle waiting in Spawner and align Hardware Spawner functionality (#1562)) From c79f9bcd24d00b13eadc5134a3da27d80ecb2de2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Thu, 15 Aug 2024 20:06:04 +0200 Subject: [PATCH 7/9] Test fixes. --- .../controller_manager/spawner.py | 39 +++++++++++-------- 1 file changed, 22 insertions(+), 17 deletions(-) diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index 12574fe29a..dcfd7c6ca9 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/controller_manager/controller_manager/spawner.py @@ -169,7 +169,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 @@ -178,19 +177,25 @@ 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!" + ) + + 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("/"): - spawner_namespace = node.get_namespace() - if spawner_namespace != "/": + if spawner_namespace and spawner_namespace != "/": controller_manager_name = f"{spawner_namespace}/{controller_manager_name}" else: controller_manager_name = f"/{controller_manager_name}" try: for controller_name in controller_names: - 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, controller_manager_timeout ): @@ -201,7 +206,7 @@ def main(args=None): ) else: controller_type = ( - None + args.controller_type if param_file is None else get_parameter_from_param_file( controller_name, spawner_namespace, param_file, "type" @@ -209,7 +214,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( @@ -224,7 +229,7 @@ def main(args=None): + controller_type + '" for ' + bcolors.BOLD - + prefixed_controller_name + + controller_name + bcolors.ENDC ) else: @@ -234,14 +239,14 @@ def main(args=None): + controller_type + '" for ' + bcolors.BOLD - + prefixed_controller_name + + controller_name + bcolors.ENDC ) return 1 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( @@ -256,7 +261,7 @@ def main(args=None): + param_file + '" for ' + bcolors.BOLD - + prefixed_controller_name + + controller_name + bcolors.ENDC ) else: @@ -266,7 +271,7 @@ def main(args=None): + param_file + '" for ' + bcolors.BOLD - + prefixed_controller_name + + controller_name + bcolors.ENDC ) return 1 @@ -277,7 +282,7 @@ def main(args=None): bcolors.FAIL + "Failed loading controller " + bcolors.BOLD - + prefixed_controller_name + + controller_name + bcolors.ENDC ) return 1 @@ -285,7 +290,7 @@ def main(args=None): bcolors.OKBLUE + "Loaded " + bcolors.BOLD - + prefixed_controller_name + + controller_name + bcolors.ENDC ) @@ -311,7 +316,7 @@ def main(args=None): bcolors.OKGREEN + "Configured and activated " + bcolors.BOLD - + prefixed_controller_name + + controller_name + bcolors.ENDC ) From b01655af00db8eacf1edfc79983e04611579dbac Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Thu, 15 Aug 2024 20:06:42 +0200 Subject: [PATCH 8/9] Fix formatting. --- controller_manager/controller_manager/spawner.py | 7 ++----- controller_manager/test/controller_manager_test_common.hpp | 3 +-- 2 files changed, 3 insertions(+), 7 deletions(-) diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index dcfd7c6ca9..0dd66b840a 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/controller_manager/controller_manager/spawner.py @@ -55,6 +55,7 @@ class bcolors: def first_match(iterable, predicate): return next((n for n in iterable if predicate(n)), None) + def combine_name_and_namespace(name_and_namespace): node_name, namespace = name_and_namespace return namespace + ("" if namespace.endswith("/") else "/") + node_name @@ -287,11 +288,7 @@ def main(args=None): ) return 1 node.get_logger().info( - bcolors.OKBLUE - + "Loaded " - + bcolors.BOLD - + controller_name - + bcolors.ENDC + bcolors.OKBLUE + "Loaded " + bcolors.BOLD + controller_name + bcolors.ENDC ) if not args.load_only: diff --git a/controller_manager/test/controller_manager_test_common.hpp b/controller_manager/test/controller_manager_test_common.hpp index 570190e81c..9b38200f1c 100644 --- a/controller_manager/test/controller_manager_test_common.hpp +++ b/controller_manager/test/controller_manager_test_common.hpp @@ -67,8 +67,7 @@ class ControllerManagerFixture : public ::testing::Test public: explicit ControllerManagerFixture( const std::string & robot_description = ros2_control_test_assets::minimal_robot_urdf, - const bool & pass_urdf_as_parameter = false, - const std::string & cm_namespace = "") + const bool & pass_urdf_as_parameter = false, const std::string & cm_namespace = "") : robot_description_(robot_description), pass_urdf_as_parameter_(pass_urdf_as_parameter) { executor_ = std::make_shared(); From 8954f5ff95aa9aee236a21bed17e5ed99a720f38 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Thu, 15 Aug 2024 20:18:52 +0200 Subject: [PATCH 9/9] Update controller_manager/doc/userdoc.rst --- controller_manager/doc/userdoc.rst | 15 --------------- 1 file changed, 15 deletions(-) diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index b13c5b11c7..f5e962047f 100644 --- a/controller_manager/doc/userdoc.rst +++ b/controller_manager/doc/userdoc.rst @@ -147,21 +147,6 @@ There are two scripts to interact with controller manager from launch files: --activate Activates the given components. Note: Components are by default configured before activated. --configure Configures the given components. -rqt_controller_manager ----------------------- -A GUI tool to interact with the controller manager services to be able to switch the lifecycle states of the controllers as well as the hardware components. - -.. image:: images/rqt_controller_manager.png - -It can be launched independently using the following command or as rqt plugin: - -.. code-block:: console - - ros2 run rqt_controller_manager rqt_controller_manager - -* Double-click on a controller or hardware component to show the additional info. -* Right-click on a controller or hardware component to show a context menu with options for lifecycle management. - Using the Controller Manager in a Process -----------------------------------------