From ccc1c2155bc546cd529f829fee809011b7591638 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sun, 17 Nov 2024 16:10:20 +0000 Subject: [PATCH 1/5] catch keyboard interrupt --- .../publisher_forward_position_controller.py | 13 ++++++++++--- .../publisher_joint_trajectory_controller.py | 13 ++++++++++--- 2 files changed, 20 insertions(+), 6 deletions(-) diff --git a/ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_forward_position_controller.py b/ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_forward_position_controller.py index bb6add77ef..51582a90d4 100644 --- a/ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_forward_position_controller.py +++ b/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__": diff --git a/ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_joint_trajectory_controller.py b/ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_joint_trajectory_controller.py index 27f28da1be..91d39855aa 100644 --- a/ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_joint_trajectory_controller.py +++ b/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__": From 4d9181f9ad0059b229af75df9cb4f3b1b6b3535b Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sun, 17 Nov 2024 23:11:40 +0000 Subject: [PATCH 2/5] Add tests for the test_nodes --- ros2_controllers_test_nodes/setup.py | 3 +- .../rrbot_forward_position_publisher.yaml | 11 +++ .../rrbot_joint_trajectory_publisher.yaml | 24 +++++ ...sher_forward_position_controller_launch.py | 95 +++++++++++++++++++ ...sher_joint_trajectory_controller_launch.py | 95 +++++++++++++++++++ 5 files changed, 226 insertions(+), 2 deletions(-) create mode 100644 ros2_controllers_test_nodes/test/rrbot_forward_position_publisher.yaml create mode 100644 ros2_controllers_test_nodes/test/rrbot_joint_trajectory_publisher.yaml create mode 100644 ros2_controllers_test_nodes/test/test_publisher_forward_position_controller_launch.py create mode 100644 ros2_controllers_test_nodes/test/test_publisher_joint_trajectory_controller_launch.py diff --git a/ros2_controllers_test_nodes/setup.py b/ros2_controllers_test_nodes/setup.py index fb26e9c5f7..3900392a10 100644 --- a/ros2_controllers_test_nodes/setup.py +++ b/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, diff --git a/ros2_controllers_test_nodes/test/rrbot_forward_position_publisher.yaml b/ros2_controllers_test_nodes/test/rrbot_forward_position_publisher.yaml new file mode 100644 index 0000000000..879ad34ab9 --- /dev/null +++ b/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] diff --git a/ros2_controllers_test_nodes/test/rrbot_joint_trajectory_publisher.yaml b/ros2_controllers_test_nodes/test/rrbot_joint_trajectory_publisher.yaml new file mode 100644 index 0000000000..96de7d843a --- /dev/null +++ b/ros2_controllers_test_nodes/test/rrbot_joint_trajectory_publisher.yaml @@ -0,0 +1,24 @@ +publisher_joint_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] diff --git a/ros2_controllers_test_nodes/test/test_publisher_forward_position_controller_launch.py b/ros2_controllers_test_nodes/test/test_publisher_forward_position_controller_launch.py new file mode 100644 index 0000000000..95e61f5049 --- /dev/null +++ b/ros2_controllers_test_nodes/test/test_publisher_forward_position_controller_launch.py @@ -0,0 +1,95 @@ +# 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 + +import launch_testing.markers +import rclpy +import launch_ros.actions +from rclpy.node import Node + + +# 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!" + + +@launch_testing.post_shutdown_test() +# These tests are run after the processes in generate_test_description() have shutdown. +class TestDescriptionCraneShutdown(unittest.TestCase): + + def test_exit_codes(self, proc_info): + """Check if the processes exited normally.""" + launch_testing.asserts.assertExitCodes(proc_info) diff --git a/ros2_controllers_test_nodes/test/test_publisher_joint_trajectory_controller_launch.py b/ros2_controllers_test_nodes/test/test_publisher_joint_trajectory_controller_launch.py new file mode 100644 index 0000000000..d68d4141fb --- /dev/null +++ b/ros2_controllers_test_nodes/test/test_publisher_joint_trajectory_controller_launch.py @@ -0,0 +1,95 @@ +# 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 + +import launch_testing.markers +import rclpy +import launch_ros.actions +from rclpy.node import Node + + +# 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!" + + +@launch_testing.post_shutdown_test() +# These tests are run after the processes in generate_test_description() have shutdown. +class TestDescriptionCraneShutdown(unittest.TestCase): + + def test_exit_codes(self, proc_info): + """Check if the processes exited normally.""" + launch_testing.asserts.assertExitCodes(proc_info) From 3e62d3474619e062c43b988dec36750929134199 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sun, 17 Nov 2024 23:15:01 +0000 Subject: [PATCH 3/5] Fix node name --- .../test/test_publisher_joint_trajectory_controller_launch.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ros2_controllers_test_nodes/test/test_publisher_joint_trajectory_controller_launch.py b/ros2_controllers_test_nodes/test/test_publisher_joint_trajectory_controller_launch.py index d68d4141fb..ac61883963 100644 --- a/ros2_controllers_test_nodes/test/test_publisher_joint_trajectory_controller_launch.py +++ b/ros2_controllers_test_nodes/test/test_publisher_joint_trajectory_controller_launch.py @@ -81,9 +81,9 @@ 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() + found = "publisher_position_trajectory_controller" in self.node.get_node_names() time.sleep(0.1) - assert found, "/publisher_position_trajectory_controller not found!" + assert found, "publisher_position_trajectory_controller not found!" @launch_testing.post_shutdown_test() From ad3be0eef08a36cd3644dc39da2d0c134b304e78 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 18 Nov 2024 07:49:28 +0000 Subject: [PATCH 4/5] Add tests for publishers --- ...blisher_forward_position_controller_launch.py | 16 ++++++++++++++-- ...blisher_joint_trajectory_controller_launch.py | 15 +++++++++++++-- 2 files changed, 27 insertions(+), 4 deletions(-) diff --git a/ros2_controllers_test_nodes/test/test_publisher_forward_position_controller_launch.py b/ros2_controllers_test_nodes/test/test_publisher_forward_position_controller_launch.py index 95e61f5049..8ebb2df7b3 100644 --- a/ros2_controllers_test_nodes/test/test_publisher_forward_position_controller_launch.py +++ b/ros2_controllers_test_nodes/test/test_publisher_forward_position_controller_launch.py @@ -36,12 +36,15 @@ 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 @@ -85,10 +88,19 @@ def test_node_start(self): 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 shutdown. -class TestDescriptionCraneShutdown(unittest.TestCase): +# 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.""" diff --git a/ros2_controllers_test_nodes/test/test_publisher_joint_trajectory_controller_launch.py b/ros2_controllers_test_nodes/test/test_publisher_joint_trajectory_controller_launch.py index ac61883963..94b6418e76 100644 --- a/ros2_controllers_test_nodes/test/test_publisher_joint_trajectory_controller_launch.py +++ b/ros2_controllers_test_nodes/test/test_publisher_joint_trajectory_controller_launch.py @@ -36,11 +36,13 @@ 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 @@ -85,10 +87,19 @@ def test_node_start(self): time.sleep(0.1) assert found, "publisher_position_trajectory_controller not found!" + def test_check_if_topic_published(self): + topic = "/position_trajectory_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 shutdown. -class TestDescriptionCraneShutdown(unittest.TestCase): +# 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.""" From b0cbe8e5c6b761bfa9da987278cddde90c9b67c1 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 18 Nov 2024 08:18:00 +0000 Subject: [PATCH 5/5] Fix parameter file --- .../test/rrbot_joint_trajectory_publisher.yaml | 2 +- .../test/test_publisher_joint_trajectory_controller_launch.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/ros2_controllers_test_nodes/test/rrbot_joint_trajectory_publisher.yaml b/ros2_controllers_test_nodes/test/rrbot_joint_trajectory_publisher.yaml index 96de7d843a..7dd8304134 100644 --- a/ros2_controllers_test_nodes/test/rrbot_joint_trajectory_publisher.yaml +++ b/ros2_controllers_test_nodes/test/rrbot_joint_trajectory_publisher.yaml @@ -1,4 +1,4 @@ -publisher_joint_trajectory_controller: +publisher_position_trajectory_controller: ros__parameters: controller_name: "joint_trajectory_position_controller" diff --git a/ros2_controllers_test_nodes/test/test_publisher_joint_trajectory_controller_launch.py b/ros2_controllers_test_nodes/test/test_publisher_joint_trajectory_controller_launch.py index 94b6418e76..d8d6e2a577 100644 --- a/ros2_controllers_test_nodes/test/test_publisher_joint_trajectory_controller_launch.py +++ b/ros2_controllers_test_nodes/test/test_publisher_joint_trajectory_controller_launch.py @@ -88,7 +88,7 @@ def test_node_start(self): assert found, "publisher_position_trajectory_controller not found!" def test_check_if_topic_published(self): - topic = "/position_trajectory_controller/joint_trajectory" + 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)