diff --git a/controller_manager/controller_manager/test_utils.py b/controller_manager/controller_manager/test_utils.py new file mode 100644 index 0000000000..8fb4ef8d97 --- /dev/null +++ b/controller_manager/controller_manager/test_utils.py @@ -0,0 +1,129 @@ +# 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"): + """ + 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 + 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): + """ + 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) + 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() diff --git a/controller_manager/package.xml b/controller_manager/package.xml index 4bbd6306b1..ed18511b18 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -33,9 +33,15 @@ ament_cmake_gmock ament_cmake_pytest - python3-coverage hardware_interface_testing + launch_testing_ros + launch_testing + launch + python3-coverage + rclpy + robot_state_publisher ros2_control_test_assets + sensor_msgs example_interfaces 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..b382d3b09d 100644 --- a/controller_manager/test/test_ros2_control_node_launch.py +++ b/controller_manager/test/test_ros2_control_node_launch.py @@ -30,29 +30,31 @@ import pytest import unittest -import time +import os +from ament_index_python.packages import get_package_share_directory, get_package_prefix from launch import LaunchDescription -from launch.substitutions import PathJoinSubstitution -from launch_ros.substitutions import FindPackageShare 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"), - "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( @@ -61,29 +63,72 @@ def generate_test_description(): parameters=[robot_controllers], output="both", ) + # Get URDF, without involving xacro + urdf = os.path.join( + get_package_share_directory("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} - return LaunchDescription([control_node, ReadyToTest()]) + 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() diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 9b5360ced0..ace9ea8acf 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -85,6 +85,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 ****************** 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 - - - - - -