From b48df964c1489476ebf6b72eb5a34934ecf1179f Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Wed, 14 Aug 2024 16:24:23 +0100 Subject: [PATCH 01/10] Handle waiting in Spawner and align Hardware Spawner functionality (#1562) (cherry picked from commit af4b48f75a0ff4c57f30df3477f9fa2786e07965) # Conflicts: # controller_manager/controller_manager/spawner.py # controller_manager/test/test_spawner_unspawner.cpp --- .../controller_manager_services.py | 37 +- .../controller_manager/hardware_spawner.py | 77 +-- .../controller_manager/spawner.py | 78 +-- .../controller_manager/unspawner.py | 6 + controller_manager/doc/userdoc.rst | 20 + .../test/test_spawner_unspawner.cpp | 461 +++++++++++++++++- 6 files changed, 547 insertions(+), 132 deletions(-) diff --git a/controller_manager/controller_manager/controller_manager_services.py b/controller_manager/controller_manager/controller_manager_services.py index 62b350f7d3..be11a4b782 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( @@ -128,7 +133,7 @@ def reload_controller_libraries(node, controller_manager_name, force_kill, servi def set_hardware_component_state( - node, controller_manager_name, component_name, lifecyle_state, service_timeout=10.0 + node, controller_manager_name, component_name, lifecyle_state, service_timeout=0.0 ): request = SetHardwareComponentState.Request() request.name = component_name @@ -165,7 +170,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 c95fb6181e..3e3a487c6a 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,17 +45,6 @@ def first_match(iterable, predicate): return next((n for n in iterable if predicate(n)), None) -def wait_for_value_or(function, node, timeout, default, description): - while node.get_clock().now() < timeout: - if result := function(): - return result - node.get_logger().info( - f"Waiting for {description}", throttle_duration_sec=2, skip_first=True - ) - time.sleep(0.2) - return default - - def combine_name_and_namespace(name_and_namespace): node_name, namespace = name_and_namespace return namespace + ("" if namespace.endswith("/") else "/") + node_name @@ -75,35 +66,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( @@ -167,10 +134,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", @@ -202,13 +168,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) @@ -218,6 +186,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 cf78442856..dd252a20b3 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.parameter import get_parameter_value from rclpy.signals import SignalHandlerOptions @@ -55,17 +55,6 @@ def first_match(iterable, predicate): return next((n for n in iterable if predicate(n)), None) -def wait_for_value_or(function, node, timeout, default, description): - while node.get_clock().now() < timeout: - if result := function(): - return result - node.get_logger().info( - f"Waiting for {description}", throttle_duration_sec=2, skip_first=True - ) - time.sleep(0.2) - return default - - def combine_name_and_namespace(name_and_namespace): node_name, namespace = name_and_namespace return namespace + ("" if namespace.endswith("/") else "/") + node_name @@ -86,46 +75,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) @@ -178,8 +129,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", @@ -211,20 +162,18 @@ 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 - ): - node.get_logger().error( - bcolors.FAIL + "Controller manager not available" + bcolors.ENDC - ) - return 1 - 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" @@ -386,6 +335,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 5acf05e65d..e42d85aee9 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 @@ -57,6 +58,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 47c19fa907..84f78d33e2 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`` @@ -129,6 +130,25 @@ 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 +``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. diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index 1ecc3164b8..fbada642c8 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) @@ -252,11 +253,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) @@ -274,3 +315,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 c9649c17510a4dd8c6624691ddabf7e3017cc148 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Wed, 14 Aug 2024 17:46:00 +0200 Subject: [PATCH 02/10] Update hardware_spawner.py From a3bfc50964b4ced8fcc00a809bf2206487d430d6 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Wed, 14 Aug 2024 17:47:08 +0200 Subject: [PATCH 03/10] Update spawner.py --- controller_manager/controller_manager/spawner.py | 4 ---- 1 file changed, 4 deletions(-) diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index dd252a20b3..8829cf3488 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/controller_manager/controller_manager/spawner.py @@ -167,13 +167,9 @@ def main(args=None): 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 f243467889dd54e958e569799e8bf6be77751827 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Wed, 14 Aug 2024 17:48:39 +0200 Subject: [PATCH 04/10] Update userdoc.rst --- controller_manager/doc/userdoc.rst | 1 + 1 file changed, 1 insertion(+) diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index 84f78d33e2..55e36b1e6f 100644 --- a/controller_manager/doc/userdoc.rst +++ b/controller_manager/doc/userdoc.rst @@ -164,6 +164,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. + Using the Controller Manager in a Process ----------------------------------------- From bb1bc17f8c520b0a6961caed1bd30a1acc14b775 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Wed, 14 Aug 2024 17:50:06 +0200 Subject: [PATCH 05/10] Update controller_manager_services.py From 5252f28cf225fbc7263a67dbb58a34e1403c7712 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 06/10] 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 bfa6b35434..500b60f309 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -182,6 +182,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 @@ -189,6 +190,9 @@ if(BUILD_TESTING) ros2_control_test_assets::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 8829cf3488..fc5ba7755c 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, @@ -80,6 +81,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) parser = argparse.ArgumentParser() @@ -146,7 +165,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): @@ -176,6 +194,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 2d10117fc9..8ade6d224e 100644 --- a/controller_manager/test/controller_manager_test_common.hpp +++ b/controller_manager/test/controller_manager_test_common.hpp @@ -66,7 +66,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(); @@ -74,7 +75,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 { @@ -84,7 +86,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 { @@ -93,11 +95,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_); } } time_ = rclcpp::Time(0, 0, cm_->get_node_clock_interface()->get_clock()->get_clock_type()); @@ -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, @@ -156,6 +168,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_; rclcpp::Time time_; }; 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 fbada642c8..be261ece96 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::_; @@ -253,51 +254,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) @@ -315,63 +276,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 @@ -460,7 +364,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") { } @@ -730,4 +634,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 3a49a0c3a1cfd2a9982daac53086590e5da44ba3 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 07/10] 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 fc5ba7755c..ee2b04659c 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/controller_manager/controller_manager/spawner.py @@ -163,7 +163,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 @@ -172,19 +171,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 ): @@ -195,7 +200,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" @@ -203,7 +208,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( @@ -218,7 +223,7 @@ def main(args=None): + controller_type + '" for ' + bcolors.BOLD - + prefixed_controller_name + + controller_name + bcolors.ENDC ) else: @@ -228,14 +233,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( @@ -250,7 +255,7 @@ def main(args=None): + param_file + '" for ' + bcolors.BOLD - + prefixed_controller_name + + controller_name + bcolors.ENDC ) else: @@ -260,7 +265,7 @@ def main(args=None): + param_file + '" for ' + bcolors.BOLD - + prefixed_controller_name + + controller_name + bcolors.ENDC ) return 1 @@ -271,7 +276,7 @@ def main(args=None): bcolors.FAIL + "Failed loading controller " + bcolors.BOLD - + prefixed_controller_name + + controller_name + bcolors.ENDC ) return 1 @@ -279,7 +284,7 @@ def main(args=None): bcolors.OKBLUE + "Loaded " + bcolors.BOLD - + prefixed_controller_name + + controller_name + bcolors.ENDC ) @@ -305,7 +310,7 @@ def main(args=None): bcolors.OKGREEN + "Configured and activated " + bcolors.BOLD - + prefixed_controller_name + + controller_name + bcolors.ENDC ) From 72a67ede67966741dcbbca91dea90514fbc4d614 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 08/10] Fix formatting. --- controller_manager/controller_manager/spawner.py | 6 +----- controller_manager/test/controller_manager_test_common.hpp | 3 +-- 2 files changed, 2 insertions(+), 7 deletions(-) diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index ee2b04659c..8061f5fab1 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/controller_manager/controller_manager/spawner.py @@ -281,11 +281,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 8ade6d224e..0e14714efd 100644 --- a/controller_manager/test/controller_manager_test_common.hpp +++ b/controller_manager/test/controller_manager_test_common.hpp @@ -66,8 +66,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 77c8e4feaa6e2381b39567dbd831db81678d16ea Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Thu, 15 Aug 2024 20:20:21 +0200 Subject: [PATCH 09/10] Fixup compilation. --- controller_manager/test/controller_manager_test_common.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/controller_manager/test/controller_manager_test_common.hpp b/controller_manager/test/controller_manager_test_common.hpp index 0e14714efd..bcdba4a907 100644 --- a/controller_manager/test/controller_manager_test_common.hpp +++ b/controller_manager/test/controller_manager_test_common.hpp @@ -169,7 +169,6 @@ class ControllerManagerFixture : public ::testing::Test const std::string robot_description_; rclcpp::Time time_; const bool pass_urdf_as_parameter_; - rclcpp::Time time_; }; class TestControllerManagerSrvs From 91482414123d5cd0d98bc8409ed26c0b826af570 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Thu, 15 Aug 2024 20:21:23 +0200 Subject: [PATCH 10/10] Update controller_manager/doc/userdoc.rst --- controller_manager/doc/userdoc.rst | 1 - 1 file changed, 1 deletion(-) diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index 55e36b1e6f..84f78d33e2 100644 --- a/controller_manager/doc/userdoc.rst +++ b/controller_manager/doc/userdoc.rst @@ -164,7 +164,6 @@ 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. - Using the Controller Manager in a Process -----------------------------------------