Move test_utils module from demos repo (#1955)

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()

controller_manager/package.xml

@@ -33,9 +33,15 @@
 
   <test_depend>ament_cmake_gmock</test_depend>
   <test_depend>ament_cmake_pytest</test_depend>
-  <test_depend>python3-coverage</test_depend>
   <test_depend>hardware_interface_testing</test_depend>
+  <test_depend>launch_testing_ros</test_depend>
+  <test_depend>launch_testing</test_depend>
+  <test_depend>launch</test_depend>
+  <test_depend>python3-coverage</test_depend>
+  <test_depend>rclpy</test_depend>
+  <test_depend>robot_state_publisher</test_depend>
   <test_depend>ros2_control_test_assets</test_depend>
+  <test_depend>sensor_msgs</test_depend>
   <test_depend>example_interfaces</test_depend>
 
   <export>

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"]

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()

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 <https://github.com/ros-controls/ros2_control/pull/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 <https://github.com/ros-controls/ros2_control/pull/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 <https://github.com/ros-controls/ros2_control/pull/1915>`_).
+* A python module ``test_utils`` was added to the ``controller_manager`` package to help with integration testing (`#1955 <https://github.com/ros-controls/ros2_control/pull/1955>`_).
 
 hardware_interface
 ******************

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

hardware_interface/test/test_hardware_components/test_hardware_components.urdf → ros2_control_test_assets/urdf/test_hardware_components.urdf

@@ -1,5 +1,6 @@
 <?xml version="1.0" encoding="utf-8"?>
 <robot name="MinimalRobot">
+  <link name="world" />
   <joint name="base_joint" type="fixed">
     <origin rpy="0 0 0" xyz="0 0 0"/>
     <parent link="world"/>
@@ -46,18 +47,11 @@
     <parent link="link2"/>
     <child link="tool_link"/>
   </joint>
-  <ros2_control name="TestActuatorComponent" type="actuator">
-    <hardware>
-      <plugin>test_robot_hardware/TestSingleJointActuator</plugin>
-    </hardware>
-    <joint name="base_joint">
-      <command_interface name="position"/>
-      <state_interface name="position"/>
-    </joint>
-  </ros2_control>
+  <link name="tool_link" />
+
   <ros2_control name="TestSensorComponent" type="sensor">
     <hardware>
-      <plugin>test_robot_hardware/TestForceTorqueSensor</plugin>
+      <plugin>test_hardware_components/TestForceTorqueSensor</plugin>
     </hardware>
     <sensor name="ft_sensor">
       <state_interface name="fx"/>
@@ -68,36 +62,18 @@
       <state_interface name="tz"/>
     </sensor>
   </ros2_control>
+
   <ros2_control name="TestSystemComponent" type="system">
     <hardware>
-      <plugin>test_robot_hardware/TestTwoJointSystem</plugin>
-    </hardware>
-    <joint name="joint1">
-      <command_interface name="position"/>
-      <state_interface name="position"/>
-    </joint>
-    <joint name="joint2">
-      <command_interface name="position"/>
-      <state_interface name="position"/>
-    </joint>
-  </ros2_control>
-  <ros2_control name="TestSystemCommandModes" type="system">
-    <hardware>
-      <plugin>test_robot_hardware/TestSystemCommandModes</plugin>
+      <plugin>test_hardware_components/TestTwoJointSystem</plugin>
     </hardware>
     <joint name="joint1">
       <command_interface name="position"/>
-      <command_interface name="velocity"/>
       <state_interface name="position"/>
-      <state_interface name="velocity"/>
-      <state_interface name="acceleration"/>
     </joint>
     <joint name="joint2">
       <command_interface name="position"/>
-      <command_interface name="velocity"/>
       <state_interface name="position"/>
-      <state_interface name="velocity"/>
-      <state_interface name="acceleration"/>
     </joint>
   </ros2_control>
 </robot>