From 324a01207cb04419fd24138fff092785ed25b57c Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 23 Apr 2024 12:00:26 +0200 Subject: [PATCH 01/21] Add integration test with a lot of controller spawners With the current implementation I expect this to fail most of the times. --- controller_manager/CMakeLists.txt | 10 + .../test/test_spawner_integration.py | 198 ++++++++++++++++++ ros2_control_test_assets/CMakeLists.txt | 4 + .../urdf/test_description_mock.urdf | 90 ++++++++ 4 files changed, 302 insertions(+) create mode 100644 controller_manager/test/test_spawner_integration.py create mode 100644 ros2_control_test_assets/urdf/test_description_mock.urdf diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index e267856eb1..8385b13dcf 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -201,6 +201,16 @@ if(BUILD_TESTING) ament_target_dependencies(test_hardware_management_srvs controller_manager_msgs ) + + install( + DIRECTORY test + DESTINATION share/${PROJECT_NAME} + ) + + find_package(launch_testing_ament_cmake) + add_launch_test(test/test_spawner_integration.py + TIMEOUT 180 + ) endif() install( diff --git a/controller_manager/test/test_spawner_integration.py b/controller_manager/test/test_spawner_integration.py new file mode 100644 index 0000000000..9d68f7326f --- /dev/null +++ b/controller_manager/test/test_spawner_integration.py @@ -0,0 +1,198 @@ +# Copyright 2024 FZI Forschungszentrum Informatik +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# Author: Felix Exner + +import time +import pytest +import unittest + +import launch +import launch_testing.actions + +from launch.substitutions import ( + Command, + FindExecutable, + PathJoinSubstitution, +) +from launch_ros.substitutions import FindPackageShare +from launch_ros.parameter_descriptions import ParameterFile + +from launch_ros.actions import Node as LaunchNode + +# Imports for tests +import rclpy +from rclpy.node import Node +import logging + +from controller_manager_msgs.srv import ListControllers + +CONTROLLER_SPAWNER_TIMEOUT = "10" + +active_controllers = [ + "joint_state_broadcaster0", + "joint_state_broadcaster1", + "joint_state_broadcaster2", + "joint_state_broadcaster3", + "joint_state_broadcaster4", + "joint_state_broadcaster5", + "joint_state_broadcaster6", + "joint_state_broadcaster7", + "joint_state_broadcaster8", + "joint_state_broadcaster9", + "joint_state_broadcaster10", + "joint_state_broadcaster11", + "joint_state_broadcaster12", + "joint_state_broadcaster13", + "joint_state_broadcaster14", + "joint_state_broadcaster15", + "joint_state_broadcaster16", + "joint_state_broadcaster17", + "joint_state_broadcaster18", + "joint_state_broadcaster19", + "joint_state_broadcaster20", + "joint_state_broadcaster21", + "joint_state_broadcaster22", + "joint_state_broadcaster23", + "joint_state_broadcaster24", + "joint_state_broadcaster25", + "joint_state_broadcaster26", + "joint_state_broadcaster27", + "joint_state_broadcaster28", + "joint_state_broadcaster29", + "joint_state_broadcaster30", + "joint_state_broadcaster31", + "joint_state_broadcaster32", + "joint_state_broadcaster33", + "joint_state_broadcaster34", + "joint_state_broadcaster35", + "joint_state_broadcaster36", + "joint_state_broadcaster37", + "joint_state_broadcaster38", + "joint_state_broadcaster39", + "joint_state_broadcaster40", + "joint_state_broadcaster41", + "joint_state_broadcaster42", + "joint_state_broadcaster43", + "joint_state_broadcaster44", + "joint_state_broadcaster45", + "joint_state_broadcaster46", + "joint_state_broadcaster47", + "joint_state_broadcaster48", + "joint_state_broadcaster49", + "fts_broadcaster", + "base_controller", + "joint1_controller", + "joint2_controller", +] + + +def controller_spawner(controllers, active=True): + inactive_flags = ["--inactive"] if not active else [] + return LaunchNode( + package="controller_manager", + executable="spawner", + arguments=[ + "--controller-manager", + "/controller_manager", + "--controller-manager-timeout", + CONTROLLER_SPAWNER_TIMEOUT, + ] + + inactive_flags + + controllers, + ) + + +@pytest.mark.launch_test +def generate_test_description(): + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [ + FindPackageShare("ros2_control_test_assets"), + "urdf", + "test_description_mock.urdf", + ] + ), + ] + ) + robot_state_publisher_node = LaunchNode( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[{"robot_description": robot_description_content}], + ) + + control_node = LaunchNode( + package="controller_manager", + executable="ros2_control_node", + parameters=[ + ParameterFile( + PathJoinSubstitution( + [FindPackageShare("controller_manager"), "test", "test_controllers.yaml"] + ) + ) + ], + output="screen", + ) + + spawners = [controller_spawner([x]) for x in active_controllers] + + return launch.LaunchDescription( + [ + robot_state_publisher_node, + control_node, + launch_testing.actions.ReadyToTest(), + ] + + spawners + ) + + +class TestControllersRunning(unittest.TestCase): + @classmethod + def setUpClass(cls): + # Initialize the ROS context + rclpy.init() + cls.node = Node("controller_spawner_test") + + def _wait_for_service(self, srv_name, srv_type, timeout=10): + client = self.node.create_client(srv_type, srv_name) + + logging.info("Waiting for service '%s' with timeout %fs...", srv_name, timeout) + if client.wait_for_service(timeout) is False: + raise Exception(f"Could not reach service '{srv_name}' within timeout of {timeout}") + logging.info(" Successfully connected to service '%s'", srv_name) + + return client + + def test_all_controllers_available(self): + client = self._wait_for_service( + srv_name="controller_manager/list_controllers", srv_type=ListControllers + ) + # This is basically a dirty hack. It would be better to add events to all the spawners + # exiting and emit ReadyToTest() only after all of them have quit. One problem there: They + # do not necessarily quit, but can get into a deadlock waiting for a service response. + time.sleep(30) + request = ListControllers.Request() + future = client.call_async(request) + rclpy.spin_until_future_complete(self.node, future) + if future.result() is None: + raise Exception( + f"Error while calling service '{client.srv_name}': {future.exception()}" + ) + + result = future.result() + self.assertEqual(len(result.controller), len(active_controllers)) diff --git a/ros2_control_test_assets/CMakeLists.txt b/ros2_control_test_assets/CMakeLists.txt index d1bb895eed..8182766b44 100644 --- a/ros2_control_test_assets/CMakeLists.txt +++ b/ros2_control_test_assets/CMakeLists.txt @@ -14,6 +14,10 @@ install( DIRECTORY include/ DESTINATION include/ros2_control_test_assets ) +install( + DIRECTORY urdf + DESTINATION share/${PROJECT_NAME} +) install(TARGETS ros2_control_test_assets EXPORT export_ros2_control_test_assets ARCHIVE DESTINATION lib diff --git a/ros2_control_test_assets/urdf/test_description_mock.urdf b/ros2_control_test_assets/urdf/test_description_mock.urdf new file mode 100644 index 0000000000..9509c60dd9 --- /dev/null +++ b/ros2_control_test_assets/urdf/test_description_mock.urdf @@ -0,0 +1,90 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + mock_components/GenericSystem + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From 7ee0fad653c0c1783af5a9c3dd91fda284c37c2f Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 23 Apr 2024 12:15:39 +0200 Subject: [PATCH 02/21] Do not wait for controller_manager node This waiting mechanism seems to be very error-prone and is strictly not needed since we wait for all the services, anyway. NOTE: This change removes the ability to specify the controller_manager_timeout. --- controller_manager/controller_manager/spawner.py | 8 -------- 1 file changed, 8 deletions(-) diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index 1d11d80fe1..e67a524c90 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/controller_manager/controller_manager/spawner.py @@ -217,14 +217,6 @@ 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: From e25ea4fe862844810ec89182bc9d5edfe9c8f6e9 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 23 Apr 2024 12:32:44 +0200 Subject: [PATCH 03/21] Re-add controller_manager_timeout by setting it as the timeout for the first service call. --- controller_manager/controller_manager/spawner.py | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index e67a524c90..5e4acaa13b 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/controller_manager/controller_manager/spawner.py @@ -130,8 +130,12 @@ def wait_for_controller_manager(node, controller_manager, timeout_duration): 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, controller_manager_timeout=10.0 +): + controllers = list_controllers( + node, controller_manager, service_timeout=controller_manager_timeout + ).controller return any(c.name == controller_name for c in controllers) @@ -222,7 +226,12 @@ def main(args=None): if controller_namespace: prefixed_controller_name = controller_namespace + "/" + controller_name - if is_controller_loaded(node, controller_manager_name, prefixed_controller_name): + if is_controller_loaded( + node, + controller_manager_name, + prefixed_controller_name, + controller_manager_timeout=controller_manager_timeout, + ): node.get_logger().warn( bcolors.WARNING + "Controller already loaded, skipping load_controller" From c7bbbb56fa2c054e4fb7623507e61e71acc41976 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 23 Apr 2024 12:33:06 +0200 Subject: [PATCH 04/21] Resolve deadlock in calling controller_manager services Apparently, sercice calls can end up in a deadlock where the server says it cannot send the response to the client. In that case, if we spin_until_future_complete() we will be in a deadlock forever. Hence, this commit adds a timeout and retry mechanism to the service call abstraction. --- .../controller_manager_services.py | 22 +++++++++++++------ 1 file changed, 15 insertions(+), 7 deletions(-) diff --git a/controller_manager/controller_manager/controller_manager_services.py b/controller_manager/controller_manager/controller_manager_services.py index 62b350f7d3..51d55e1507 100644 --- a/controller_manager/controller_manager/controller_manager_services.py +++ b/controller_manager/controller_manager/controller_manager_services.py @@ -28,7 +28,9 @@ import rclpy -def service_caller(node, service_name, service_type, request, service_timeout=10.0): +def service_caller( + node, service_name, service_type, request, service_timeout=10.0, max_attempts=3 +): cli = node.create_client(service_type, service_name) if not cli.service_is_ready(): @@ -39,12 +41,18 @@ def service_caller(node, service_name, service_type, request, service_timeout=10 raise RuntimeError(f"Could not contact service {service_name}") node.get_logger().debug(f"requester: making request: {request}\n") - future = cli.call_async(request) - rclpy.spin_until_future_complete(node, future) - if future.result() is not None: - return future.result() - else: - raise RuntimeError(f"Exception while calling service: {future.exception()}") + future = None + for attempt in range(max_attempts): # This is a rather arbitrary retry number + future = cli.call_async(request) + rclpy.spin_until_future_complete(node, future, timeout_sec=service_timeout) + if future.result() is None: + node.get_logger().warning( + f"Failed getting a result from calling {service_name} in " + f"{service_timeout}. (Attempt {attempt} of {max_attempts}.)" + ) + else: + return future.result() + raise RuntimeError(f"Exception while calling service {service_name}: {future.exception()}") def configure_controller(node, controller_manager_name, controller_name, service_timeout=10.0): From 9b4ecfc23d4898e4587603e8e4e6e657f22feba1 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 23 Apr 2024 13:04:34 +0200 Subject: [PATCH 05/21] Add missing controllers file --- controller_manager/test/test_controllers.yaml | 145 ++++++++++++++++++ 1 file changed, 145 insertions(+) create mode 100644 controller_manager/test/test_controllers.yaml diff --git a/controller_manager/test/test_controllers.yaml b/controller_manager/test/test_controllers.yaml new file mode 100644 index 0000000000..3370e6a966 --- /dev/null +++ b/controller_manager/test/test_controllers.yaml @@ -0,0 +1,145 @@ +--- +controller_manager: + ros__parameters: + update_rate: 100 + + joint_state_broadcaster0: + type: joint_state_broadcaster/JointStateBroadcaster + joint_state_broadcaster1: + type: joint_state_broadcaster/JointStateBroadcaster + joint_state_broadcaster2: + type: joint_state_broadcaster/JointStateBroadcaster + joint_state_broadcaster3: + type: joint_state_broadcaster/JointStateBroadcaster + joint_state_broadcaster4: + type: joint_state_broadcaster/JointStateBroadcaster + joint_state_broadcaster5: + type: joint_state_broadcaster/JointStateBroadcaster + joint_state_broadcaster6: + type: joint_state_broadcaster/JointStateBroadcaster + joint_state_broadcaster7: + type: joint_state_broadcaster/JointStateBroadcaster + joint_state_broadcaster8: + type: joint_state_broadcaster/JointStateBroadcaster + joint_state_broadcaster9: + type: joint_state_broadcaster/JointStateBroadcaster + joint_state_broadcaster10: + type: joint_state_broadcaster/JointStateBroadcaster + joint_state_broadcaster11: + type: joint_state_broadcaster/JointStateBroadcaster + joint_state_broadcaster12: + type: joint_state_broadcaster/JointStateBroadcaster + joint_state_broadcaster13: + type: joint_state_broadcaster/JointStateBroadcaster + joint_state_broadcaster14: + type: joint_state_broadcaster/JointStateBroadcaster + joint_state_broadcaster15: + type: joint_state_broadcaster/JointStateBroadcaster + joint_state_broadcaster16: + type: joint_state_broadcaster/JointStateBroadcaster + joint_state_broadcaster17: + type: joint_state_broadcaster/JointStateBroadcaster + joint_state_broadcaster18: + type: joint_state_broadcaster/JointStateBroadcaster + joint_state_broadcaster19: + type: joint_state_broadcaster/JointStateBroadcaster + joint_state_broadcaster20: + type: joint_state_broadcaster/JointStateBroadcaster + joint_state_broadcaster21: + type: joint_state_broadcaster/JointStateBroadcaster + joint_state_broadcaster22: + type: joint_state_broadcaster/JointStateBroadcaster + joint_state_broadcaster23: + type: joint_state_broadcaster/JointStateBroadcaster + joint_state_broadcaster24: + type: joint_state_broadcaster/JointStateBroadcaster + joint_state_broadcaster25: + type: joint_state_broadcaster/JointStateBroadcaster + joint_state_broadcaster26: + type: joint_state_broadcaster/JointStateBroadcaster + joint_state_broadcaster27: + type: joint_state_broadcaster/JointStateBroadcaster + joint_state_broadcaster28: + type: joint_state_broadcaster/JointStateBroadcaster + joint_state_broadcaster29: + type: joint_state_broadcaster/JointStateBroadcaster + joint_state_broadcaster30: + type: joint_state_broadcaster/JointStateBroadcaster + joint_state_broadcaster31: + type: joint_state_broadcaster/JointStateBroadcaster + joint_state_broadcaster32: + type: joint_state_broadcaster/JointStateBroadcaster + joint_state_broadcaster33: + type: joint_state_broadcaster/JointStateBroadcaster + joint_state_broadcaster34: + type: joint_state_broadcaster/JointStateBroadcaster + joint_state_broadcaster35: + type: joint_state_broadcaster/JointStateBroadcaster + joint_state_broadcaster36: + type: joint_state_broadcaster/JointStateBroadcaster + joint_state_broadcaster37: + type: joint_state_broadcaster/JointStateBroadcaster + joint_state_broadcaster38: + type: joint_state_broadcaster/JointStateBroadcaster + joint_state_broadcaster39: + type: joint_state_broadcaster/JointStateBroadcaster + joint_state_broadcaster40: + type: joint_state_broadcaster/JointStateBroadcaster + joint_state_broadcaster41: + type: joint_state_broadcaster/JointStateBroadcaster + joint_state_broadcaster42: + type: joint_state_broadcaster/JointStateBroadcaster + joint_state_broadcaster43: + type: joint_state_broadcaster/JointStateBroadcaster + joint_state_broadcaster44: + type: joint_state_broadcaster/JointStateBroadcaster + joint_state_broadcaster45: + type: joint_state_broadcaster/JointStateBroadcaster + joint_state_broadcaster46: + type: joint_state_broadcaster/JointStateBroadcaster + joint_state_broadcaster47: + type: joint_state_broadcaster/JointStateBroadcaster + joint_state_broadcaster48: + type: joint_state_broadcaster/JointStateBroadcaster + joint_state_broadcaster49: + type: joint_state_broadcaster/JointStateBroadcaster + + fts_broadcaster: + type: force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster + + base_controller: + type: position_controllers/JointGroupPositionController + + joint1_controller: + type: position_controllers/JointGroupPositionController + + joint2_controller: + type: position_controllers/JointGroupPositionController + +fts_broadcaster: + ros__parameters: + sensor_name: base_ft_sensor + state_interface_names: + - force.x + - force.y + - force.z + - torque.x + - torque.y + - torque.z + frame_id: base_link + topic_name: base_wrench + +base_controller: + ros__parameters: + joints: + - base_joint + +joint1_controller: + ros__parameters: + joints: + - joint1 + +joint2_controller: + ros__parameters: + joints: + - joint2 From de424ac417bcbcdc3c2633a4a0392377a8c53f9e Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Wed, 14 Aug 2024 17:33:30 +0200 Subject: [PATCH 06/21] Update controller_manager/controller_manager/controller_manager_services.py --- .../controller_manager/controller_manager_services.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controller_manager/controller_manager/controller_manager_services.py b/controller_manager/controller_manager/controller_manager_services.py index 4763fa88c7..30a46f4a92 100644 --- a/controller_manager/controller_manager/controller_manager_services.py +++ b/controller_manager/controller_manager/controller_manager_services.py @@ -33,7 +33,7 @@ class ServiceNotFoundError(Exception): def service_caller( - node, service_name, service_type, request, service_timeout=10.0, max_attempts=3 + node, service_name, service_type, request, service_timeout=0.0, max_attempts=3 ): cli = node.create_client(service_type, service_name) From 484cd4af67d980e65a64ef1799c6c31f5456486e Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 14 Aug 2024 20:52:54 +0200 Subject: [PATCH 07/21] Add call_timeout parameter to service_caller --- .../controller_manager/controller_manager_services.py | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/controller_manager/controller_manager/controller_manager_services.py b/controller_manager/controller_manager/controller_manager_services.py index 30a46f4a92..9761399c7c 100644 --- a/controller_manager/controller_manager/controller_manager_services.py +++ b/controller_manager/controller_manager/controller_manager_services.py @@ -33,7 +33,13 @@ class ServiceNotFoundError(Exception): def service_caller( - node, service_name, service_type, request, service_timeout=0.0, max_attempts=3 + node, + service_name, + service_type, + request, + service_timeout=0.0, + call_timeout=10.0, + max_attempts=3, ): cli = node.create_client(service_type, service_name) @@ -49,7 +55,7 @@ def service_caller( future = None for attempt in range(max_attempts): # This is a rather arbitrary retry number future = cli.call_async(request) - rclpy.spin_until_future_complete(node, future, timeout_sec=service_timeout) + rclpy.spin_until_future_complete(node, future, timeout_sec=call_timeout) if future.result() is None: node.get_logger().warning( f"Failed getting a result from calling {service_name} in " From 77f7e16121cf7740eeec0b3c1d438d7030d0436f Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 14 Aug 2024 20:54:24 +0200 Subject: [PATCH 08/21] Add documentation to service_caller parameters --- .../controller_manager_services.py | 26 ++++++++++++++++++- 1 file changed, 25 insertions(+), 1 deletion(-) diff --git a/controller_manager/controller_manager/controller_manager_services.py b/controller_manager/controller_manager/controller_manager_services.py index 9761399c7c..33dbe31c70 100644 --- a/controller_manager/controller_manager/controller_manager_services.py +++ b/controller_manager/controller_manager/controller_manager_services.py @@ -41,6 +41,30 @@ def service_caller( call_timeout=10.0, max_attempts=3, ): + """ + Abstraction of a service call. + + Has an optional timeout to find the service and a mechanism + to retry a call of no response is received. + + @param node Node object to be associated with + @type rclpy.node.Node + @param service_name Service URL + @type str + @param request The request to be sent + @type service request type + @param service_timeout Timeout (in seconds) to wait until the service is available. 0 means + waiting forever, retrying every 10 seconds. + @type float + @param call_timeout Timeout (in seconds) for getting a response + @type float + @param max_attempts Number of attempts until a valid response is received. With some + middlewares it can happen, that the service response doesn't reach the client leaving it in + a waiting state forever. + @type int + @return The service response + + """ cli = node.create_client(service_type, service_name) while not cli.service_is_ready(): @@ -53,7 +77,7 @@ def service_caller( node.get_logger().debug(f"requester: making request: {request}\n") future = None - for attempt in range(max_attempts): # This is a rather arbitrary retry number + for attempt in range(max_attempts): future = cli.call_async(request) rclpy.spin_until_future_complete(node, future, timeout_sec=call_timeout) if future.result() is None: From 3e3adeda38d562828dc7ff4858b2171e6285bf8d Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 14 Aug 2024 21:22:27 +0200 Subject: [PATCH 09/21] Make base_joint a revolute joint --- ros2_control_test_assets/urdf/test_description_mock.urdf | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/ros2_control_test_assets/urdf/test_description_mock.urdf b/ros2_control_test_assets/urdf/test_description_mock.urdf index 9509c60dd9..715de97540 100644 --- a/ros2_control_test_assets/urdf/test_description_mock.urdf +++ b/ros2_control_test_assets/urdf/test_description_mock.urdf @@ -1,10 +1,11 @@ - + + From 85c593c46c61b5cb5dfe86618a4774efd4cd2a4f Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 14 Aug 2024 22:48:56 +0200 Subject: [PATCH 10/21] Use attempt counting with starting from 1 --- .../controller_manager/controller_manager_services.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controller_manager/controller_manager/controller_manager_services.py b/controller_manager/controller_manager/controller_manager_services.py index 33dbe31c70..ef37fd6591 100644 --- a/controller_manager/controller_manager/controller_manager_services.py +++ b/controller_manager/controller_manager/controller_manager_services.py @@ -83,7 +83,7 @@ def service_caller( if future.result() is None: node.get_logger().warning( f"Failed getting a result from calling {service_name} in " - f"{service_timeout}. (Attempt {attempt} of {max_attempts}.)" + f"{service_timeout}. (Attempt {attempt+1} of {max_attempts}.)" ) else: return future.result() From a98ca8128ac5118df43ebf3f804d4b98bff5de8f Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 14 Aug 2024 23:09:27 +0200 Subject: [PATCH 11/21] Add test_dependency on launch_testing_ament_cmake --- controller_manager/CMakeLists.txt | 2 +- controller_manager/package.xml | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index dc1a20162f..6b51222fd0 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -216,7 +216,7 @@ if(BUILD_TESTING) DESTINATION share/${PROJECT_NAME} ) - find_package(launch_testing_ament_cmake) + find_package(launch_testing_ament_cmake REQUIRED) add_launch_test(test/test_spawner_integration.py TIMEOUT 180 ) diff --git a/controller_manager/package.xml b/controller_manager/package.xml index 9f9a9bf796..91a89c8bde 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -31,6 +31,7 @@ ament_cmake_gmock ament_cmake_pytest + launch_testing_ament_cmake python3-coverage hardware_interface_testing ros2_control_test_assets From aa0aa976d3ede287a81bee2c6b3d4c20b2d12751 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 14 Aug 2024 23:10:50 +0200 Subject: [PATCH 12/21] Correct error message on failed service call --- .../controller_manager/controller_manager_services.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/controller_manager/controller_manager/controller_manager_services.py b/controller_manager/controller_manager/controller_manager_services.py index ef37fd6591..418d507afb 100644 --- a/controller_manager/controller_manager/controller_manager_services.py +++ b/controller_manager/controller_manager/controller_manager_services.py @@ -87,7 +87,9 @@ def service_caller( ) else: return future.result() - raise RuntimeError(f"Exception while calling service {service_name}: {future.exception()}") + raise RuntimeError( + f"Could not successfully call service {service_name} in {max_attempts} attempts." + ) def configure_controller(node, controller_manager_name, controller_name, service_timeout=0.0): From 2120fd875f1ef7deb801be658bf65cd5bf3291a1 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 14 Aug 2024 23:27:43 +0200 Subject: [PATCH 13/21] Add more test dependencies --- controller_manager/package.xml | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/controller_manager/package.xml b/controller_manager/package.xml index 91a89c8bde..440154631e 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -31,10 +31,12 @@ ament_cmake_gmock ament_cmake_pytest + hardware_interface_testing launch_testing_ament_cmake python3-coverage - hardware_interface_testing + robot_state_publisher ros2_control_test_assets + xacro ament_cmake From a77c600b225f467fb670aec8401930777018ef83 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 14 Aug 2024 23:41:15 +0200 Subject: [PATCH 14/21] Add used controllers to test dependencies --- controller_manager/package.xml | 3 +++ 1 file changed, 3 insertions(+) diff --git a/controller_manager/package.xml b/controller_manager/package.xml index 440154631e..862ea027db 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -31,8 +31,11 @@ ament_cmake_gmock ament_cmake_pytest + force_torque_sensor_broadcaster hardware_interface_testing + joint_state_broadcaster launch_testing_ament_cmake + position_controllers python3-coverage robot_state_publisher ros2_control_test_assets From 14028ea023a89bf1eb3c0c4f042cfa2bfecfd86f Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 15 Aug 2024 00:06:50 +0200 Subject: [PATCH 15/21] Do not depend on other controllers --- controller_manager/package.xml | 4 - controller_manager/test/test_controllers.yaml | 140 +++++++----------- 2 files changed, 50 insertions(+), 94 deletions(-) diff --git a/controller_manager/package.xml b/controller_manager/package.xml index 862ea027db..b4b295656e 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -31,13 +31,9 @@ ament_cmake_gmock ament_cmake_pytest - force_torque_sensor_broadcaster hardware_interface_testing - joint_state_broadcaster launch_testing_ament_cmake - position_controllers python3-coverage - robot_state_publisher ros2_control_test_assets xacro diff --git a/controller_manager/test/test_controllers.yaml b/controller_manager/test/test_controllers.yaml index 3370e6a966..a465e81ad6 100644 --- a/controller_manager/test/test_controllers.yaml +++ b/controller_manager/test/test_controllers.yaml @@ -4,142 +4,102 @@ controller_manager: update_rate: 100 joint_state_broadcaster0: - type: joint_state_broadcaster/JointStateBroadcaster + type: controller_manager/test_controller joint_state_broadcaster1: - type: joint_state_broadcaster/JointStateBroadcaster + type: controller_manager/test_controller joint_state_broadcaster2: - type: joint_state_broadcaster/JointStateBroadcaster + type: controller_manager/test_controller joint_state_broadcaster3: - type: joint_state_broadcaster/JointStateBroadcaster + type: controller_manager/test_controller joint_state_broadcaster4: - type: joint_state_broadcaster/JointStateBroadcaster + type: controller_manager/test_controller joint_state_broadcaster5: - type: joint_state_broadcaster/JointStateBroadcaster + type: controller_manager/test_controller joint_state_broadcaster6: - type: joint_state_broadcaster/JointStateBroadcaster + type: controller_manager/test_controller joint_state_broadcaster7: - type: joint_state_broadcaster/JointStateBroadcaster + type: controller_manager/test_controller joint_state_broadcaster8: - type: joint_state_broadcaster/JointStateBroadcaster + type: controller_manager/test_controller joint_state_broadcaster9: - type: joint_state_broadcaster/JointStateBroadcaster + type: controller_manager/test_controller joint_state_broadcaster10: - type: joint_state_broadcaster/JointStateBroadcaster + type: controller_manager/test_controller joint_state_broadcaster11: - type: joint_state_broadcaster/JointStateBroadcaster + type: controller_manager/test_controller joint_state_broadcaster12: - type: joint_state_broadcaster/JointStateBroadcaster + type: controller_manager/test_controller joint_state_broadcaster13: - type: joint_state_broadcaster/JointStateBroadcaster + type: controller_manager/test_controller joint_state_broadcaster14: - type: joint_state_broadcaster/JointStateBroadcaster + type: controller_manager/test_controller joint_state_broadcaster15: - type: joint_state_broadcaster/JointStateBroadcaster + type: controller_manager/test_controller joint_state_broadcaster16: - type: joint_state_broadcaster/JointStateBroadcaster + type: controller_manager/test_controller joint_state_broadcaster17: - type: joint_state_broadcaster/JointStateBroadcaster + type: controller_manager/test_controller joint_state_broadcaster18: - type: joint_state_broadcaster/JointStateBroadcaster + type: controller_manager/test_controller joint_state_broadcaster19: - type: joint_state_broadcaster/JointStateBroadcaster + type: controller_manager/test_controller joint_state_broadcaster20: - type: joint_state_broadcaster/JointStateBroadcaster + type: controller_manager/test_controller joint_state_broadcaster21: - type: joint_state_broadcaster/JointStateBroadcaster + type: controller_manager/test_controller joint_state_broadcaster22: - type: joint_state_broadcaster/JointStateBroadcaster + type: controller_manager/test_controller joint_state_broadcaster23: - type: joint_state_broadcaster/JointStateBroadcaster + type: controller_manager/test_controller joint_state_broadcaster24: - type: joint_state_broadcaster/JointStateBroadcaster + type: controller_manager/test_controller joint_state_broadcaster25: - type: joint_state_broadcaster/JointStateBroadcaster + type: controller_manager/test_controller joint_state_broadcaster26: - type: joint_state_broadcaster/JointStateBroadcaster + type: controller_manager/test_controller joint_state_broadcaster27: - type: joint_state_broadcaster/JointStateBroadcaster + type: controller_manager/test_controller joint_state_broadcaster28: - type: joint_state_broadcaster/JointStateBroadcaster + type: controller_manager/test_controller joint_state_broadcaster29: - type: joint_state_broadcaster/JointStateBroadcaster + type: controller_manager/test_controller joint_state_broadcaster30: - type: joint_state_broadcaster/JointStateBroadcaster + type: controller_manager/test_controller joint_state_broadcaster31: - type: joint_state_broadcaster/JointStateBroadcaster + type: controller_manager/test_controller joint_state_broadcaster32: - type: joint_state_broadcaster/JointStateBroadcaster + type: controller_manager/test_controller joint_state_broadcaster33: - type: joint_state_broadcaster/JointStateBroadcaster + type: controller_manager/test_controller joint_state_broadcaster34: - type: joint_state_broadcaster/JointStateBroadcaster + type: controller_manager/test_controller joint_state_broadcaster35: - type: joint_state_broadcaster/JointStateBroadcaster + type: controller_manager/test_controller joint_state_broadcaster36: - type: joint_state_broadcaster/JointStateBroadcaster + type: controller_manager/test_controller joint_state_broadcaster37: - type: joint_state_broadcaster/JointStateBroadcaster + type: controller_manager/test_controller joint_state_broadcaster38: - type: joint_state_broadcaster/JointStateBroadcaster + type: controller_manager/test_controller joint_state_broadcaster39: - type: joint_state_broadcaster/JointStateBroadcaster + type: controller_manager/test_controller joint_state_broadcaster40: - type: joint_state_broadcaster/JointStateBroadcaster + type: controller_manager/test_controller joint_state_broadcaster41: - type: joint_state_broadcaster/JointStateBroadcaster + type: controller_manager/test_controller joint_state_broadcaster42: - type: joint_state_broadcaster/JointStateBroadcaster + type: controller_manager/test_controller joint_state_broadcaster43: - type: joint_state_broadcaster/JointStateBroadcaster + type: controller_manager/test_controller joint_state_broadcaster44: - type: joint_state_broadcaster/JointStateBroadcaster + type: controller_manager/test_controller joint_state_broadcaster45: - type: joint_state_broadcaster/JointStateBroadcaster + type: controller_manager/test_controller joint_state_broadcaster46: - type: joint_state_broadcaster/JointStateBroadcaster + type: controller_manager/test_controller joint_state_broadcaster47: - type: joint_state_broadcaster/JointStateBroadcaster + type: controller_manager/test_controller joint_state_broadcaster48: - type: joint_state_broadcaster/JointStateBroadcaster + type: controller_manager/test_controller joint_state_broadcaster49: - type: joint_state_broadcaster/JointStateBroadcaster - - fts_broadcaster: - type: force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster - - base_controller: - type: position_controllers/JointGroupPositionController - - joint1_controller: - type: position_controllers/JointGroupPositionController - - joint2_controller: - type: position_controllers/JointGroupPositionController - -fts_broadcaster: - ros__parameters: - sensor_name: base_ft_sensor - state_interface_names: - - force.x - - force.y - - force.z - - torque.x - - torque.y - - torque.z - frame_id: base_link - topic_name: base_wrench - -base_controller: - ros__parameters: - joints: - - base_joint - -joint1_controller: - ros__parameters: - joints: - - joint1 - -joint2_controller: - ros__parameters: - joints: - - joint2 + type: controller_manager/test_controller From 66ac4e879b77ae79fbfb9de0e5c05addd77d7c69 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 15 Aug 2024 00:18:52 +0200 Subject: [PATCH 16/21] Reduce installation scope --- controller_manager/CMakeLists.txt | 6 ++---- controller_manager/test/test_spawner_integration.py | 4 ---- 2 files changed, 2 insertions(+), 8 deletions(-) diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index 6b51222fd0..3671179fae 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -211,10 +211,8 @@ if(BUILD_TESTING) controller_manager_msgs ) - install( - DIRECTORY test - DESTINATION share/${PROJECT_NAME} - ) + install(FILES test/test_controllers.yaml + DESTINATION test) find_package(launch_testing_ament_cmake REQUIRED) add_launch_test(test/test_spawner_integration.py diff --git a/controller_manager/test/test_spawner_integration.py b/controller_manager/test/test_spawner_integration.py index 9d68f7326f..73299bf000 100644 --- a/controller_manager/test/test_spawner_integration.py +++ b/controller_manager/test/test_spawner_integration.py @@ -91,10 +91,6 @@ "joint_state_broadcaster47", "joint_state_broadcaster48", "joint_state_broadcaster49", - "fts_broadcaster", - "base_controller", - "joint1_controller", - "joint2_controller", ] From a117f8c6b61b0bc959572439cea4e7f8a37d480d Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 15 Aug 2024 00:19:10 +0200 Subject: [PATCH 17/21] Fix test_ros2_control_node.yaml --- controller_manager/test/test_ros2_control_node.yaml | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/controller_manager/test/test_ros2_control_node.yaml b/controller_manager/test/test_ros2_control_node.yaml index ce0602d6b3..b3181b3a04 100644 --- a/controller_manager/test/test_ros2_control_node.yaml +++ b/controller_manager/test/test_ros2_control_node.yaml @@ -1,3 +1,4 @@ +--- controller_manager: ros__parameters: update_rate: 100 # Hz @@ -8,4 +9,5 @@ ctrl_with_parameters_and_type: joint_names: ["joint0"] joint_state_broadcaster: - type: joint_state_broadcaster/JointStateBroadcaster + ros__parameters: + type: joint_state_broadcaster/JointStateBroadcaster From 723a8968b31ed724411d73b860a92cddaf6a9803 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 15 Aug 2024 10:17:57 +0200 Subject: [PATCH 18/21] Use gtest instead of launch_testing for integration test --- controller_manager/CMakeLists.txt | 8 - controller_manager/package.xml | 2 - controller_manager/test/test_controllers.yaml | 105 ---------- .../test/test_spawner_integration.py | 194 ------------------ .../test/test_spawner_unspawner.cpp | 26 +++ ros2_control_test_assets/CMakeLists.txt | 4 - .../urdf/test_description_mock.urdf | 91 -------- 7 files changed, 26 insertions(+), 404 deletions(-) delete mode 100644 controller_manager/test/test_controllers.yaml delete mode 100644 controller_manager/test/test_spawner_integration.py delete mode 100644 ros2_control_test_assets/urdf/test_description_mock.urdf diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index 3671179fae..5dea15c0d1 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -211,14 +211,6 @@ if(BUILD_TESTING) controller_manager_msgs ) - install(FILES test/test_controllers.yaml - DESTINATION test) - - find_package(launch_testing_ament_cmake REQUIRED) - add_launch_test(test/test_spawner_integration.py - TIMEOUT 180 - ) - find_package(ament_cmake_pytest REQUIRED) install(FILES test/test_ros2_control_node.yaml DESTINATION test) diff --git a/controller_manager/package.xml b/controller_manager/package.xml index b4b295656e..c0e02df044 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -32,10 +32,8 @@ ament_cmake_gmock ament_cmake_pytest hardware_interface_testing - launch_testing_ament_cmake python3-coverage ros2_control_test_assets - xacro ament_cmake diff --git a/controller_manager/test/test_controllers.yaml b/controller_manager/test/test_controllers.yaml deleted file mode 100644 index a465e81ad6..0000000000 --- a/controller_manager/test/test_controllers.yaml +++ /dev/null @@ -1,105 +0,0 @@ ---- -controller_manager: - ros__parameters: - update_rate: 100 - - joint_state_broadcaster0: - type: controller_manager/test_controller - joint_state_broadcaster1: - type: controller_manager/test_controller - joint_state_broadcaster2: - type: controller_manager/test_controller - joint_state_broadcaster3: - type: controller_manager/test_controller - joint_state_broadcaster4: - type: controller_manager/test_controller - joint_state_broadcaster5: - type: controller_manager/test_controller - joint_state_broadcaster6: - type: controller_manager/test_controller - joint_state_broadcaster7: - type: controller_manager/test_controller - joint_state_broadcaster8: - type: controller_manager/test_controller - joint_state_broadcaster9: - type: controller_manager/test_controller - joint_state_broadcaster10: - type: controller_manager/test_controller - joint_state_broadcaster11: - type: controller_manager/test_controller - joint_state_broadcaster12: - type: controller_manager/test_controller - joint_state_broadcaster13: - type: controller_manager/test_controller - joint_state_broadcaster14: - type: controller_manager/test_controller - joint_state_broadcaster15: - type: controller_manager/test_controller - joint_state_broadcaster16: - type: controller_manager/test_controller - joint_state_broadcaster17: - type: controller_manager/test_controller - joint_state_broadcaster18: - type: controller_manager/test_controller - joint_state_broadcaster19: - type: controller_manager/test_controller - joint_state_broadcaster20: - type: controller_manager/test_controller - joint_state_broadcaster21: - type: controller_manager/test_controller - joint_state_broadcaster22: - type: controller_manager/test_controller - joint_state_broadcaster23: - type: controller_manager/test_controller - joint_state_broadcaster24: - type: controller_manager/test_controller - joint_state_broadcaster25: - type: controller_manager/test_controller - joint_state_broadcaster26: - type: controller_manager/test_controller - joint_state_broadcaster27: - type: controller_manager/test_controller - joint_state_broadcaster28: - type: controller_manager/test_controller - joint_state_broadcaster29: - type: controller_manager/test_controller - joint_state_broadcaster30: - type: controller_manager/test_controller - joint_state_broadcaster31: - type: controller_manager/test_controller - joint_state_broadcaster32: - type: controller_manager/test_controller - joint_state_broadcaster33: - type: controller_manager/test_controller - joint_state_broadcaster34: - type: controller_manager/test_controller - joint_state_broadcaster35: - type: controller_manager/test_controller - joint_state_broadcaster36: - type: controller_manager/test_controller - joint_state_broadcaster37: - type: controller_manager/test_controller - joint_state_broadcaster38: - type: controller_manager/test_controller - joint_state_broadcaster39: - type: controller_manager/test_controller - joint_state_broadcaster40: - type: controller_manager/test_controller - joint_state_broadcaster41: - type: controller_manager/test_controller - joint_state_broadcaster42: - type: controller_manager/test_controller - joint_state_broadcaster43: - type: controller_manager/test_controller - joint_state_broadcaster44: - type: controller_manager/test_controller - joint_state_broadcaster45: - type: controller_manager/test_controller - joint_state_broadcaster46: - type: controller_manager/test_controller - joint_state_broadcaster47: - type: controller_manager/test_controller - joint_state_broadcaster48: - type: controller_manager/test_controller - joint_state_broadcaster49: - type: controller_manager/test_controller diff --git a/controller_manager/test/test_spawner_integration.py b/controller_manager/test/test_spawner_integration.py deleted file mode 100644 index 73299bf000..0000000000 --- a/controller_manager/test/test_spawner_integration.py +++ /dev/null @@ -1,194 +0,0 @@ -# Copyright 2024 FZI Forschungszentrum Informatik -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -# -# Author: Felix Exner - -import time -import pytest -import unittest - -import launch -import launch_testing.actions - -from launch.substitutions import ( - Command, - FindExecutable, - PathJoinSubstitution, -) -from launch_ros.substitutions import FindPackageShare -from launch_ros.parameter_descriptions import ParameterFile - -from launch_ros.actions import Node as LaunchNode - -# Imports for tests -import rclpy -from rclpy.node import Node -import logging - -from controller_manager_msgs.srv import ListControllers - -CONTROLLER_SPAWNER_TIMEOUT = "10" - -active_controllers = [ - "joint_state_broadcaster0", - "joint_state_broadcaster1", - "joint_state_broadcaster2", - "joint_state_broadcaster3", - "joint_state_broadcaster4", - "joint_state_broadcaster5", - "joint_state_broadcaster6", - "joint_state_broadcaster7", - "joint_state_broadcaster8", - "joint_state_broadcaster9", - "joint_state_broadcaster10", - "joint_state_broadcaster11", - "joint_state_broadcaster12", - "joint_state_broadcaster13", - "joint_state_broadcaster14", - "joint_state_broadcaster15", - "joint_state_broadcaster16", - "joint_state_broadcaster17", - "joint_state_broadcaster18", - "joint_state_broadcaster19", - "joint_state_broadcaster20", - "joint_state_broadcaster21", - "joint_state_broadcaster22", - "joint_state_broadcaster23", - "joint_state_broadcaster24", - "joint_state_broadcaster25", - "joint_state_broadcaster26", - "joint_state_broadcaster27", - "joint_state_broadcaster28", - "joint_state_broadcaster29", - "joint_state_broadcaster30", - "joint_state_broadcaster31", - "joint_state_broadcaster32", - "joint_state_broadcaster33", - "joint_state_broadcaster34", - "joint_state_broadcaster35", - "joint_state_broadcaster36", - "joint_state_broadcaster37", - "joint_state_broadcaster38", - "joint_state_broadcaster39", - "joint_state_broadcaster40", - "joint_state_broadcaster41", - "joint_state_broadcaster42", - "joint_state_broadcaster43", - "joint_state_broadcaster44", - "joint_state_broadcaster45", - "joint_state_broadcaster46", - "joint_state_broadcaster47", - "joint_state_broadcaster48", - "joint_state_broadcaster49", -] - - -def controller_spawner(controllers, active=True): - inactive_flags = ["--inactive"] if not active else [] - return LaunchNode( - package="controller_manager", - executable="spawner", - arguments=[ - "--controller-manager", - "/controller_manager", - "--controller-manager-timeout", - CONTROLLER_SPAWNER_TIMEOUT, - ] - + inactive_flags - + controllers, - ) - - -@pytest.mark.launch_test -def generate_test_description(): - robot_description_content = Command( - [ - PathJoinSubstitution([FindExecutable(name="xacro")]), - " ", - PathJoinSubstitution( - [ - FindPackageShare("ros2_control_test_assets"), - "urdf", - "test_description_mock.urdf", - ] - ), - ] - ) - robot_state_publisher_node = LaunchNode( - package="robot_state_publisher", - executable="robot_state_publisher", - output="both", - parameters=[{"robot_description": robot_description_content}], - ) - - control_node = LaunchNode( - package="controller_manager", - executable="ros2_control_node", - parameters=[ - ParameterFile( - PathJoinSubstitution( - [FindPackageShare("controller_manager"), "test", "test_controllers.yaml"] - ) - ) - ], - output="screen", - ) - - spawners = [controller_spawner([x]) for x in active_controllers] - - return launch.LaunchDescription( - [ - robot_state_publisher_node, - control_node, - launch_testing.actions.ReadyToTest(), - ] - + spawners - ) - - -class TestControllersRunning(unittest.TestCase): - @classmethod - def setUpClass(cls): - # Initialize the ROS context - rclpy.init() - cls.node = Node("controller_spawner_test") - - def _wait_for_service(self, srv_name, srv_type, timeout=10): - client = self.node.create_client(srv_type, srv_name) - - logging.info("Waiting for service '%s' with timeout %fs...", srv_name, timeout) - if client.wait_for_service(timeout) is False: - raise Exception(f"Could not reach service '{srv_name}' within timeout of {timeout}") - logging.info(" Successfully connected to service '%s'", srv_name) - - return client - - def test_all_controllers_available(self): - client = self._wait_for_service( - srv_name="controller_manager/list_controllers", srv_type=ListControllers - ) - # This is basically a dirty hack. It would be better to add events to all the spawners - # exiting and emit ReadyToTest() only after all of them have quit. One problem there: They - # do not necessarily quit, but can get into a deadlock waiting for a service response. - time.sleep(30) - request = ListControllers.Request() - future = client.call_async(request) - rclpy.spin_until_future_complete(self.node, future) - if future.result() is None: - raise Exception( - f"Error while calling service '{client.srv_name}': {future.exception()}" - ) - - result = future.result() - self.assertEqual(len(result.controller), len(active_controllers)) diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index 3edf770436..3de8708235 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -374,6 +374,32 @@ TEST_F(TestLoadController, spawner_test_fallback_controllers) } } +TEST_F(TestLoadController, spawner_with_many_controllers) +{ + std::stringstream ss; + const size_t num_controllers = 50; + const std::string controller_base_name = "ctrl_"; + for (size_t i = 0; i < num_controllers; i++) + { + std::string controller_name = controller_base_name + std::to_string(static_cast(i)); + cm_->set_parameter( + rclcpp::Parameter(controller_name + ".type", test_controller::TEST_CONTROLLER_CLASS_NAME)); + ss << controller_name << " "; + } + + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(call_spawner(ss.str() + " -c test_controller_manager"), 0); + + ASSERT_EQ(cm_->get_loaded_controllers().size(), num_controllers); + + for (size_t i = 0; i < num_controllers; i++) + { + auto ctrl = cm_->get_loaded_controllers()[i]; + 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); + } +} + class TestLoadControllerWithoutRobotDescription : public ControllerManagerFixture { diff --git a/ros2_control_test_assets/CMakeLists.txt b/ros2_control_test_assets/CMakeLists.txt index 8182766b44..d1bb895eed 100644 --- a/ros2_control_test_assets/CMakeLists.txt +++ b/ros2_control_test_assets/CMakeLists.txt @@ -14,10 +14,6 @@ install( DIRECTORY include/ DESTINATION include/ros2_control_test_assets ) -install( - DIRECTORY urdf - DESTINATION share/${PROJECT_NAME} -) install(TARGETS ros2_control_test_assets EXPORT export_ros2_control_test_assets ARCHIVE DESTINATION lib diff --git a/ros2_control_test_assets/urdf/test_description_mock.urdf b/ros2_control_test_assets/urdf/test_description_mock.urdf deleted file mode 100644 index 715de97540..0000000000 --- a/ros2_control_test_assets/urdf/test_description_mock.urdf +++ /dev/null @@ -1,91 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - mock_components/GenericSystem - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - From 26262c69ffc4d23890128346dc3ac9d8fdec818c Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 15 Aug 2024 10:29:23 +0200 Subject: [PATCH 19/21] Revert sorting package.xml This is not related to this PR --- controller_manager/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controller_manager/package.xml b/controller_manager/package.xml index c0e02df044..9f9a9bf796 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -31,8 +31,8 @@ ament_cmake_gmock ament_cmake_pytest - hardware_interface_testing python3-coverage + hardware_interface_testing ros2_control_test_assets From 49137f242fc2a8fc3ec1237414a2b6e0719f16c4 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 15 Aug 2024 10:29:54 +0200 Subject: [PATCH 20/21] Revert fixing yaml This is not related to this PR --- controller_manager/test/test_ros2_control_node.yaml | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/controller_manager/test/test_ros2_control_node.yaml b/controller_manager/test/test_ros2_control_node.yaml index b3181b3a04..ce0602d6b3 100644 --- a/controller_manager/test/test_ros2_control_node.yaml +++ b/controller_manager/test/test_ros2_control_node.yaml @@ -1,4 +1,3 @@ ---- controller_manager: ros__parameters: update_rate: 100 # Hz @@ -9,5 +8,4 @@ ctrl_with_parameters_and_type: joint_names: ["joint0"] joint_state_broadcaster: - ros__parameters: - type: joint_state_broadcaster/JointStateBroadcaster + type: joint_state_broadcaster/JointStateBroadcaster From 6d627b5707c491e9328999aeb1540da95556e15f Mon Sep 17 00:00:00 2001 From: "Felix Exner (fexner)" Date: Thu, 15 Aug 2024 13:17:30 +0200 Subject: [PATCH 21/21] Apply suggestions from code review Co-authored-by: Dr. Denis --- .../controller_manager/controller_manager_services.py | 6 +++--- controller_manager/test/test_spawner_unspawner.cpp | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/controller_manager/controller_manager/controller_manager_services.py b/controller_manager/controller_manager/controller_manager_services.py index 418d507afb..3f5a820fae 100644 --- a/controller_manager/controller_manager/controller_manager_services.py +++ b/controller_manager/controller_manager/controller_manager_services.py @@ -44,8 +44,8 @@ def service_caller( """ Abstraction of a service call. - Has an optional timeout to find the service and a mechanism - to retry a call of no response is received. + Has an optional timeout to find the service, receive the answer to a call + and a mechanism to retry a call of no response is received. @param node Node object to be associated with @type rclpy.node.Node @@ -88,7 +88,7 @@ def service_caller( else: return future.result() raise RuntimeError( - f"Could not successfully call service {service_name} in {max_attempts} attempts." + f"Could not successfully call service {service_name} after {max_attempts} attempts." ) diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index 3de8708235..db3940dbcc 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -381,7 +381,7 @@ TEST_F(TestLoadController, spawner_with_many_controllers) const std::string controller_base_name = "ctrl_"; for (size_t i = 0; i < num_controllers; i++) { - std::string controller_name = controller_base_name + std::to_string(static_cast(i)); + const std::string controller_name = controller_base_name + std::to_string(static_cast(i)); cm_->set_parameter( rclcpp::Parameter(controller_name + ".type", test_controller::TEST_CONTROLLER_CLASS_NAME)); ss << controller_name << " ";