test_nodes: catch keyboard interrupt and add simple launch tests (#1369)

ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_forward_position_controller.py

@@ -68,9 +68,16 @@ def main(args=None):
 
     publisher_forward_position = PublisherForwardPosition()
 
-    rclpy.spin(publisher_forward_position)
-    publisher_forward_position.destroy_node()
-    rclpy.shutdown()
+    try:
+        rclpy.spin(publisher_forward_position)
+    except KeyboardInterrupt:
+        print("Keyboard interrupt received. Shutting down node.")
+    except Exception as e:
+        print(f"Unhandled exception: {e}")
+    finally:
+        if rclpy.ok():
+            publisher_forward_position.destroy_node()
+            rclpy.shutdown()
 
 
 if __name__ == "__main__":

ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_joint_trajectory_controller.py

@@ -184,9 +184,16 @@ def main(args=None):
 
     publisher_joint_trajectory = PublisherJointTrajectory()
 
-    rclpy.spin(publisher_joint_trajectory)
-    publisher_joint_trajectory.destroy_node()
-    rclpy.shutdown()
+    try:
+        rclpy.spin(publisher_joint_trajectory)
+    except KeyboardInterrupt:
+        print("Keyboard interrupt received. Shutting down node.")
+    except Exception as e:
+        print(f"Unhandled exception: {e}")
+    finally:
+        if rclpy.ok():
+            publisher_joint_trajectory.destroy_node()
+            rclpy.shutdown()
 
 
 if __name__ == "__main__":

ros2_controllers_test_nodes/setup.py

@@ -25,8 +25,7 @@
     data_files=[
         ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
         ("share/" + package_name, ["package.xml"]),
-        ("share/" + package_name, glob("launch/*.launch.py")),
-        ("share/" + package_name + "/configs", glob("configs/*.*")),
+        ("share/" + package_name + "/test", glob("test/*.yaml")),
     ],
     install_requires=["setuptools"],
     zip_safe=True,

ros2_controllers_test_nodes/test/rrbot_forward_position_publisher.yaml

@@ -0,0 +1,11 @@
+publisher_forward_position_controller:
+  ros__parameters:
+
+    wait_sec_between_publish: 5
+    publish_topic: "/forward_position_controller/commands"
+
+    goal_names: ["pos1", "pos2", "pos3", "pos4"]
+    pos1: [0.785, 0.785]
+    pos2: [0.0, 0.0]
+    pos3: [-0.785, -0.785]
+    pos4: [0.0, 0.0]

ros2_controllers_test_nodes/test/rrbot_joint_trajectory_publisher.yaml

@@ -0,0 +1,24 @@
+publisher_position_trajectory_controller:
+  ros__parameters:
+
+    controller_name: "joint_trajectory_position_controller"
+    wait_sec_between_publish: 6
+
+    goal_names: ["pos1", "pos2", "pos3", "pos4"]
+    pos1:
+      positions: [0.785, 0.785]
+    pos2:
+      positions: [0.0, 0.0]
+    pos3:
+      positions: [-0.785, -0.785]
+    pos4:
+      positions: [0.0, 0.0]
+
+    joints:
+      - joint1
+      - joint2
+
+    check_starting_point: false
+    starting_point_limits:
+      joint1: [-0.1,0.1]
+      joint2: [-0.1,0.1]

ros2_controllers_test_nodes/test/test_publisher_forward_position_controller_launch.py

@@ -0,0 +1,107 @@
+# 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 pytest
+import unittest
+import time
+
+from launch import LaunchDescription
+from launch.substitutions import PathJoinSubstitution
+from launch_ros.substitutions import FindPackageShare
+from launch_testing.actions import ReadyToTest
+from launch_testing_ros import WaitForTopics
+
+import launch_testing.markers
+import rclpy
+import launch_ros.actions
+from rclpy.node import Node
+
+from std_msgs.msg import Float64MultiArray
+
+
+# Executes the given launch file and checks if all nodes can be started
+@pytest.mark.launch_test
+def generate_test_description():
+
+    params = PathJoinSubstitution(
+        [
+            FindPackageShare("ros2_controllers_test_nodes"),
+            "test",
+            "rrbot_forward_position_publisher.yaml",
+        ]
+    )
+
+    pub_node = launch_ros.actions.Node(
+        package="ros2_controllers_test_nodes",
+        executable="publisher_forward_position_controller",
+        parameters=[params],
+        output="both",
+    )
+
+    return LaunchDescription([pub_node, 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):
+
+    def setUp(self):
+        rclpy.init()
+        self.node = Node("test_node")
+
+    def tearDown(self):
+        self.node.destroy_node()
+        rclpy.shutdown()
+
+    def test_node_start(self):
+        start = time.time()
+        found = False
+        while time.time() - start < 2.0 and not found:
+            found = "publisher_forward_position_controller" in self.node.get_node_names()
+            time.sleep(0.1)
+        assert found, "publisher_forward_position_controller not found!"
+
+    def test_check_if_topic_published(self):
+        topic = "/forward_position_controller/commands"
+        wait_for_topics = WaitForTopics([(topic, Float64MultiArray)], 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.data) == 2, "Wrong number of joints in message"
+        wait_for_topics.shutdown()
+
+
+@launch_testing.post_shutdown_test()
+# These tests are run after the processes in generate_test_description() have shut down.
+class TestPublisherShutdown(unittest.TestCase):
+
+    def test_exit_codes(self, proc_info):
+        """Check if the processes exited normally."""
+        launch_testing.asserts.assertExitCodes(proc_info)

ros2_controllers_test_nodes/test/test_publisher_joint_trajectory_controller_launch.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 pytest
+import unittest
+import time
+
+from launch import LaunchDescription
+from launch.substitutions import PathJoinSubstitution
+from launch_ros.substitutions import FindPackageShare
+from launch_testing.actions import ReadyToTest
+from launch_testing_ros import WaitForTopics
+
+import launch_testing.markers
+import rclpy
+import launch_ros.actions
+from rclpy.node import Node
+from trajectory_msgs.msg import JointTrajectory
+
+
+# Executes the given launch file and checks if all nodes can be started
+@pytest.mark.launch_test
+def generate_test_description():
+
+    params = PathJoinSubstitution(
+        [
+            FindPackageShare("ros2_controllers_test_nodes"),
+            "test",
+            "rrbot_joint_trajectory_publisher.yaml",
+        ]
+    )
+
+    pub_node = launch_ros.actions.Node(
+        package="ros2_controllers_test_nodes",
+        executable="publisher_joint_trajectory_controller",
+        parameters=[params],
+        output="both",
+    )
+
+    return LaunchDescription([pub_node, 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):
+
+    def setUp(self):
+        rclpy.init()
+        self.node = Node("test_node")
+
+    def tearDown(self):
+        self.node.destroy_node()
+        rclpy.shutdown()
+
+    def test_node_start(self):
+        start = time.time()
+        found = False
+        while time.time() - start < 2.0 and not found:
+            found = "publisher_position_trajectory_controller" in self.node.get_node_names()
+            time.sleep(0.1)
+        assert found, "publisher_position_trajectory_controller not found!"
+
+    def test_check_if_topic_published(self):
+        topic = "/joint_trajectory_position_controller/joint_trajectory"
+        wait_for_topics = WaitForTopics([(topic, JointTrajectory)], 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.joint_names) == 2, "Wrong number of joints in message"
+        wait_for_topics.shutdown()
+
+
+@launch_testing.post_shutdown_test()
+# These tests are run after the processes in generate_test_description() have shut down.
+class TestPublisherShutdown(unittest.TestCase):
+
+    def test_exit_codes(self, proc_info):
+        """Check if the processes exited normally."""
+        launch_testing.asserts.assertExitCodes(proc_info)