From 63168b558b9b7458cdfba16da7f4cc691916d867 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 18 Dec 2024 19:47:29 +0000 Subject: [PATCH 01/10] Make a valid urdf but move it to ros2_control_test_assets --- ros2_control_test_assets/CMakeLists.txt | 4 +++ .../urdf}/test_hardware_components.urdf | 36 ++++--------------- 2 files changed, 10 insertions(+), 30 deletions(-) rename {hardware_interface/test/test_hardware_components => ros2_control_test_assets/urdf}/test_hardware_components.urdf (65%) diff --git a/ros2_control_test_assets/CMakeLists.txt b/ros2_control_test_assets/CMakeLists.txt index d1bb895eed..d63fb52c86 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( + FILES urdf/test_hardware_components.urdf + DESTINATION share/ros2_control_test_assets/urdf +) install(TARGETS ros2_control_test_assets EXPORT export_ros2_control_test_assets ARCHIVE DESTINATION lib diff --git a/hardware_interface/test/test_hardware_components/test_hardware_components.urdf b/ros2_control_test_assets/urdf/test_hardware_components.urdf similarity index 65% rename from hardware_interface/test/test_hardware_components/test_hardware_components.urdf rename to ros2_control_test_assets/urdf/test_hardware_components.urdf index e61b2d1c4c..c49c79bf55 100644 --- a/hardware_interface/test/test_hardware_components/test_hardware_components.urdf +++ b/ros2_control_test_assets/urdf/test_hardware_components.urdf @@ -1,5 +1,6 @@ + @@ -46,18 +47,11 @@ - - - test_robot_hardware/TestSingleJointActuator - - - - - - + + - test_robot_hardware/TestForceTorqueSensor + test_hardware_components/TestForceTorqueSensor @@ -68,36 +62,18 @@ + - test_robot_hardware/TestTwoJointSystem - - - - - - - - - - - - - test_robot_hardware/TestSystemCommandModes + test_hardware_components/TestTwoJointSystem - - - - - - From 98cd242a030ba2e5be74637ff46dbcb7de8f4eb9 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 18 Dec 2024 19:47:29 +0000 Subject: [PATCH 02/10] Add test_utils from ros2_control_demo repo --- .../controller_manager/test_utils.py | 106 ++++++++++++++++++ 1 file changed, 106 insertions(+) create mode 100644 controller_manager/controller_manager/test_utils.py diff --git a/controller_manager/controller_manager/test_utils.py b/controller_manager/controller_manager/test_utils.py new file mode 100644 index 0000000000..e23ca7bc8d --- /dev/null +++ b/controller_manager/controller_manager/test_utils.py @@ -0,0 +1,106 @@ +# Copyright (c) 2024 AIT - Austrian Institute of Technology GmbH +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the {copyright_holder} nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +# Author: Christoph Froehlich + +import time + +from launch_testing_ros import WaitForTopics +from sensor_msgs.msg import JointState +from controller_manager.controller_manager_services import list_controllers + + +def check_node_running(node, node_name, timeout=5.0): + + start = time.time() + found = False + while time.time() - start < timeout and not found: + found = node_name in node.get_node_names() + time.sleep(0.1) + assert found, f"{node_name} not found!" + + +def check_controllers_running(node, cnames, namespace="", state="active"): + + # wait for controller to be loaded before we call the CM services + found = {cname: False for cname in cnames} # Define 'found' as a dictionary + start = time.time() + # namespace is either "/" (empty) or "/ns" if set + if namespace: + namespace_api = namespace + if not namespace_api.startswith("/"): + namespace_api = "/" + namespace_api + if namespace.endswith("/"): + namespace_api = namespace_api[:-1] + else: + namespace_api = "/" + + while time.time() - start < 10.0 and not all(found.values()): + node_names_namespaces = node.get_node_names_and_namespaces() + for cname in cnames: + if any(name == cname and ns == namespace_api for name, ns in node_names_namespaces): + found[cname] = True + time.sleep(0.1) + assert all( + found.values() + ), f"Controller node(s) not found: {', '.join(['ns: ' + namespace_api + ', ctrl:' + cname for cname, is_found in found.items() if not is_found])}, but seeing {node.get_node_names_and_namespaces()}" + + found = {cname: False for cname in cnames} # Define 'found' as a dictionary + start = time.time() + # namespace is either "/" (empty) or "/ns" if set + if not namespace: + cm = "controller_manager" + else: + if namespace.endswith("/"): + cm = namespace + "controller_manager" + else: + cm = namespace + "/controller_manager" + while time.time() - start < 10.0 and not all(found.values()): + controllers = list_controllers(node, cm, 5.0).controller + assert controllers, "No controllers found!" + for c in controllers: + for cname in cnames: + if c.name == cname and c.state == state: + found[cname] = True + break + time.sleep(0.1) + + assert all( + found.values() + ), f"Controller(s) not found or not {state}: {', '.join([cname for cname, is_found in found.items() if not is_found])}" + + +def check_if_js_published(topic, joint_names): + wait_for_topics = WaitForTopics([(topic, JointState)], timeout=20.0) + assert wait_for_topics.wait(), f"Topic '{topic}' not found!" + msgs = wait_for_topics.received_messages(topic) + msg = msgs[0] + assert len(msg.name) == len(joint_names), "Wrong number of joints in message" + # use a set to compare the joint names, as the order might be different + assert set(msg.name) == set(joint_names), "Wrong joint names" + wait_for_topics.shutdown() From d5d3ae7472408ddc9d72e397e901695d746c4caa Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 18 Dec 2024 19:47:29 +0000 Subject: [PATCH 03/10] Update ros2_control_node tests to use test_utils --- controller_manager/CMakeLists.txt | 9 +- controller_manager/package.xml | 14 ++- .../test/test_ros2_control_node.yaml | 5 +- .../test/test_ros2_control_node_launch.py | 101 ++++++++++++++---- 4 files changed, 100 insertions(+), 29 deletions(-) diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index 1b0f308613..15748c5551 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -237,10 +237,15 @@ if(BUILD_TESTING) controller_manager_msgs ) - find_package(ament_cmake_pytest REQUIRED) + find_package(ament_cmake_ros REQUIRED) + find_package(launch_testing_ament_cmake REQUIRED) install(FILES test/test_ros2_control_node.yaml DESTINATION test) - ament_add_pytest_test(test_ros2_control_node test/test_ros2_control_node_launch.py) + function(add_ros_isolated_launch_test path) + set(RUNNER "${ament_cmake_ros_DIR}/run_test_isolated.py") + add_launch_test("${path}" RUNNER "${RUNNER}" ${ARGN}) + endfunction() + add_ros_isolated_launch_test(test/test_ros2_control_node_launch.py) endif() install( diff --git a/controller_manager/package.xml b/controller_manager/package.xml index 5447040537..4f808fc026 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -31,11 +31,19 @@ libstatistics_collector generate_parameter_library - ament_cmake_gmock - ament_cmake_pytest - python3-coverage + ament_cmake_ros hardware_interface_testing + launch + launch_ros + launch_testing + launch_testing_ros + launch_testing_ament_cmake + python3-coverage + rclpy + robot_state_publisher + ros2_control_testing ros2_control_test_assets + sensor_msgs ament_cmake diff --git a/controller_manager/test/test_ros2_control_node.yaml b/controller_manager/test/test_ros2_control_node.yaml index ce0602d6b3..74f6cf8504 100644 --- a/controller_manager/test/test_ros2_control_node.yaml +++ b/controller_manager/test/test_ros2_control_node.yaml @@ -5,7 +5,4 @@ controller_manager: ctrl_with_parameters_and_type: ros__parameters: type: "controller_manager/test_controller" - joint_names: ["joint0"] - -joint_state_broadcaster: - type: joint_state_broadcaster/JointStateBroadcaster + joint_names: ["joint1"] diff --git a/controller_manager/test/test_ros2_control_node_launch.py b/controller_manager/test/test_ros2_control_node_launch.py index c8c1136849..cd3d33f395 100644 --- a/controller_manager/test/test_ros2_control_node_launch.py +++ b/controller_manager/test/test_ros2_control_node_launch.py @@ -28,28 +28,32 @@ # # Author: Christoph Froehlich -import pytest import unittest -import time from launch import LaunchDescription -from launch.substitutions import PathJoinSubstitution -from launch_ros.substitutions import FindPackageShare +from launch.substitutions import PathJoinSubstitution, Command, FindExecutable +from launch_ros.substitutions import FindPackageShare, FindPackagePrefix from launch_testing.actions import ReadyToTest - import launch_testing.markers -import rclpy import launch_ros.actions -from rclpy.node import Node +from sensor_msgs.msg import JointState + +import rclpy +from controller_manager.test_utils import ( + check_controllers_running, + check_if_js_published, + check_node_running, +) +from controller_manager.launch_utils import generate_controllers_spawner_launch_description +import threading # Executes the given launch file and checks if all nodes can be started -@pytest.mark.launch_test def generate_test_description(): robot_controllers = PathJoinSubstitution( [ - FindPackageShare("controller_manager"), + FindPackagePrefix("controller_manager"), "test", "test_ros2_control_node.yaml", ] @@ -61,29 +65,86 @@ def generate_test_description(): parameters=[robot_controllers], output="both", ) - - return LaunchDescription([control_node, ReadyToTest()]) + # Get URDF + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [ + FindPackageShare("ros2_control_test_assets"), + "urdf", + "test_hardware_components.urdf", + ] + ), + ] + ) + # why not this? + # robot_description_content = PathJoinSubstitution( + # [ + # FindPackageShare("ros2_control_test_assets"), + # "urdf", + # "test_hardware_components.urdf", + # ] + # ) + robot_description = {"robot_description": robot_description_content} + + robot_state_pub_node = launch_ros.actions.Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[robot_description], + ) + ctrl_spawner = generate_controllers_spawner_launch_description( + [ + "ctrl_with_parameters_and_type", + ], + controller_params_files=[robot_controllers], + ) + return LaunchDescription([robot_state_pub_node, control_node, ctrl_spawner, ReadyToTest()]) # This is our test fixture. Each method is a test case. # These run alongside the processes specified in generate_test_description() class TestFixture(unittest.TestCase): + @classmethod + def setUpClass(cls): + rclpy.init() + + @classmethod + def tearDownClass(cls): + rclpy.shutdown() def setUp(self): - rclpy.init() - self.node = Node("test_node") + self.node = rclpy.create_node("test_node") def tearDown(self): self.node.destroy_node() - rclpy.shutdown() + + def timer_callback(self): + js_msg = JointState() + js_msg.name = ["joint0"] + js_msg.position = [0.0] + self.pub.publish(js_msg) + + def publish_joint_states(self): + self.pub = self.node.create_publisher(JointState, "/joint_states", 10) + self.timer = self.node.create_timer(1.0, self.timer_callback) + rclpy.spin(self.node) def test_node_start(self): - start = time.time() - found = False - while time.time() - start < 2.0 and not found: - found = "controller_manager" in self.node.get_node_names() - time.sleep(0.1) - assert found, "controller_manager not found!" + check_node_running(self.node, "controller_manager") + + def test_controllers_start(self): + cnames = ["ctrl_with_parameters_and_type"] + check_controllers_running(self.node, cnames, state="active") + + def test_check_if_msgs_published(self): + # we don't have a joint_state_broadcaster in this repo, + # publish joint states manually to test check_if_js_published + thread = threading.Thread(target=self.publish_joint_states) + thread.start() + check_if_js_published("/joint_states", ["joint0"]) @launch_testing.post_shutdown_test() From 0dce82aec5c26bf697a56a6de47b773efc84dacb Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 18 Dec 2024 19:47:29 +0000 Subject: [PATCH 04/10] Add docstrings --- .../controller_manager/test_utils.py | 23 +++++++++++++++++++ 1 file changed, 23 insertions(+) diff --git a/controller_manager/controller_manager/test_utils.py b/controller_manager/controller_manager/test_utils.py index e23ca7bc8d..8fb4ef8d97 100644 --- a/controller_manager/controller_manager/test_utils.py +++ b/controller_manager/controller_manager/test_utils.py @@ -46,6 +46,18 @@ def check_node_running(node, node_name, timeout=5.0): def check_controllers_running(node, cnames, namespace="", state="active"): + """ + Check if the specified controllers are running on the given node. + + Args: + node (Node): The ROS2 node instance to check for controllers. + cnames (list of str): List of controller names to check. + namespace (str, optional): The namespace in which to look for controllers. Defaults to "". + state (str, optional): The desired state of the controllers. Defaults to "active". + + Raises: + AssertionError: If any of the specified controllers are not found or not in the desired state within the timeout period. + """ # wait for controller to be loaded before we call the CM services found = {cname: False for cname in cnames} # Define 'found' as a dictionary @@ -96,6 +108,17 @@ def check_controllers_running(node, cnames, namespace="", state="active"): def check_if_js_published(topic, joint_names): + """ + Check if a JointState message is published on a given topic with the expected joint names. + + Args: + topic (str): The name of the topic to check. + joint_names (list of str): The expected joint names in the JointState message. + + Raises: + AssertionError: If the topic is not found, the number of joints in the message is incorrect, + or the joint names do not match the expected names. + """ wait_for_topics = WaitForTopics([(topic, JointState)], timeout=20.0) assert wait_for_topics.wait(), f"Topic '{topic}' not found!" msgs = wait_for_topics.received_messages(topic) From 0a6ee0dfb6710046c1f006b292548e09f7a749e4 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 18 Dec 2024 19:47:29 +0000 Subject: [PATCH 05/10] Add release_notes --- doc/release_notes.rst | 1 + 1 file changed, 1 insertion(+) diff --git a/doc/release_notes.rst b/doc/release_notes.rst index a187e62437..4eb1e60075 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -84,6 +84,7 @@ controller_manager * The ``ros2_control_node`` node has a new ``cpu_affinity`` parameter to bind the process to a specific CPU core. By default, this is not enabled. (`#1852 `_). * The ``--service-call-timeout`` was added as parameter to the helper scripts ``spawner.py``. Useful when the CPU load is high at startup and the service call does not return immediately (`#1808 `_). * The ``cpu_affinity`` parameter can now accept of types ``int`` or ``int_array`` to bind the process to a specific CPU core or multiple CPU cores. (`#1915 `_). +* A python module ``test_utils`` was added to the ``controller_manager`` package to help with integration testing (`#1955 `_). hardware_interface ****************** From 099309956a968e60ab7dbb046850805d9d5bdf84 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 18 Dec 2024 19:47:29 +0000 Subject: [PATCH 06/10] Remove xacro and load URDF directly --- .../test/test_ros2_control_node_launch.py | 40 +++++-------------- 1 file changed, 11 insertions(+), 29 deletions(-) diff --git a/controller_manager/test/test_ros2_control_node_launch.py b/controller_manager/test/test_ros2_control_node_launch.py index cd3d33f395..e82246ac50 100644 --- a/controller_manager/test/test_ros2_control_node_launch.py +++ b/controller_manager/test/test_ros2_control_node_launch.py @@ -29,10 +29,10 @@ # Author: Christoph Froehlich import unittest +import os +from ament_index_python.packages import get_package_share_directory, get_package_prefix from launch import LaunchDescription -from launch.substitutions import PathJoinSubstitution, Command, FindExecutable -from launch_ros.substitutions import FindPackageShare, FindPackagePrefix from launch_testing.actions import ReadyToTest import launch_testing.markers import launch_ros.actions @@ -51,12 +51,8 @@ # Executes the given launch file and checks if all nodes can be started def generate_test_description(): - robot_controllers = PathJoinSubstitution( - [ - FindPackagePrefix("controller_manager"), - "test", - "test_ros2_control_node.yaml", - ] + robot_controllers = os.path.join( + get_package_prefix("controller_manager"), "test", "test_ros2_control_node.yaml" ) control_node = launch_ros.actions.Node( @@ -65,28 +61,14 @@ def generate_test_description(): parameters=[robot_controllers], output="both", ) - # Get URDF - robot_description_content = Command( - [ - PathJoinSubstitution([FindExecutable(name="xacro")]), - " ", - PathJoinSubstitution( - [ - FindPackageShare("ros2_control_test_assets"), - "urdf", - "test_hardware_components.urdf", - ] - ), - ] + # Get URDF, without involving xacro + urdf = os.path.join( + get_package_share_directory("ros2_control_test_assets"), + "urdf", + "test_hardware_components.urdf", ) - # why not this? - # robot_description_content = PathJoinSubstitution( - # [ - # FindPackageShare("ros2_control_test_assets"), - # "urdf", - # "test_hardware_components.urdf", - # ] - # ) + with open(urdf) as infp: + robot_description_content = infp.read() robot_description = {"robot_description": robot_description_content} robot_state_pub_node = launch_ros.actions.Node( From 65eec396c29110f7d8524cebfdff9cd4eb9cf806 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 18 Dec 2024 19:47:29 +0000 Subject: [PATCH 07/10] Remove wrong dependency --- controller_manager/package.xml | 1 - 1 file changed, 1 deletion(-) diff --git a/controller_manager/package.xml b/controller_manager/package.xml index 4f808fc026..e0b4d46989 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -41,7 +41,6 @@ python3-coverage rclpy robot_state_publisher - ros2_control_testing ros2_control_test_assets sensor_msgs From 18f8371506471926eec72b8e075a7e6c807d2243 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 18 Dec 2024 19:47:29 +0000 Subject: [PATCH 08/10] Add missing dependency --- controller_manager/package.xml | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/controller_manager/package.xml b/controller_manager/package.xml index e0b4d46989..c41c773d90 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -31,13 +31,14 @@ libstatistics_collector generate_parameter_library + ament_cmake_gmock ament_cmake_ros hardware_interface_testing - launch launch_ros - launch_testing - launch_testing_ros launch_testing_ament_cmake + launch_testing_ros + launch_testing + launch python3-coverage rclpy robot_state_publisher From 246114c3f168c275727937177c85ad83b394e103 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 18 Dec 2024 19:47:29 +0000 Subject: [PATCH 09/10] Cleanup double dependency --- controller_manager/package.xml | 1 - 1 file changed, 1 deletion(-) diff --git a/controller_manager/package.xml b/controller_manager/package.xml index c41c773d90..57a0e31693 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -34,7 +34,6 @@ ament_cmake_gmock ament_cmake_ros hardware_interface_testing - launch_ros launch_testing_ament_cmake launch_testing_ros launch_testing From df209eb260aea84ae533f2d99a9b6d3adbac6716 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 18 Dec 2024 19:47:29 +0000 Subject: [PATCH 10/10] Use pytest again, does coverage work? --- controller_manager/CMakeLists.txt | 9 ++------- controller_manager/package.xml | 3 +-- controller_manager/test/test_ros2_control_node_launch.py | 2 ++ 3 files changed, 5 insertions(+), 9 deletions(-) diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index 15748c5551..1b0f308613 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -237,15 +237,10 @@ if(BUILD_TESTING) controller_manager_msgs ) - find_package(ament_cmake_ros REQUIRED) - find_package(launch_testing_ament_cmake REQUIRED) + find_package(ament_cmake_pytest REQUIRED) install(FILES test/test_ros2_control_node.yaml DESTINATION test) - function(add_ros_isolated_launch_test path) - set(RUNNER "${ament_cmake_ros_DIR}/run_test_isolated.py") - add_launch_test("${path}" RUNNER "${RUNNER}" ${ARGN}) - endfunction() - add_ros_isolated_launch_test(test/test_ros2_control_node_launch.py) + ament_add_pytest_test(test_ros2_control_node test/test_ros2_control_node_launch.py) endif() install( diff --git a/controller_manager/package.xml b/controller_manager/package.xml index 57a0e31693..48cb29b14b 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -32,9 +32,8 @@ generate_parameter_library ament_cmake_gmock - ament_cmake_ros + ament_cmake_pytest hardware_interface_testing - launch_testing_ament_cmake launch_testing_ros launch_testing launch diff --git a/controller_manager/test/test_ros2_control_node_launch.py b/controller_manager/test/test_ros2_control_node_launch.py index e82246ac50..b382d3b09d 100644 --- a/controller_manager/test/test_ros2_control_node_launch.py +++ b/controller_manager/test/test_ros2_control_node_launch.py @@ -28,6 +28,7 @@ # # Author: Christoph Froehlich +import pytest import unittest import os @@ -49,6 +50,7 @@ # Executes the given launch file and checks if all nodes can be started +@pytest.mark.launch_test def generate_test_description(): robot_controllers = os.path.join(