From dba4aa68129e0326c9af0695e8b1b6865c106c92 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Wed, 31 Jul 2024 10:18:01 +0200 Subject: [PATCH] Add launch tests (backport #540) (#549) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Add launch tests to all examples (#540) * Add test for ex1 * Add test for ex2 * Add test for ex3 * Add test for ex4 * Add missing dependency * Deactivate GUI * Add test for ex5 * Add act stuff to gitignore * Deactivate gui in tests * Reduce update rate to avoid clogging the log * Use set instead of list * Add test for ex6 * Fix xacro macros and add gui launch argument * Add test for example_7 * Add test for example_8 * Add test for example_9 * Add test for example_10 * Add test for example_11 * Add test for example_12 * Add test for example_14 * Update test for example_15 * Add test for example_15 multi_cm * Add missing dependency * Add missing dependency for ex4+ex5 * Robustify tests and reuse methos for launch_testing * Fix dependencies * Use a set to compare the joint names * Reorder controller spawners * Activate assertExitCodes tests * Deactivate failing tests for example_15 * Increase timeout for example_10 * Add backward_ros everywhere * Add error output and update includes * Revert "Activate assertExitCodes tests" This reverts commit fe28c67cb2625fbff37bd4417af8b131e5f5ee4c. * Add missing import * Update ros2_control_demo_testing/package.xml Co-authored-by: Sai Kishor Kothakota * Reuse check_node_running * Try to robustify example_14 tests --------- Co-authored-by: Sai Kishor Kothakota (cherry picked from commit 0ba16d24595ad1a221a2726f697783a961019c7c) * Fix double quotes * Increase cm update rate to decrease startup time * Remove startup delay marker --------- Co-authored-by: Christoph Fröhlich Co-authored-by: Christoph Froehlich --- .gitignore | 8 +- example_1/CMakeLists.txt | 2 + example_1/bringup/launch/rrbot.launch.py | 13 +- example_1/package.xml | 3 +- example_1/test/test_rrbot_launch.py | 99 ++++++++++++ example_10/CMakeLists.txt | 2 + .../bringup/config/rrbot_controllers.yaml | 2 +- example_10/bringup/launch/rrbot.launch.py | 21 ++- example_10/package.xml | 4 +- example_10/test/test_rrbot_launch.py | 100 ++++++++++++ example_11/CMakeLists.txt | 2 + .../bringup/launch/carlikebot.launch.py | 13 +- example_11/package.xml | 7 +- example_11/test/test_carlikebot_launch.py | 105 +++++++++++++ example_12/CMakeLists.txt | 2 + example_12/bringup/launch/rrbot.launch.py | 35 +++-- example_12/package.xml | 4 +- example_12/test/test_rrbot_launch.py | 144 ++++++++++++++++++ example_14/CMakeLists.txt | 7 +- ...eedback_sensors_for_position_feedback.yaml | 2 +- ...ck_sensors_for_position_feedback.launch.py | 13 +- .../rrbot_actuator_without_feedback.hpp | 1 - .../rrbot_sensor_for_position_feedback.hpp | 1 - .../rrbot_actuator_without_feedback.cpp | 40 ++++- .../rrbot_sensor_for_position_feedback.cpp | 7 +- example_14/package.xml | 4 +- ...ck_sensors_for_position_feedback_launch.py | 102 +++++++++++++ example_14/test/test_urdf_xacro.py | 4 +- example_15/CMakeLists.txt | 6 +- ...oller_manager_example_two_rrbots.launch.py | 18 ++- .../bringup/launch/rrbot_base.launch.py | 13 +- .../bringup/launch/rrbot_namespace.launch.py | 38 +++-- example_15/package.xml | 3 +- .../test_multi_controller_manager_launch.py | 55 ++++++- .../test/test_rrbot_namespace_launch.py | 54 ++++++- example_2/CMakeLists.txt | 2 + example_2/bringup/launch/diffbot.launch.py | 13 +- example_2/package.xml | 4 +- example_2/test/test_diffbot_launch.py | 99 ++++++++++++ example_3/CMakeLists.txt | 2 + ...t_multi_interface_forward_controllers.yaml | 2 +- .../rrbot_system_multi_interface.launch.py | 13 +- example_3/package.xml | 6 +- ...est_rrbot_system_multi_interface_launch.py | 99 ++++++++++++ example_4/CMakeLists.txt | 2 + .../config/rrbot_with_sensor_controllers.yaml | 2 +- .../launch/rrbot_system_with_sensor.launch.py | 13 +- example_4/package.xml | 5 +- .../test_rrbot_system_with_sensor_launch.py | 99 ++++++++++++ example_5/CMakeLists.txt | 2 + ...rbot_with_external_sensor_controllers.yaml | 2 +- ...rbot_system_with_external_sensor.launch.py | 13 +- example_5/package.xml | 5 +- ...rbot_system_with_external_sensor_launch.py | 99 ++++++++++++ example_6/CMakeLists.txt | 2 + .../config/rrbot_modular_actuators.yaml | 2 +- .../launch/rrbot_modular_actuators.launch.py | 13 +- example_6/package.xml | 4 +- .../test_rrbot_modular_actuators_launch.py | 99 ++++++++++++ example_7/CMakeLists.txt | 2 + .../bringup/launch/r6bot_controller.launch.py | 32 ++-- .../ros2_control/r6bot.ros2_control.xacro | 24 +-- example_7/package.xml | 4 +- .../test/test_r6bot_controller_launch.py | 101 ++++++++++++ example_8/CMakeLists.txt | 2 + ...ansmissions_system_position_only.launch.py | 13 +- example_8/package.xml | 4 +- ...ansmissions_system_position_only_launch.py | 99 ++++++++++++ example_9/CMakeLists.txt | 2 + example_9/bringup/launch/rrbot.launch.py | 13 +- example_9/package.xml | 4 +- example_9/test/test_rrbot_launch.py | 99 ++++++++++++ ros2_control_demo_testing/package.xml | 17 +++ .../resource/ros2_control_demo_testing | 0 .../ros2_control_demo_testing/__init__.py | 0 .../ros2_control_demo_testing/test_utils.py | 106 +++++++++++++ ros2_control_demo_testing/setup.cfg | 4 + ros2_control_demo_testing/setup.py | 53 +++++++ 78 files changed, 1945 insertions(+), 170 deletions(-) create mode 100644 example_1/test/test_rrbot_launch.py create mode 100644 example_10/test/test_rrbot_launch.py create mode 100644 example_11/test/test_carlikebot_launch.py create mode 100644 example_12/test/test_rrbot_launch.py create mode 100644 example_14/test/test_rrbot_modular_actuators_without_feedback_sensors_for_position_feedback_launch.py create mode 100644 example_2/test/test_diffbot_launch.py create mode 100644 example_3/test/test_rrbot_system_multi_interface_launch.py create mode 100644 example_4/test/test_rrbot_system_with_sensor_launch.py create mode 100644 example_5/test/test_rrbot_system_with_external_sensor_launch.py create mode 100644 example_6/test/test_rrbot_modular_actuators_launch.py create mode 100644 example_7/test/test_r6bot_controller_launch.py create mode 100644 example_8/test/test_rrbot_transmissions_system_position_only_launch.py create mode 100644 example_9/test/test_rrbot_launch.py create mode 100644 ros2_control_demo_testing/package.xml create mode 100644 ros2_control_demo_testing/resource/ros2_control_demo_testing create mode 100644 ros2_control_demo_testing/ros2_control_demo_testing/__init__.py create mode 100644 ros2_control_demo_testing/ros2_control_demo_testing/test_utils.py create mode 100644 ros2_control_demo_testing/setup.cfg create mode 100644 ros2_control_demo_testing/setup.py diff --git a/.gitignore b/.gitignore index 3147b10e1..02e922e35 100644 --- a/.gitignore +++ b/.gitignore @@ -1,6 +1,2 @@ -.kdev4/ - - -*~ -*.kate-swp -*.kdev4 +.ccache +.work diff --git a/example_1/CMakeLists.txt b/example_1/CMakeLists.txt index c322e118d..2ec1702ed 100644 --- a/example_1/CMakeLists.txt +++ b/example_1/CMakeLists.txt @@ -17,6 +17,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS ) # find dependencies +find_package(backward_ros REQUIRED) find_package(ament_cmake REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) @@ -66,6 +67,7 @@ if(BUILD_TESTING) ament_add_pytest_test(example_1_urdf_xacro test/test_urdf_xacro.py) ament_add_pytest_test(view_example_1_launch test/test_view_robot_launch.py) + ament_add_pytest_test(run_example_1_launch test/test_rrbot_launch.py) endif() diff --git a/example_1/bringup/launch/rrbot.launch.py b/example_1/bringup/launch/rrbot.launch.py index 975677fe1..2208b58e0 100644 --- a/example_1/bringup/launch/rrbot.launch.py +++ b/example_1/bringup/launch/rrbot.launch.py @@ -108,20 +108,21 @@ def generate_launch_description(): ) ) - # Delay start of robot_controller after `joint_state_broadcaster` - delay_robot_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler( + # Delay start of joint_state_broadcaster after `robot_controller` + # TODO(anyone): This is a workaround for flaky tests. Remove when fixed. + delay_joint_state_broadcaster_after_robot_controller_spawner = RegisterEventHandler( event_handler=OnProcessExit( - target_action=joint_state_broadcaster_spawner, - on_exit=[robot_controller_spawner], + target_action=robot_controller_spawner, + on_exit=[joint_state_broadcaster_spawner], ) ) nodes = [ control_node, robot_state_pub_node, - joint_state_broadcaster_spawner, + robot_controller_spawner, delay_rviz_after_joint_state_broadcaster_spawner, - delay_robot_controller_spawner_after_joint_state_broadcaster_spawner, + delay_joint_state_broadcaster_after_robot_controller_spawner, ] return LaunchDescription(declared_arguments + nodes) diff --git a/example_1/package.xml b/example_1/package.xml index 141351687..35257c505 100644 --- a/example_1/package.xml +++ b/example_1/package.xml @@ -15,6 +15,7 @@ ament_cmake + backward_ros hardware_interface pluginlib rclcpp @@ -34,9 +35,9 @@ xacro ament_cmake_pytest - launch_testing_ament_cmake launch_testing_ros liburdfdom-tools + ros2_control_demo_testing xacro diff --git a/example_1/test/test_rrbot_launch.py b/example_1/test/test_rrbot_launch.py new file mode 100644 index 000000000..2b25f51ae --- /dev/null +++ b/example_1/test/test_rrbot_launch.py @@ -0,0 +1,99 @@ +# 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 os +import pytest +import unittest + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_testing.actions import ReadyToTest + +# import launch_testing.markers +import rclpy +from rclpy.node import Node +from ros2_control_demo_testing.test_utils import ( + check_controllers_running, + check_if_js_published, + check_node_running, +) + + +# Executes the given launch file and checks if all nodes can be started +@pytest.mark.rostest +def generate_test_description(): + launch_include = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join( + get_package_share_directory("ros2_control_demo_example_1"), + "launch/rrbot.launch.py", + ) + ), + launch_arguments={"gui": "False"}.items(), + ) + + return LaunchDescription([launch_include, 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, proc_output): + check_node_running(self.node, "robot_state_publisher") + + def test_controller_running(self, proc_output): + + cnames = ["forward_position_controller", "joint_state_broadcaster"] + + check_controllers_running(self.node, cnames) + + def test_check_if_msgs_published(self): + check_if_js_published("/joint_states", ["joint1", "joint2"]) + + +# TODO(anyone): enable this if shutdown of ros2_control_node does not fail anymore +# @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/example_10/CMakeLists.txt b/example_10/CMakeLists.txt index 35c1319a9..a9ee61abc 100644 --- a/example_10/CMakeLists.txt +++ b/example_10/CMakeLists.txt @@ -20,6 +20,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS ) # find dependencies +find_package(backward_ros REQUIRED) find_package(ament_cmake REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) @@ -77,6 +78,7 @@ if(BUILD_TESTING) ament_add_pytest_test(example_10_urdf_xacro test/test_urdf_xacro.py) ament_add_pytest_test(view_example_10_launch test/test_view_robot_launch.py) + ament_add_pytest_test(run_example_10_launch test/test_rrbot_launch.py) endif() ## EXPORTS diff --git a/example_10/bringup/config/rrbot_controllers.yaml b/example_10/bringup/config/rrbot_controllers.yaml index 90434b572..71090a953 100644 --- a/example_10/bringup/config/rrbot_controllers.yaml +++ b/example_10/bringup/config/rrbot_controllers.yaml @@ -1,6 +1,6 @@ controller_manager: ros__parameters: - update_rate: 1 # Hz + update_rate: 10 # Hz joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster diff --git a/example_10/bringup/launch/rrbot.launch.py b/example_10/bringup/launch/rrbot.launch.py index f085790f8..2edfc25e8 100644 --- a/example_10/bringup/launch/rrbot.launch.py +++ b/example_10/bringup/launch/rrbot.launch.py @@ -96,20 +96,27 @@ def generate_launch_description(): arguments=["gpio_controller", "-c", "/controller_manager"], ) - # Delay start of robot_controller after `joint_state_broadcaster` - delay_robot_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler( + # Delay start of joint_state_broadcaster after `robot_controller` + # TODO(anyone): This is a workaround for flaky tests. Remove when fixed. + delay_gpio_after_robot_controller_spawner = RegisterEventHandler( event_handler=OnProcessExit( - target_action=joint_state_broadcaster_spawner, - on_exit=[robot_controller_spawner], + target_action=robot_controller_spawner, + on_exit=[gpio_controller_spawner], + ) + ) + delay_joint_state_broadcaster_after_gpio_controller_spawner = RegisterEventHandler( + event_handler=OnProcessExit( + target_action=gpio_controller_spawner, + on_exit=[joint_state_broadcaster_spawner], ) ) nodes = [ control_node, robot_state_pub_node, - joint_state_broadcaster_spawner, - gpio_controller_spawner, - delay_robot_controller_spawner_after_joint_state_broadcaster_spawner, + robot_controller_spawner, + delay_gpio_after_robot_controller_spawner, + delay_joint_state_broadcaster_after_gpio_controller_spawner, ] return LaunchDescription(declared_arguments + nodes) diff --git a/example_10/package.xml b/example_10/package.xml index e94c0a1e2..8943501fe 100644 --- a/example_10/package.xml +++ b/example_10/package.xml @@ -15,6 +15,7 @@ ament_cmake + backward_ros hardware_interface pluginlib rclcpp @@ -32,11 +33,10 @@ rviz2 xacro - ament_cmake_gtest ament_cmake_pytest - launch_testing_ament_cmake launch_testing_ros liburdfdom-tools + ros2_control_demo_testing xacro diff --git a/example_10/test/test_rrbot_launch.py b/example_10/test/test_rrbot_launch.py new file mode 100644 index 000000000..005ee36e8 --- /dev/null +++ b/example_10/test/test_rrbot_launch.py @@ -0,0 +1,100 @@ +# 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 os +import pytest +import unittest + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_testing.actions import ReadyToTest + +import rclpy +from rclpy.node import Node +from ros2_control_demo_testing.test_utils import ( + check_controllers_running, + check_if_js_published, + check_node_running, +) + + +# This function specifies the processes to be run for our test +# The ReadyToTest action waits for 15 second by default for the processes to +# start, if processes take more time an error is thrown. +@pytest.mark.launch_test +def generate_test_description(): + launch_include = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join( + get_package_share_directory("ros2_control_demo_example_10"), + "launch/rrbot.launch.py", + ) + ), + launch_arguments={"gui": "false"}.items(), + ) + + return LaunchDescription([launch_include, 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, proc_output): + check_node_running(self.node, "robot_state_publisher") + + def test_controller_running(self, proc_output): + + cnames = ["forward_position_controller", "gpio_controller", "joint_state_broadcaster"] + + check_controllers_running(self.node, cnames) + + def test_check_if_msgs_published(self): + check_if_js_published("/joint_states", ["joint1", "joint2"]) + + +# TODO(anyone): enable this if shutdown of ros2_control_node does not fail anymore +# @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/example_11/CMakeLists.txt b/example_11/CMakeLists.txt index 83fbf11be..e6abfba4b 100644 --- a/example_11/CMakeLists.txt +++ b/example_11/CMakeLists.txt @@ -17,6 +17,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS ) # find dependencies +find_package(backward_ros REQUIRED) find_package(ament_cmake REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) @@ -66,6 +67,7 @@ if(BUILD_TESTING) ament_add_pytest_test(example_11_urdf_xacro test/test_urdf_xacro.py) ament_add_pytest_test(view_example_11_launch test/test_view_robot_launch.py) + ament_add_pytest_test(run_example_11_launch test/test_carlikebot_launch.py) endif() ## EXPORTS diff --git a/example_11/bringup/launch/carlikebot.launch.py b/example_11/bringup/launch/carlikebot.launch.py index 231abde30..0d4a732a0 100644 --- a/example_11/bringup/launch/carlikebot.launch.py +++ b/example_11/bringup/launch/carlikebot.launch.py @@ -128,11 +128,12 @@ def generate_launch_description(): ) ) - # Delay start of robot_controller after `joint_state_broadcaster` - delay_robot_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler( + # Delay start of joint_state_broadcaster after `robot_controller` + # TODO(anyone): This is a workaround for flaky tests. Remove when fixed. + delay_joint_state_broadcaster_after_robot_controller_spawner = RegisterEventHandler( event_handler=OnProcessExit( - target_action=joint_state_broadcaster_spawner, - on_exit=[robot_bicycle_controller_spawner], + target_action=robot_bicycle_controller_spawner, + on_exit=[joint_state_broadcaster_spawner], ) ) @@ -140,9 +141,9 @@ def generate_launch_description(): control_node, control_node_remapped, robot_state_pub_bicycle_node, - joint_state_broadcaster_spawner, + robot_bicycle_controller_spawner, + delay_joint_state_broadcaster_after_robot_controller_spawner, delay_rviz_after_joint_state_broadcaster_spawner, - delay_robot_controller_spawner_after_joint_state_broadcaster_spawner, ] return LaunchDescription(declared_arguments + nodes) diff --git a/example_11/package.xml b/example_11/package.xml index 1514e9ef2..27e00444d 100644 --- a/example_11/package.xml +++ b/example_11/package.xml @@ -15,6 +15,7 @@ ament_cmake + backward_ros hardware_interface pluginlib rclcpp @@ -31,7 +32,11 @@ rviz2 xacro - ament_cmake_gtest + ament_cmake_pytest + launch_testing_ros + liburdfdom-tools + ros2_control_demo_testing + xacro ament_cmake diff --git a/example_11/test/test_carlikebot_launch.py b/example_11/test/test_carlikebot_launch.py new file mode 100644 index 000000000..5636eda21 --- /dev/null +++ b/example_11/test/test_carlikebot_launch.py @@ -0,0 +1,105 @@ +# 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 os +import pytest +import unittest + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_testing.actions import ReadyToTest + +# import launch_testing.markers +import rclpy +from rclpy.node import Node +from ros2_control_demo_testing.test_utils import ( + check_controllers_running, + check_if_js_published, + check_node_running, +) + + +# Executes the given launch file and checks if all nodes can be started +@pytest.mark.rostest +def generate_test_description(): + launch_include = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join( + get_package_share_directory("ros2_control_demo_example_11"), + "launch/carlikebot.launch.py", + ) + ), + launch_arguments={"gui": "false"}.items(), + ) + + return LaunchDescription([launch_include, 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, proc_output): + check_node_running(self.node, "robot_state_publisher") + + def test_controller_running(self, proc_output): + + cnames = ["bicycle_steering_controller", "joint_state_broadcaster"] + + check_controllers_running(self.node, cnames) + + def test_check_if_msgs_published(self): + check_if_js_published( + "/joint_states", + [ + "virtual_front_wheel_joint", + "virtual_rear_wheel_joint", + ], + ) + + +# TODO(anyone): enable this if shutdown of ros2_control_node does not fail anymore +# @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/example_12/CMakeLists.txt b/example_12/CMakeLists.txt index 7652449a9..ec887f4c3 100644 --- a/example_12/CMakeLists.txt +++ b/example_12/CMakeLists.txt @@ -23,6 +23,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS ) # find dependencies +find_package(backward_ros REQUIRED) find_package(ament_cmake REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) @@ -104,6 +105,7 @@ if(BUILD_TESTING) ament_add_pytest_test(example_12_urdf_xacro test/test_urdf_xacro.py) ament_add_pytest_test(view_example_12_launch test/test_view_robot_launch.py) + ament_add_pytest_test(run_example_12_launch test/test_rrbot_launch.py) endif() ## EXPORTS diff --git a/example_12/bringup/launch/rrbot.launch.py b/example_12/bringup/launch/rrbot.launch.py index 9bd3880e5..ac9fd1e36 100644 --- a/example_12/bringup/launch/rrbot.launch.py +++ b/example_12/bringup/launch/rrbot.launch.py @@ -14,15 +14,29 @@ from launch import LaunchDescription -from launch.actions import RegisterEventHandler +from launch.actions import DeclareLaunchArgument, RegisterEventHandler +from launch.conditions import IfCondition from launch.event_handlers import OnProcessExit -from launch.substitutions import Command, FindExecutable, PathJoinSubstitution +from launch.substitutions import Command, FindExecutable, PathJoinSubstitution, LaunchConfiguration from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare def generate_launch_description(): + # Declare arguments + declared_arguments = [] + declared_arguments.append( + DeclareLaunchArgument( + "gui", + default_value="true", + description="Start RViz2 automatically with this launch file.", + ) + ) + + # Initialize Arguments + gui = LaunchConfiguration("gui") + # Get URDF via xacro robot_description_content = Command( [ @@ -71,6 +85,7 @@ def generate_launch_description(): name="rviz2", output="log", arguments=["-d", rviz_config_file], + condition=IfCondition(gui), ) joint_state_broadcaster_spawner = Node( @@ -99,20 +114,22 @@ def generate_launch_description(): ) ) - # Delay start of robot_controller after `joint_state_broadcaster` - delay_robot_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler( + # Delay start of joint_state_broadcaster after `robot_controller` + # TODO(anyone): This is a workaround for flaky tests. Remove when fixed. + delay_joint_state_broadcaster_after_robot_controller_spawner = RegisterEventHandler( event_handler=OnProcessExit( - target_action=joint_state_broadcaster_spawner, - on_exit=[j1_controller_spawner, j2_controller_spawner], + target_action=j1_controller_spawner, + on_exit=[joint_state_broadcaster_spawner], ) ) nodes = [ control_node, robot_state_pub_node, - joint_state_broadcaster_spawner, + j1_controller_spawner, + j2_controller_spawner, delay_rviz_after_joint_state_broadcaster_spawner, - delay_robot_controller_spawner_after_joint_state_broadcaster_spawner, + delay_joint_state_broadcaster_after_robot_controller_spawner, ] - return LaunchDescription(nodes) + return LaunchDescription(declared_arguments + nodes) diff --git a/example_12/package.xml b/example_12/package.xml index 040f29593..1982b1591 100644 --- a/example_12/package.xml +++ b/example_12/package.xml @@ -16,6 +16,7 @@ ament_cmake + backward_ros hardware_interface pluginlib rclcpp @@ -33,11 +34,10 @@ rviz2 xacro - ament_cmake_gtest ament_cmake_pytest - launch_testing_ament_cmake launch_testing_ros liburdfdom-tools + ros2_control_demo_testing xacro diff --git a/example_12/test/test_rrbot_launch.py b/example_12/test/test_rrbot_launch.py new file mode 100644 index 000000000..5f716c906 --- /dev/null +++ b/example_12/test/test_rrbot_launch.py @@ -0,0 +1,144 @@ +# 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 os +import pytest +import unittest +import subprocess + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_testing.actions import ReadyToTest + +# import launch_testing.markers +import rclpy +from rclpy.node import Node +from ros2_control_demo_testing.test_utils import ( + check_controllers_running, + check_if_js_published, + check_node_running, +) + + +# Executes the given launch file and checks if all nodes can be started +@pytest.mark.rostest +def generate_test_description(): + launch_include = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join( + get_package_share_directory("ros2_control_demo_example_12"), + "launch/rrbot.launch.py", + ) + ), + launch_arguments={"gui": "false"}.items(), + ) + + return LaunchDescription([launch_include, 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, proc_output): + check_node_running(self.node, "robot_state_publisher") + + def test_controller_running(self, proc_output): + + cnames = [ + "joint1_position_controller", + "joint2_position_controller", + "joint_state_broadcaster", + ] + + check_controllers_running(self.node, cnames) + + def test_check_if_msgs_published(self): + check_if_js_published("/joint_states", ["joint1", "joint2"]) + + +# This is our second test fixture. Each method is a test case. +# These run alongside the processes specified in generate_test_description() +class TestFixtureChained(unittest.TestCase): + + def setUp(self): + rclpy.init() + self.node = Node("test_node") + # Command to run the launch file + command = [ + "ros2", + "launch", + "ros2_control_demo_example_12", + "launch_chained_controllers.launch.py", + ] + # Execute the command + subprocess.run(command) + + def tearDown(self): + self.node.destroy_node() + rclpy.shutdown() + + def test_node_start(self, proc_output): + check_node_running(self.node, "robot_state_publisher") + + def test_controller_running(self, proc_output): + + cnames = [ + "joint1_position_controller", + "joint2_position_controller", + "joint_state_broadcaster", + "position_controller", + "forward_position_controller", + ] + + check_controllers_running(self.node, cnames) + + def test_check_if_msgs_published(self): + check_if_js_published("/joint_states", ["joint1", "joint2"]) + + +# TODO(anyone): enable this if shutdown of ros2_control_node does not fail anymore +# @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/example_14/CMakeLists.txt b/example_14/CMakeLists.txt index eca9253e9..6287ceba6 100644 --- a/example_14/CMakeLists.txt +++ b/example_14/CMakeLists.txt @@ -18,6 +18,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS ) # find dependencies +find_package(backward_ros REQUIRED) find_package(ament_cmake REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) @@ -64,7 +65,11 @@ install(TARGETS ros2_control_demo_example_14 ) if(BUILD_TESTING) - find_package(ament_cmake_gtest REQUIRED) + find_package(ament_cmake_pytest REQUIRED) + + ament_add_pytest_test(example_14_urdf_xacro test/test_urdf_xacro.py) + ament_add_pytest_test(view_example_14_launch test/test_view_robot_launch.py) + ament_add_pytest_test(run_example_14_launch test/test_rrbot_modular_actuators_without_feedback_sensors_for_position_feedback_launch.py) endif() ## EXPORTS diff --git a/example_14/bringup/config/rrbot_modular_actuators_without_feedback_sensors_for_position_feedback.yaml b/example_14/bringup/config/rrbot_modular_actuators_without_feedback_sensors_for_position_feedback.yaml index 981ca5647..7763a69d1 100644 --- a/example_14/bringup/config/rrbot_modular_actuators_without_feedback_sensors_for_position_feedback.yaml +++ b/example_14/bringup/config/rrbot_modular_actuators_without_feedback_sensors_for_position_feedback.yaml @@ -1,6 +1,6 @@ controller_manager: ros__parameters: - update_rate: 100 # Hz + update_rate: 10 # Hz joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster diff --git a/example_14/bringup/launch/rrbot_modular_actuators_without_feedback_sensors_for_position_feedback.launch.py b/example_14/bringup/launch/rrbot_modular_actuators_without_feedback_sensors_for_position_feedback.launch.py index 8acdf84ce..62aac1d51 100644 --- a/example_14/bringup/launch/rrbot_modular_actuators_without_feedback_sensors_for_position_feedback.launch.py +++ b/example_14/bringup/launch/rrbot_modular_actuators_without_feedback_sensors_for_position_feedback.launch.py @@ -137,20 +137,21 @@ def generate_launch_description(): ) ) - # Delay start of robot_controller after `joint_state_broadcaster` - delay_robot_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler( + # Delay start of joint_state_broadcaster after `robot_controller` + # TODO(anyone): This is a workaround for flaky tests. Remove when fixed. + delay_joint_state_broadcaster_after_robot_controller_spawner = RegisterEventHandler( event_handler=OnProcessExit( - target_action=joint_state_broadcaster_spawner, - on_exit=[robot_controller_spawner], + target_action=robot_controller_spawner, + on_exit=[joint_state_broadcaster_spawner], ) ) nodes = [ control_node, robot_state_pub_node, - joint_state_broadcaster_spawner, + robot_controller_spawner, delay_rviz_after_joint_state_broadcaster_spawner, - delay_robot_controller_spawner_after_joint_state_broadcaster_spawner, + delay_joint_state_broadcaster_after_robot_controller_spawner, ] return LaunchDescription(declared_arguments + nodes) diff --git a/example_14/hardware/include/ros2_control_demo_example_14/rrbot_actuator_without_feedback.hpp b/example_14/hardware/include/ros2_control_demo_example_14/rrbot_actuator_without_feedback.hpp index 7a2f99609..837d3aa2e 100644 --- a/example_14/hardware/include/ros2_control_demo_example_14/rrbot_actuator_without_feedback.hpp +++ b/example_14/hardware/include/ros2_control_demo_example_14/rrbot_actuator_without_feedback.hpp @@ -20,7 +20,6 @@ #define ROS2_CONTROL_DEMO_EXAMPLE_14__RRBOT_ACTUATOR_WITHOUT_FEEDBACK_HPP_ #include -#include #include #include #include diff --git a/example_14/hardware/include/ros2_control_demo_example_14/rrbot_sensor_for_position_feedback.hpp b/example_14/hardware/include/ros2_control_demo_example_14/rrbot_sensor_for_position_feedback.hpp index 8816f7a9b..5644d714d 100644 --- a/example_14/hardware/include/ros2_control_demo_example_14/rrbot_sensor_for_position_feedback.hpp +++ b/example_14/hardware/include/ros2_control_demo_example_14/rrbot_sensor_for_position_feedback.hpp @@ -20,7 +20,6 @@ #define ROS2_CONTROL_DEMO_EXAMPLE_14__RRBOT_SENSOR_FOR_POSITION_FEEDBACK_HPP_ #include -#include #include #include #include diff --git a/example_14/hardware/rrbot_actuator_without_feedback.cpp b/example_14/hardware/rrbot_actuator_without_feedback.cpp index 6feb8067c..04d18af3f 100644 --- a/example_14/hardware/rrbot_actuator_without_feedback.cpp +++ b/example_14/hardware/rrbot_actuator_without_feedback.cpp @@ -19,8 +19,10 @@ #include "ros2_control_demo_example_14/rrbot_actuator_without_feedback.hpp" #include +#include #include #include +#include #include #include #include @@ -88,16 +90,48 @@ hardware_interface::CallbackReturn RRBotActuatorWithoutFeedback::on_init( server->h_length); address_.sin_port = htons(socket_port_); + const int max_retries = 5; + const int initial_delay_ms = 1000; // Initial delay of 1 second + RCLCPP_INFO( rclcpp::get_logger("RRBotActuatorWithoutFeedback"), "Trying to connect to port %d.", socket_port_); - if (connect(sock_, (struct sockaddr *)&address_, sizeof(address_)) < 0) + + int retries = 0; + int delay_ms = initial_delay_ms; + bool connected = false; + + while (retries < max_retries) + { + if (connect(sock_, (struct sockaddr *)&address_, sizeof(address_)) == 0) + { + connected = true; + break; + } + + RCLCPP_WARN( + rclcpp::get_logger("RRBotActuatorWithoutFeedback"), + "Connection attempt %d failed: %s. Retrying in %d ms...", retries + 1, strerror(errno), + delay_ms); + + std::this_thread::sleep_for(std::chrono::milliseconds(delay_ms)); + delay_ms *= 2; // Exponential backoff + retries++; + } + + if (!connected) { RCLCPP_FATAL( - rclcpp::get_logger("RRBotActuatorWithoutFeedback"), "Connection over socket failed."); + rclcpp::get_logger("RRBotActuatorWithoutFeedback"), + "Connection over socket failed after %d attempts: %s", retries, strerror(errno)); return hardware_interface::CallbackReturn::ERROR; } - RCLCPP_INFO(rclcpp::get_logger("RRBotActuatorWithoutFeedback"), "Connected to socket"); + else + { + RCLCPP_INFO( + rclcpp::get_logger("RRBotActuatorWithoutFeedback"), "Successfully connected to port %d.", + socket_port_); + } // END: This part here is for exemplary purposes - Please do not copy to your production code return hardware_interface::CallbackReturn::SUCCESS; diff --git a/example_14/hardware/rrbot_sensor_for_position_feedback.cpp b/example_14/hardware/rrbot_sensor_for_position_feedback.cpp index 395c922c6..96e7ee866 100644 --- a/example_14/hardware/rrbot_sensor_for_position_feedback.cpp +++ b/example_14/hardware/rrbot_sensor_for_position_feedback.cpp @@ -18,10 +18,11 @@ #include "ros2_control_demo_example_14/rrbot_sensor_for_position_feedback.hpp" -#include +#include #include #include #include +#include #include #include #include @@ -101,7 +102,9 @@ hardware_interface::CallbackReturn RRBotSensorPositionFeedback::on_init( RCLCPP_INFO(rclcpp::get_logger("RRBotSensorPositionFeedback"), "Binding to socket address."); if (bind(obj_socket_, reinterpret_cast(&address_), sizeof(address_)) < 0) { - RCLCPP_FATAL(rclcpp::get_logger("RRBotSensorPositionFeedback"), "Binding to socket failed."); + RCLCPP_FATAL( + rclcpp::get_logger("RRBotSensorPositionFeedback"), "Binding to socket failed: %s", + strerror(errno)); // Print the error message return hardware_interface::CallbackReturn::ERROR; } diff --git a/example_14/package.xml b/example_14/package.xml index 3b52728ab..6c9672bb0 100644 --- a/example_14/package.xml +++ b/example_14/package.xml @@ -24,17 +24,17 @@ joint_state_broadcaster joint_state_publisher_gui robot_state_publisher + ros2_control_demo_description ros2_controllers_test_nodes ros2controlcli ros2launch rviz2 xacro - ament_cmake_gtest ament_cmake_pytest - launch_testing_ament_cmake launch_testing_ros liburdfdom-tools + ros2_control_demo_testing xacro diff --git a/example_14/test/test_rrbot_modular_actuators_without_feedback_sensors_for_position_feedback_launch.py b/example_14/test/test_rrbot_modular_actuators_without_feedback_sensors_for_position_feedback_launch.py new file mode 100644 index 000000000..9f8158944 --- /dev/null +++ b/example_14/test/test_rrbot_modular_actuators_without_feedback_sensors_for_position_feedback_launch.py @@ -0,0 +1,102 @@ +# 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 os +import pytest +import unittest + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_testing.actions import ReadyToTest + +# import launch_testing.markers +import rclpy +from rclpy.node import Node +from ros2_control_demo_testing.test_utils import ( + check_controllers_running, + check_if_js_published, + check_node_running, +) + + +# Executes the given launch file and checks if all nodes can be started +@pytest.mark.rostest +def generate_test_description(): + launch_include = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join( + get_package_share_directory("ros2_control_demo_example_14"), + "launch/rrbot_modular_actuators_without_feedback_sensors_for_position_feedback.launch.py", + ) + ), + launch_arguments={"gui": "false"}.items(), + ) + + return LaunchDescription([launch_include, 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, proc_output): + check_node_running(self.node, "robot_state_publisher") + + def test_controller_running(self, proc_output): + + cnames = [ + "forward_velocity_controller", + "joint_state_broadcaster", + ] + + check_controllers_running(self.node, cnames) + + def test_check_if_msgs_published(self): + check_if_js_published("/joint_states", ["joint1", "joint2"]) + + +# TODO(anyone): enable this if shutdown of ros2_control_node does not fail anymore +# @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/example_14/test/test_urdf_xacro.py b/example_14/test/test_urdf_xacro.py index 60d784a7e..6697b8583 100644 --- a/example_14/test/test_urdf_xacro.py +++ b/example_14/test/test_urdf_xacro.py @@ -39,7 +39,9 @@ def test_urdf_xacro(): # General Arguments description_package = "ros2_control_demo_example_14" - description_file = "rrbot.urdf.xacro" + description_file = ( + "rrbot_modular_actuators_without_feedback_sensors_for_position_feedback.urdf.xacro" + ) description_file_path = os.path.join( get_package_share_directory(description_package), "urdf", description_file diff --git a/example_15/CMakeLists.txt b/example_15/CMakeLists.txt index 84e220a57..2ca67931b 100644 --- a/example_15/CMakeLists.txt +++ b/example_15/CMakeLists.txt @@ -26,11 +26,11 @@ install( ) if(BUILD_TESTING) - find_package(ament_cmake_gtest REQUIRED) find_package(ament_cmake_pytest REQUIRED) - ament_add_pytest_test(test_rrbot_namespace_launch test/test_rrbot_namespace_launch.py) - ament_add_pytest_test(test_multi_controller_manager_launch test/test_multi_controller_manager_launch.py) + # TODO(christophfroehlich) deactivated because of bug in spawner + # ament_add_pytest_test(test_rrbot_namespace_launch test/test_rrbot_namespace_launch.py) + # ament_add_pytest_test(test_multi_controller_manager_launch test/test_multi_controller_manager_launch.py) endif() diff --git a/example_15/bringup/launch/multi_controller_manager_example_two_rrbots.launch.py b/example_15/bringup/launch/multi_controller_manager_example_two_rrbots.launch.py index b96e5426a..1af2ee8b4 100644 --- a/example_15/bringup/launch/multi_controller_manager_example_two_rrbots.launch.py +++ b/example_15/bringup/launch/multi_controller_manager_example_two_rrbots.launch.py @@ -14,6 +14,7 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, ThisLaunchFileDir @@ -51,6 +52,16 @@ def generate_launch_description(): description="Robot controller to start.", ) ) + declared_arguments.append( + DeclareLaunchArgument( + "start_rviz_multi", + default_value="true", + description="Start RViz2 automatically with this launch file.", + ) + ) + + # Initialize Arguments + start_rviz_lc = LaunchConfiguration("start_rviz_multi") # Initialize Arguments use_mock_hardware = LaunchConfiguration("use_mock_hardware") @@ -120,12 +131,13 @@ def generate_launch_description(): [FindPackageShare("ros2_control_demo_example_15"), "rviz", "multi_controller_manager.rviz"] ) - rviz_node = Node( + rviz_node_multi = Node( package="rviz2", executable="rviz2", - name="rviz2", + name="rviz2_multi", output="log", arguments=["-d", rviz_config_file], + condition=IfCondition(start_rviz_lc), ) included_launch_files = [ @@ -136,7 +148,7 @@ def generate_launch_description(): nodes_to_start = [ rrbot_1_position_trajectory_controller_spawner, rrbot_2_position_trajectory_controller_spawner, - rviz_node, + rviz_node_multi, ] return LaunchDescription(declared_arguments + included_launch_files + nodes_to_start) diff --git a/example_15/bringup/launch/rrbot_base.launch.py b/example_15/bringup/launch/rrbot_base.launch.py index 1de7e6900..b64b4e192 100644 --- a/example_15/bringup/launch/rrbot_base.launch.py +++ b/example_15/bringup/launch/rrbot_base.launch.py @@ -220,12 +220,21 @@ def generate_launch_description(): ) ) + # Delay start of joint_state_broadcaster after `robot_controller` + # TODO(anyone): This is a workaround for flaky tests. Remove when fixed. + delay_joint_state_broadcaster_after_robot_controller_spawner = RegisterEventHandler( + event_handler=OnProcessExit( + target_action=robot_controller_spawner, + on_exit=[joint_state_broadcaster_spawner], + ) + ) + nodes = [ control_node, robot_state_pub_node, - joint_state_broadcaster_spawner, - delay_rviz_after_joint_state_broadcaster_spawner, robot_controller_spawner, + delay_joint_state_broadcaster_after_robot_controller_spawner, + delay_rviz_after_joint_state_broadcaster_spawner, ] return LaunchDescription(declared_arguments + nodes) diff --git a/example_15/bringup/launch/rrbot_namespace.launch.py b/example_15/bringup/launch/rrbot_namespace.launch.py index 0cd59332b..3fab3632e 100644 --- a/example_15/bringup/launch/rrbot_namespace.launch.py +++ b/example_15/bringup/launch/rrbot_namespace.launch.py @@ -14,15 +14,29 @@ from launch import LaunchDescription -from launch.actions import RegisterEventHandler +from launch.actions import DeclareLaunchArgument, RegisterEventHandler +from launch.conditions import IfCondition from launch.event_handlers import OnProcessExit -from launch.substitutions import Command, FindExecutable, PathJoinSubstitution +from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare def generate_launch_description(): + # Declare arguments + declared_arguments = [] + declared_arguments.append( + DeclareLaunchArgument( + "start_rviz", + default_value="true", + description="Start RViz2 automatically with this launch file.", + ) + ) + + # Initialize Arguments + start_rviz = LaunchConfiguration("start_rviz") + # Get URDF via xacro robot_description_content = Command( [ @@ -79,6 +93,7 @@ def generate_launch_description(): name="rviz2", output="log", arguments=["-d", rviz_config_file], + condition=IfCondition(start_rviz), ) joint_state_broadcaster_spawner = Node( @@ -112,13 +127,12 @@ def generate_launch_description(): ) ) - # Delay start of robot_controller after `joint_state_broadcaster` - delay_robot_forward_position_controller_spawner_after_joint_state_broadcaster_spawner = ( - RegisterEventHandler( - event_handler=OnProcessExit( - target_action=joint_state_broadcaster_spawner, - on_exit=[robot_forward_position_controller_spawner], - ) + # Delay start of joint_state_broadcaster after `robot_controller` + # TODO(anyone): This is a workaround for flaky tests. Remove when fixed. + delay_joint_state_broadcaster_after_robot_controller_spawner = RegisterEventHandler( + event_handler=OnProcessExit( + target_action=robot_forward_position_controller_spawner, + on_exit=[joint_state_broadcaster_spawner], ) ) @@ -135,10 +149,10 @@ def generate_launch_description(): nodes = [ control_node, robot_state_pub_node, - joint_state_broadcaster_spawner, delay_rviz_after_joint_state_broadcaster_spawner, - delay_robot_forward_position_controller_spawner_after_joint_state_broadcaster_spawner, + robot_forward_position_controller_spawner, + delay_joint_state_broadcaster_after_robot_controller_spawner, delay_robot_position_trajectory_controller_spawner_after_joint_state_broadcaster_spawner, ] - return LaunchDescription(nodes) + return LaunchDescription(declared_arguments + nodes) diff --git a/example_15/package.xml b/example_15/package.xml index 8aea9905c..b4594cb17 100644 --- a/example_15/package.xml +++ b/example_15/package.xml @@ -29,11 +29,10 @@ rviz2 xacro - ament_cmake_gtest ament_cmake_pytest - launch_testing_ament_cmake launch_testing_ros liburdfdom-tools + ros2_control_demo_testing xacro diff --git a/example_15/test/test_multi_controller_manager_launch.py b/example_15/test/test_multi_controller_manager_launch.py index f86fcb44f..a927fdb6d 100644 --- a/example_15/test/test_multi_controller_manager_launch.py +++ b/example_15/test/test_multi_controller_manager_launch.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022 FZI Forschungszentrum Informatik +# 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: @@ -26,10 +26,11 @@ # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. # -# Author: Lukas Sackewitz +# Author: Christoph Froehlich import os import pytest +import unittest from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription @@ -37,6 +38,15 @@ from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_testing.actions import ReadyToTest +# import launch_testing.markers +import rclpy +from rclpy.node import Node +from ros2_control_demo_testing.test_utils import ( + check_controllers_running, + check_if_js_published, + check_node_running, +) + # Executes the given launch file and checks if all nodes can be started @pytest.mark.rostest @@ -48,7 +58,46 @@ def generate_test_description(): "launch/multi_controller_manager_example_two_rrbots.launch.py", ) ), - launch_arguments={"gui": "true"}.items(), + launch_arguments={"start_rviz_multi": "false"}.items(), ) return LaunchDescription([launch_include, 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, proc_output): + check_node_running(self.node, "robot_state_publisher") + + def test_controller_running_cm1(self, proc_output): + + cnames = [ + "forward_position_controller", + "joint_state_broadcaster", + ] + check_controllers_running(self.node, cnames, "/rrbot_1") + check_controllers_running(self.node, cnames, "/rrbot_2") + + def test_check_if_msgs_published(self): + check_if_js_published("/rrbot_1/joint_states", ["rrbot_1_joint1", "rrbot_1_joint2"]) + check_if_js_published("/rrbot_2/joint_states", ["rrbot_2_joint1", "rrbot_2_joint2"]) + + +# TODO(anyone): enable this if shutdown of ros2_control_node does not fail anymore +# @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/example_15/test/test_rrbot_namespace_launch.py b/example_15/test/test_rrbot_namespace_launch.py index 806fa94a6..a4e5dfb97 100644 --- a/example_15/test/test_rrbot_namespace_launch.py +++ b/example_15/test/test_rrbot_namespace_launch.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022 FZI Forschungszentrum Informatik +# 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: @@ -26,10 +26,11 @@ # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. # -# Author: Lukas Sackewitz +# Author: Christoph Froehlich import os import pytest +import unittest from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription @@ -37,6 +38,15 @@ from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_testing.actions import ReadyToTest +# import launch_testing.markers +import rclpy +from rclpy.node import Node +from ros2_control_demo_testing.test_utils import ( + check_controllers_running, + check_if_js_published, + check_node_running, +) + # Executes the given launch file and checks if all nodes can be started @pytest.mark.rostest @@ -48,7 +58,45 @@ def generate_test_description(): "launch/rrbot_namespace.launch.py", ) ), - launch_arguments={"gui": "true"}.items(), + launch_arguments={"start_rviz": "false"}.items(), ) return LaunchDescription([launch_include, 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, proc_output): + check_node_running(self.node, "robot_state_publisher") + + def test_controller_running(self, proc_output): + + cnames = [ + "forward_position_controller", + "joint_state_broadcaster", + ] + + check_controllers_running(self.node, cnames, "/rrbot") + + def test_check_if_msgs_published(self): + check_if_js_published("/rrbot/joint_states", ["joint1", "joint2"]) + + +# TODO(anyone): enable this if shutdown of ros2_control_node does not fail anymore +# @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/example_2/CMakeLists.txt b/example_2/CMakeLists.txt index 01150f422..f419e6fde 100644 --- a/example_2/CMakeLists.txt +++ b/example_2/CMakeLists.txt @@ -17,6 +17,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS ) # find dependencies +find_package(backward_ros REQUIRED) find_package(ament_cmake REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) @@ -66,6 +67,7 @@ if(BUILD_TESTING) ament_add_pytest_test(example_2_urdf_xacro test/test_urdf_xacro.py) ament_add_pytest_test(view_example_2_launch test/test_view_robot_launch.py) + ament_add_pytest_test(run_example_2_launch test/test_diffbot_launch.py) endif() ## EXPORTS diff --git a/example_2/bringup/launch/diffbot.launch.py b/example_2/bringup/launch/diffbot.launch.py index f70b415ce..1e1be4b0c 100644 --- a/example_2/bringup/launch/diffbot.launch.py +++ b/example_2/bringup/launch/diffbot.launch.py @@ -117,20 +117,21 @@ def generate_launch_description(): ) ) - # Delay start of robot_controller after `joint_state_broadcaster` - delay_robot_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler( + # Delay start of joint_state_broadcaster after `robot_controller` + # TODO(anyone): This is a workaround for flaky tests. Remove when fixed. + delay_joint_state_broadcaster_after_robot_controller_spawner = RegisterEventHandler( event_handler=OnProcessExit( - target_action=joint_state_broadcaster_spawner, - on_exit=[robot_controller_spawner], + target_action=robot_controller_spawner, + on_exit=[joint_state_broadcaster_spawner], ) ) nodes = [ control_node, robot_state_pub_node, - joint_state_broadcaster_spawner, + robot_controller_spawner, delay_rviz_after_joint_state_broadcaster_spawner, - delay_robot_controller_spawner_after_joint_state_broadcaster_spawner, + delay_joint_state_broadcaster_after_robot_controller_spawner, ] return LaunchDescription(declared_arguments + nodes) diff --git a/example_2/package.xml b/example_2/package.xml index ef767ca3f..54c8f0876 100644 --- a/example_2/package.xml +++ b/example_2/package.xml @@ -15,6 +15,7 @@ ament_cmake + backward_ros hardware_interface pluginlib rclcpp @@ -32,11 +33,10 @@ rviz2 xacro - ament_cmake_gtest ament_cmake_pytest - launch_testing_ament_cmake launch_testing_ros liburdfdom-tools + ros2_control_demo_testing xacro diff --git a/example_2/test/test_diffbot_launch.py b/example_2/test/test_diffbot_launch.py new file mode 100644 index 000000000..5f762872b --- /dev/null +++ b/example_2/test/test_diffbot_launch.py @@ -0,0 +1,99 @@ +# 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 os +import pytest +import unittest + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_testing.actions import ReadyToTest + +# import launch_testing.markers +import rclpy +from rclpy.node import Node +from ros2_control_demo_testing.test_utils import ( + check_controllers_running, + check_if_js_published, + check_node_running, +) + + +# Executes the given launch file and checks if all nodes can be started +@pytest.mark.rostest +def generate_test_description(): + launch_include = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join( + get_package_share_directory("ros2_control_demo_example_2"), + "launch/diffbot.launch.py", + ) + ), + launch_arguments={"gui": "False"}.items(), + ) + + return LaunchDescription([launch_include, 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, proc_output): + check_node_running(self.node, "robot_state_publisher") + + def test_controller_running(self, proc_output): + + cnames = ["diffbot_base_controller", "joint_state_broadcaster"] + + check_controllers_running(self.node, cnames) + + def test_check_if_msgs_published(self): + check_if_js_published("/joint_states", ["left_wheel_joint", "right_wheel_joint"]) + + +# TODO(anyone): enable this if shutdown of ros2_control_node does not fail anymore +# @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/example_3/CMakeLists.txt b/example_3/CMakeLists.txt index cf5470c6e..703c47f9d 100644 --- a/example_3/CMakeLists.txt +++ b/example_3/CMakeLists.txt @@ -17,6 +17,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS ) # find dependencies +find_package(backward_ros REQUIRED) find_package(ament_cmake REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) @@ -66,6 +67,7 @@ if(BUILD_TESTING) ament_add_pytest_test(example_3_urdf_xacro test/test_urdf_xacro.py) ament_add_pytest_test(view_example_3_launch test/test_view_robot_launch.py) + ament_add_pytest_test(run_example_3_launch test/test_rrbot_system_multi_interface_launch.py) endif() ## EXPORTS diff --git a/example_3/bringup/config/rrbot_multi_interface_forward_controllers.yaml b/example_3/bringup/config/rrbot_multi_interface_forward_controllers.yaml index 0b832d944..6bffd2754 100644 --- a/example_3/bringup/config/rrbot_multi_interface_forward_controllers.yaml +++ b/example_3/bringup/config/rrbot_multi_interface_forward_controllers.yaml @@ -1,6 +1,6 @@ controller_manager: ros__parameters: - update_rate: 100 # Hz + update_rate: 10 # Hz forward_position_controller: type: position_controllers/JointGroupPositionController diff --git a/example_3/bringup/launch/rrbot_system_multi_interface.launch.py b/example_3/bringup/launch/rrbot_system_multi_interface.launch.py index c197fa5e9..112a56f0d 100644 --- a/example_3/bringup/launch/rrbot_system_multi_interface.launch.py +++ b/example_3/bringup/launch/rrbot_system_multi_interface.launch.py @@ -160,20 +160,21 @@ def generate_launch_description(): ) ) - # Delay start of robot_controller after `joint_state_broadcaster` - delay_robot_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler( + # Delay start of joint_state_broadcaster after `robot_controller` + # TODO(anyone): This is a workaround for flaky tests. Remove when fixed. + delay_joint_state_broadcaster_after_robot_controller_spawner = RegisterEventHandler( event_handler=OnProcessExit( - target_action=joint_state_broadcaster_spawner, - on_exit=[robot_controller_spawner], + target_action=robot_controller_spawner, + on_exit=[joint_state_broadcaster_spawner], ) ) nodes = [ control_node, robot_state_pub_node, - joint_state_broadcaster_spawner, + robot_controller_spawner, delay_rviz_after_joint_state_broadcaster_spawner, - delay_robot_controller_spawner_after_joint_state_broadcaster_spawner, + delay_joint_state_broadcaster_after_robot_controller_spawner, ] return LaunchDescription(declared_arguments + nodes) diff --git a/example_3/package.xml b/example_3/package.xml index 3c0a21687..289e9979b 100644 --- a/example_3/package.xml +++ b/example_3/package.xml @@ -13,6 +13,7 @@ ament_cmake + backward_ros hardware_interface pluginlib rclcpp @@ -22,19 +23,20 @@ forward_command_controller joint_state_broadcaster joint_state_publisher_gui + position_controllers robot_state_publisher ros2_control_demo_description ros2_controllers_test_nodes ros2controlcli ros2launch rviz2 + velocity_controllers xacro - ament_cmake_gtest ament_cmake_pytest - launch_testing_ament_cmake launch_testing_ros liburdfdom-tools + ros2_control_demo_testing xacro diff --git a/example_3/test/test_rrbot_system_multi_interface_launch.py b/example_3/test/test_rrbot_system_multi_interface_launch.py new file mode 100644 index 000000000..8f6394df2 --- /dev/null +++ b/example_3/test/test_rrbot_system_multi_interface_launch.py @@ -0,0 +1,99 @@ +# 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 os +import pytest +import unittest + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_testing.actions import ReadyToTest + +# import launch_testing.markers +import rclpy +from rclpy.node import Node +from ros2_control_demo_testing.test_utils import ( + check_controllers_running, + check_if_js_published, + check_node_running, +) + + +# Executes the given launch file and checks if all nodes can be started +@pytest.mark.rostest +def generate_test_description(): + launch_include = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join( + get_package_share_directory("ros2_control_demo_example_3"), + "launch/rrbot_system_multi_interface.launch.py", + ) + ), + launch_arguments={"gui": "False"}.items(), + ) + + return LaunchDescription([launch_include, 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, proc_output): + check_node_running(self.node, "robot_state_publisher") + + def test_controller_running(self, proc_output): + + cnames = ["forward_velocity_controller", "joint_state_broadcaster"] + + check_controllers_running(self.node, cnames) + + def test_check_if_msgs_published(self): + check_if_js_published("/joint_states", ["joint1", "joint2"]) + + +# TODO(anyone): enable this if shutdown of ros2_control_node does not fail anymore +# @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/example_4/CMakeLists.txt b/example_4/CMakeLists.txt index a56654e0e..0666fc26d 100644 --- a/example_4/CMakeLists.txt +++ b/example_4/CMakeLists.txt @@ -17,6 +17,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS ) # find dependencies +find_package(backward_ros REQUIRED) find_package(ament_cmake REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) @@ -66,6 +67,7 @@ if(BUILD_TESTING) ament_add_pytest_test(example_4_urdf_xacro test/test_urdf_xacro.py) ament_add_pytest_test(view_example_4_launch test/test_view_robot_launch.py) + ament_add_pytest_test(run_example_4_launch test/test_rrbot_system_with_sensor_launch.py) endif() ## EXPORTS diff --git a/example_4/bringup/config/rrbot_with_sensor_controllers.yaml b/example_4/bringup/config/rrbot_with_sensor_controllers.yaml index c086d4e40..bbd2c5d8e 100644 --- a/example_4/bringup/config/rrbot_with_sensor_controllers.yaml +++ b/example_4/bringup/config/rrbot_with_sensor_controllers.yaml @@ -1,6 +1,6 @@ controller_manager: ros__parameters: - update_rate: 100 # Hz + update_rate: 10 # Hz forward_position_controller: type: forward_command_controller/ForwardCommandController diff --git a/example_4/bringup/launch/rrbot_system_with_sensor.launch.py b/example_4/bringup/launch/rrbot_system_with_sensor.launch.py index 86de28714..d2235fe36 100644 --- a/example_4/bringup/launch/rrbot_system_with_sensor.launch.py +++ b/example_4/bringup/launch/rrbot_system_with_sensor.launch.py @@ -159,20 +159,21 @@ def generate_launch_description(): ) ) - # Delay start of robot_controller after `joint_state_broadcaster` - delay_robot_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler( + # Delay start of joint_state_broadcaster after `robot_controller` + # TODO(anyone): This is a workaround for flaky tests. Remove when fixed. + delay_joint_state_broadcaster_after_robot_controller_spawner = RegisterEventHandler( event_handler=OnProcessExit( - target_action=joint_state_broadcaster_spawner, - on_exit=[robot_controller_spawner], + target_action=robot_controller_spawner, + on_exit=[joint_state_broadcaster_spawner], ) ) nodes = [ control_node, robot_state_pub_node, - joint_state_broadcaster_spawner, + robot_controller_spawner, delay_rviz_after_joint_state_broadcaster_spawner, - delay_robot_controller_spawner_after_joint_state_broadcaster_spawner, + delay_joint_state_broadcaster_after_robot_controller_spawner, fts_broadcaster_spawner, ] diff --git a/example_4/package.xml b/example_4/package.xml index 7ff6a1c2e..0f5ed8a8f 100644 --- a/example_4/package.xml +++ b/example_4/package.xml @@ -13,12 +13,14 @@ ament_cmake + backward_ros hardware_interface pluginlib rclcpp rclcpp_lifecycle controller_manager + force_torque_sensor_broadcaster forward_command_controller joint_state_broadcaster joint_state_publisher_gui @@ -30,11 +32,10 @@ rviz2 xacro - ament_cmake_gtest ament_cmake_pytest - launch_testing_ament_cmake launch_testing_ros liburdfdom-tools + ros2_control_demo_testing xacro diff --git a/example_4/test/test_rrbot_system_with_sensor_launch.py b/example_4/test/test_rrbot_system_with_sensor_launch.py new file mode 100644 index 000000000..d9238acf2 --- /dev/null +++ b/example_4/test/test_rrbot_system_with_sensor_launch.py @@ -0,0 +1,99 @@ +# 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 os +import pytest +import unittest + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_testing.actions import ReadyToTest + +# import launch_testing.markers +import rclpy +from rclpy.node import Node +from ros2_control_demo_testing.test_utils import ( + check_controllers_running, + check_if_js_published, + check_node_running, +) + + +# Executes the given launch file and checks if all nodes can be started +@pytest.mark.rostest +def generate_test_description(): + launch_include = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join( + get_package_share_directory("ros2_control_demo_example_4"), + "launch/rrbot_system_with_sensor.launch.py", + ) + ), + launch_arguments={"gui": "False"}.items(), + ) + + return LaunchDescription([launch_include, 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, proc_output): + check_node_running(self.node, "robot_state_publisher") + + def test_controller_running(self, proc_output): + + cnames = ["forward_position_controller", "fts_broadcaster", "joint_state_broadcaster"] + + check_controllers_running(self.node, cnames) + + def test_check_if_msgs_published(self): + check_if_js_published("/joint_states", ["joint1", "joint2"]) + + +# TODO(anyone): enable this if shutdown of ros2_control_node does not fail anymore +# @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/example_5/CMakeLists.txt b/example_5/CMakeLists.txt index c4ee34da2..b5e2ed8cd 100644 --- a/example_5/CMakeLists.txt +++ b/example_5/CMakeLists.txt @@ -17,6 +17,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS ) # find dependencies +find_package(backward_ros REQUIRED) find_package(ament_cmake REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) @@ -67,6 +68,7 @@ if(BUILD_TESTING) ament_add_pytest_test(example_5_urdf_xacro test/test_urdf_xacro.py) ament_add_pytest_test(view_example_5_launch test/test_view_robot_launch.py) + ament_add_pytest_test(run_example_5_launch test/test_rrbot_system_with_external_sensor_launch.py) endif() ## EXPORTS diff --git a/example_5/bringup/config/rrbot_with_external_sensor_controllers.yaml b/example_5/bringup/config/rrbot_with_external_sensor_controllers.yaml index c7759ea67..5b650ea7e 100644 --- a/example_5/bringup/config/rrbot_with_external_sensor_controllers.yaml +++ b/example_5/bringup/config/rrbot_with_external_sensor_controllers.yaml @@ -1,6 +1,6 @@ controller_manager: ros__parameters: - update_rate: 100 # Hz + update_rate: 10 # Hz forward_position_controller: type: forward_command_controller/ForwardCommandController diff --git a/example_5/bringup/launch/rrbot_system_with_external_sensor.launch.py b/example_5/bringup/launch/rrbot_system_with_external_sensor.launch.py index 8fe324224..6253f215c 100755 --- a/example_5/bringup/launch/rrbot_system_with_external_sensor.launch.py +++ b/example_5/bringup/launch/rrbot_system_with_external_sensor.launch.py @@ -159,20 +159,21 @@ def generate_launch_description(): ) ) - # Delay start of robot_controller after `joint_state_broadcaster` - delay_robot_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler( + # Delay start of joint_state_broadcaster after `robot_controller` + # TODO(anyone): This is a workaround for flaky tests. Remove when fixed. + delay_joint_state_broadcaster_after_robot_controller_spawner = RegisterEventHandler( event_handler=OnProcessExit( - target_action=joint_state_broadcaster_spawner, - on_exit=[robot_controller_spawner], + target_action=robot_controller_spawner, + on_exit=[joint_state_broadcaster_spawner], ) ) nodes = [ control_node, robot_state_pub_node, - joint_state_broadcaster_spawner, + robot_controller_spawner, delay_rviz_after_joint_state_broadcaster_spawner, - delay_robot_controller_spawner_after_joint_state_broadcaster_spawner, + delay_joint_state_broadcaster_after_robot_controller_spawner, fts_broadcaster_spawner, ] diff --git a/example_5/package.xml b/example_5/package.xml index e22bf795f..4e20d02be 100644 --- a/example_5/package.xml +++ b/example_5/package.xml @@ -13,12 +13,14 @@ ament_cmake + backward_ros hardware_interface pluginlib rclcpp rclcpp_lifecycle controller_manager + force_torque_sensor_broadcaster forward_command_controller joint_state_broadcaster joint_state_publisher_gui @@ -30,11 +32,10 @@ rviz2 xacro - ament_cmake_gtest ament_cmake_pytest - launch_testing_ament_cmake launch_testing_ros liburdfdom-tools + ros2_control_demo_testing xacro diff --git a/example_5/test/test_rrbot_system_with_external_sensor_launch.py b/example_5/test/test_rrbot_system_with_external_sensor_launch.py new file mode 100644 index 000000000..cce21b560 --- /dev/null +++ b/example_5/test/test_rrbot_system_with_external_sensor_launch.py @@ -0,0 +1,99 @@ +# 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 os +import pytest +import unittest + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_testing.actions import ReadyToTest + +# import launch_testing.markers +import rclpy +from rclpy.node import Node +from ros2_control_demo_testing.test_utils import ( + check_controllers_running, + check_if_js_published, + check_node_running, +) + + +# Executes the given launch file and checks if all nodes can be started +@pytest.mark.rostest +def generate_test_description(): + launch_include = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join( + get_package_share_directory("ros2_control_demo_example_5"), + "launch/rrbot_system_with_external_sensor.launch.py", + ) + ), + launch_arguments={"gui": "false"}.items(), + ) + + return LaunchDescription([launch_include, 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, proc_output): + check_node_running(self.node, "robot_state_publisher") + + def test_controller_running(self, proc_output): + + cnames = ["forward_position_controller", "fts_broadcaster", "joint_state_broadcaster"] + + check_controllers_running(self.node, cnames) + + def test_check_if_msgs_published(self): + check_if_js_published("/joint_states", ["joint1", "joint2"]) + + +# TODO(anyone): enable this if shutdown of ros2_control_node does not fail anymore +# @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/example_6/CMakeLists.txt b/example_6/CMakeLists.txt index 612a1c65c..da5ce30e4 100644 --- a/example_6/CMakeLists.txt +++ b/example_6/CMakeLists.txt @@ -17,6 +17,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS ) # find dependencies +find_package(backward_ros REQUIRED) find_package(ament_cmake REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) @@ -66,6 +67,7 @@ if(BUILD_TESTING) ament_add_pytest_test(example_6_urdf_xacro test/test_urdf_xacro.py) ament_add_pytest_test(view_example_6_launch test/test_view_robot_launch.py) + ament_add_pytest_test(run_example_6_launch test/test_rrbot_modular_actuators_launch.py) endif() ## EXPORTS diff --git a/example_6/bringup/config/rrbot_modular_actuators.yaml b/example_6/bringup/config/rrbot_modular_actuators.yaml index 15846b724..66f79d0ff 100644 --- a/example_6/bringup/config/rrbot_modular_actuators.yaml +++ b/example_6/bringup/config/rrbot_modular_actuators.yaml @@ -1,6 +1,6 @@ controller_manager: ros__parameters: - update_rate: 100 # Hz + update_rate: 10 # Hz joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster diff --git a/example_6/bringup/launch/rrbot_modular_actuators.launch.py b/example_6/bringup/launch/rrbot_modular_actuators.launch.py index 5f15324f5..8f4584098 100644 --- a/example_6/bringup/launch/rrbot_modular_actuators.launch.py +++ b/example_6/bringup/launch/rrbot_modular_actuators.launch.py @@ -160,20 +160,21 @@ def generate_launch_description(): ) ) - # Delay start of robot_controller after `joint_state_broadcaster` - delay_robot_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler( + # Delay start of joint_state_broadcaster after `robot_controller` + # TODO(anyone): This is a workaround for flaky tests. Remove when fixed. + delay_joint_state_broadcaster_after_robot_controller_spawner = RegisterEventHandler( event_handler=OnProcessExit( - target_action=joint_state_broadcaster_spawner, - on_exit=[robot_controller_spawner], + target_action=robot_controller_spawner, + on_exit=[joint_state_broadcaster_spawner], ) ) nodes = [ control_node, robot_state_pub_node, - joint_state_broadcaster_spawner, + robot_controller_spawner, delay_rviz_after_joint_state_broadcaster_spawner, - delay_robot_controller_spawner_after_joint_state_broadcaster_spawner, + delay_joint_state_broadcaster_after_robot_controller_spawner, ] return LaunchDescription(declared_arguments + nodes) diff --git a/example_6/package.xml b/example_6/package.xml index 856970fef..0e08490fe 100644 --- a/example_6/package.xml +++ b/example_6/package.xml @@ -13,6 +13,7 @@ ament_cmake + backward_ros hardware_interface pluginlib rclcpp @@ -30,11 +31,10 @@ rviz2 xacro - ament_cmake_gtest ament_cmake_pytest - launch_testing_ament_cmake launch_testing_ros liburdfdom-tools + ros2_control_demo_testing xacro diff --git a/example_6/test/test_rrbot_modular_actuators_launch.py b/example_6/test/test_rrbot_modular_actuators_launch.py new file mode 100644 index 000000000..b48389713 --- /dev/null +++ b/example_6/test/test_rrbot_modular_actuators_launch.py @@ -0,0 +1,99 @@ +# 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 os +import pytest +import unittest + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_testing.actions import ReadyToTest + +# import launch_testing.markers +import rclpy +from rclpy.node import Node +from ros2_control_demo_testing.test_utils import ( + check_controllers_running, + check_if_js_published, + check_node_running, +) + + +# Executes the given launch file and checks if all nodes can be started +@pytest.mark.rostest +def generate_test_description(): + launch_include = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join( + get_package_share_directory("ros2_control_demo_example_6"), + "launch/rrbot_modular_actuators.launch.py", + ) + ), + launch_arguments={"gui": "false"}.items(), + ) + + return LaunchDescription([launch_include, 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, proc_output): + check_node_running(self.node, "robot_state_publisher") + + def test_controller_running(self, proc_output): + + cnames = ["forward_position_controller", "joint_state_broadcaster"] + + check_controllers_running(self.node, cnames) + + def test_check_if_msgs_published(self): + check_if_js_published("/joint_states", ["joint1", "joint2"]) + + +# TODO(anyone): enable this if shutdown of ros2_control_node does not fail anymore +# @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/example_7/CMakeLists.txt b/example_7/CMakeLists.txt index b3871b98a..7ce5fd05e 100644 --- a/example_7/CMakeLists.txt +++ b/example_7/CMakeLists.txt @@ -28,6 +28,7 @@ set(CONTROLLER_INCLUDE_DEPENDS ) # find dependencies +find_package(backward_ros REQUIRED) find_package(ament_cmake REQUIRED) foreach(Dependency IN ITEMS ${HW_IF_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) @@ -101,6 +102,7 @@ if(BUILD_TESTING) ament_add_pytest_test(example_7_urdf_xacro test/test_urdf_xacro.py) ament_add_pytest_test(view_example_7_launch test/test_view_robot_launch.py) + ament_add_pytest_test(run_example_7_launch test/test_r6bot_controller_launch.py) endif() ## EXPORTS diff --git a/example_7/bringup/launch/r6bot_controller.launch.py b/example_7/bringup/launch/r6bot_controller.launch.py index d0bf6f403..17dac5c26 100644 --- a/example_7/bringup/launch/r6bot_controller.launch.py +++ b/example_7/bringup/launch/r6bot_controller.launch.py @@ -13,15 +13,25 @@ # limitations under the License. from launch import LaunchDescription -from launch.actions import RegisterEventHandler +from launch.actions import RegisterEventHandler, DeclareLaunchArgument +from launch.conditions import IfCondition from launch.event_handlers import OnProcessExit -from launch.substitutions import Command, FindExecutable, PathJoinSubstitution +from launch.substitutions import Command, FindExecutable, PathJoinSubstitution, LaunchConfiguration from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare def generate_launch_description(): + # Declare arguments + declared_arguments = [] + declared_arguments.append( + DeclareLaunchArgument( + "gui", + default_value="true", + description="Start RViz2 automatically with this launch file.", + ) + ) # Get URDF via xacro robot_description_content = Command( [ @@ -68,12 +78,15 @@ def generate_launch_description(): output="both", parameters=[robot_description], ) + + gui = LaunchConfiguration("gui") rviz_node = Node( package="rviz2", executable="rviz2", name="rviz2", output="log", arguments=["-d", rviz_config_file], + condition=IfCondition(gui), ) joint_state_broadcaster_spawner = Node( @@ -96,20 +109,21 @@ def generate_launch_description(): ) ) - # Delay start of robot_controller after `joint_state_broadcaster` - delay_robot_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler( + # Delay start of joint_state_broadcaster after `robot_controller` + # TODO(anyone): This is a workaround for flaky tests. Remove when fixed. + delay_joint_state_broadcaster_after_robot_controller_spawner = RegisterEventHandler( event_handler=OnProcessExit( - target_action=joint_state_broadcaster_spawner, - on_exit=[robot_controller_spawner], + target_action=robot_controller_spawner, + on_exit=[joint_state_broadcaster_spawner], ) ) nodes = [ control_node, robot_state_pub_node, - joint_state_broadcaster_spawner, + robot_controller_spawner, delay_rviz_after_joint_state_broadcaster_spawner, - delay_robot_controller_spawner_after_joint_state_broadcaster_spawner, + delay_joint_state_broadcaster_after_robot_controller_spawner, ] - return LaunchDescription(nodes) + return LaunchDescription(declared_arguments + nodes) diff --git a/example_7/description/ros2_control/r6bot.ros2_control.xacro b/example_7/description/ros2_control/r6bot.ros2_control.xacro index 4c8d8eee9..0f6c04541 100644 --- a/example_7/description/ros2_control/r6bot.ros2_control.xacro +++ b/example_7/description/ros2_control/r6bot.ros2_control.xacro @@ -11,8 +11,8 @@ - {-2*pi} - {2*pi} + ${-2*pi} + ${2*pi} -3.15 @@ -26,8 +26,8 @@ - {-2*pi} - {2*pi} + ${-2*pi} + ${2*pi} -3.15 @@ -41,8 +41,8 @@ - {-pi} - {pi} + ${-pi} + ${pi} -3.15 @@ -56,8 +56,8 @@ - {-2*pi} - {2*pi} + ${-2*pi} + ${2*pi} -3.2 @@ -71,8 +71,8 @@ - {-2*pi} - {2*pi} + ${-2*pi} + ${2*pi} -3.2 @@ -86,8 +86,8 @@ - {-2*pi} - {2*pi} + ${-2*pi} + ${2*pi} -3.2 diff --git a/example_7/package.xml b/example_7/package.xml index 9bd69814b..ef7fb3b2c 100644 --- a/example_7/package.xml +++ b/example_7/package.xml @@ -15,6 +15,7 @@ ament_cmake + backward_ros control_msgs controller_interface hardware_interface @@ -39,11 +40,10 @@ urdf xacro - ament_cmake_gtest ament_cmake_pytest - launch_testing_ament_cmake launch_testing_ros liburdfdom-tools + ros2_control_demo_testing xacro diff --git a/example_7/test/test_r6bot_controller_launch.py b/example_7/test/test_r6bot_controller_launch.py new file mode 100644 index 000000000..42a731020 --- /dev/null +++ b/example_7/test/test_r6bot_controller_launch.py @@ -0,0 +1,101 @@ +# 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 os +import pytest +import unittest + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_testing.actions import ReadyToTest + +# import launch_testing.markers +import rclpy +from rclpy.node import Node +from ros2_control_demo_testing.test_utils import ( + check_controllers_running, + check_if_js_published, + check_node_running, +) + + +# Executes the given launch file and checks if all nodes can be started +@pytest.mark.rostest +def generate_test_description(): + launch_include = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join( + get_package_share_directory("ros2_control_demo_example_7"), + "launch/r6bot_controller.launch.py", + ) + ), + launch_arguments={"gui": "false"}.items(), + ) + + return LaunchDescription([launch_include, 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, proc_output): + check_node_running(self.node, "robot_state_publisher") + + def test_controller_running(self, proc_output): + + cnames = ["r6bot_controller", "joint_state_broadcaster"] + + check_controllers_running(self.node, cnames) + + def test_check_if_msgs_published(self): + check_if_js_published( + "/joint_states", ["joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6"] + ) + + +# TODO(anyone): enable this if shutdown of ros2_control_node does not fail anymore +# @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/example_8/CMakeLists.txt b/example_8/CMakeLists.txt index 4f19d5492..1edeb7888 100644 --- a/example_8/CMakeLists.txt +++ b/example_8/CMakeLists.txt @@ -18,6 +18,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS ) # find dependencies +find_package(backward_ros REQUIRED) find_package(ament_cmake REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) @@ -67,6 +68,7 @@ if(BUILD_TESTING) ament_add_pytest_test(example_8_urdf_xacro test/test_urdf_xacro.py) ament_add_pytest_test(view_example_8_launch test/test_view_robot_launch.py) + ament_add_pytest_test(run_example_8_launch test/test_rrbot_transmissions_system_position_only_launch.py) endif() ## EXPORTS diff --git a/example_8/bringup/launch/rrbot_transmissions_system_position_only.launch.py b/example_8/bringup/launch/rrbot_transmissions_system_position_only.launch.py index 664531655..268c5884e 100644 --- a/example_8/bringup/launch/rrbot_transmissions_system_position_only.launch.py +++ b/example_8/bringup/launch/rrbot_transmissions_system_position_only.launch.py @@ -137,20 +137,21 @@ def generate_launch_description(): ) ) - # Delay start of robot_controller after `joint_state_broadcaster` - delay_robot_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler( + # Delay start of joint_state_broadcaster after `robot_controller` + # TODO(anyone): This is a workaround for flaky tests. Remove when fixed. + delay_joint_state_broadcaster_after_robot_controller_spawner = RegisterEventHandler( event_handler=OnProcessExit( - target_action=joint_state_broadcaster_spawner, - on_exit=[robot_controller_spawner], + target_action=robot_controller_spawner, + on_exit=[joint_state_broadcaster_spawner], ) ) nodes = [ control_node, robot_state_pub_node, - joint_state_broadcaster_spawner, + robot_controller_spawner, delay_rviz_after_joint_state_broadcaster_spawner, - delay_robot_controller_spawner_after_joint_state_broadcaster_spawner, + delay_joint_state_broadcaster_after_robot_controller_spawner, ] return LaunchDescription(declared_arguments + nodes) diff --git a/example_8/package.xml b/example_8/package.xml index 275fc4aaa..1e6c6ac06 100644 --- a/example_8/package.xml +++ b/example_8/package.xml @@ -15,6 +15,7 @@ ament_cmake + backward_ros hardware_interface pluginlib rclcpp @@ -33,11 +34,10 @@ rviz2 xacro - ament_cmake_gtest ament_cmake_pytest - launch_testing_ament_cmake launch_testing_ros liburdfdom-tools + ros2_control_demo_testing xacro diff --git a/example_8/test/test_rrbot_transmissions_system_position_only_launch.py b/example_8/test/test_rrbot_transmissions_system_position_only_launch.py new file mode 100644 index 000000000..96ba7d3c9 --- /dev/null +++ b/example_8/test/test_rrbot_transmissions_system_position_only_launch.py @@ -0,0 +1,99 @@ +# 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 os +import pytest +import unittest + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_testing.actions import ReadyToTest + +# import launch_testing.markers +import rclpy +from rclpy.node import Node +from ros2_control_demo_testing.test_utils import ( + check_controllers_running, + check_if_js_published, + check_node_running, +) + + +# Executes the given launch file and checks if all nodes can be started +@pytest.mark.rostest +def generate_test_description(): + launch_include = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join( + get_package_share_directory("ros2_control_demo_example_8"), + "launch/rrbot_transmissions_system_position_only.launch.py", + ) + ), + launch_arguments={"gui": "false"}.items(), + ) + + return LaunchDescription([launch_include, 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, proc_output): + check_node_running(self.node, "robot_state_publisher") + + def test_controller_running(self, proc_output): + + cnames = ["forward_position_controller", "joint_state_broadcaster"] + + check_controllers_running(self.node, cnames) + + def test_check_if_msgs_published(self): + check_if_js_published("/joint_states", ["joint1", "joint2"]) + + +# TODO(anyone): enable this if shutdown of ros2_control_node does not fail anymore +# @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/example_9/CMakeLists.txt b/example_9/CMakeLists.txt index 5f84de236..61f5b2478 100644 --- a/example_9/CMakeLists.txt +++ b/example_9/CMakeLists.txt @@ -17,6 +17,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS ) # find dependencies +find_package(backward_ros REQUIRED) find_package(ament_cmake REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) @@ -66,6 +67,7 @@ if(BUILD_TESTING) ament_add_pytest_test(example_9_urdf_xacro test/test_urdf_xacro.py) ament_add_pytest_test(view_example_9_launch test/test_view_robot_launch.py) + ament_add_pytest_test(run_example_9_launch test/test_rrbot_launch.py) endif() ## EXPORTS diff --git a/example_9/bringup/launch/rrbot.launch.py b/example_9/bringup/launch/rrbot.launch.py index f180c990e..c74566536 100644 --- a/example_9/bringup/launch/rrbot.launch.py +++ b/example_9/bringup/launch/rrbot.launch.py @@ -109,20 +109,21 @@ def generate_launch_description(): ) ) - # Delay start of robot_controller after `joint_state_broadcaster` - delay_robot_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler( + # Delay start of joint_state_broadcaster after `robot_controller` + # TODO(anyone): This is a workaround for flaky tests. Remove when fixed. + delay_joint_state_broadcaster_after_robot_controller_spawner = RegisterEventHandler( event_handler=OnProcessExit( - target_action=joint_state_broadcaster_spawner, - on_exit=[robot_controller_spawner], + target_action=robot_controller_spawner, + on_exit=[joint_state_broadcaster_spawner], ) ) nodes = [ control_node, robot_state_pub_node, - joint_state_broadcaster_spawner, + robot_controller_spawner, delay_rviz_after_joint_state_broadcaster_spawner, - delay_robot_controller_spawner_after_joint_state_broadcaster_spawner, + delay_joint_state_broadcaster_after_robot_controller_spawner, ] return LaunchDescription(declared_arguments + nodes) diff --git a/example_9/package.xml b/example_9/package.xml index 580b31746..53dbc8f13 100644 --- a/example_9/package.xml +++ b/example_9/package.xml @@ -16,6 +16,7 @@ ament_cmake + backward_ros hardware_interface pluginlib rclcpp @@ -35,11 +36,10 @@ rviz2 xacro - ament_cmake_gtest ament_cmake_pytest - launch_testing_ament_cmake launch_testing_ros liburdfdom-tools + ros2_control_demo_testing xacro diff --git a/example_9/test/test_rrbot_launch.py b/example_9/test/test_rrbot_launch.py new file mode 100644 index 000000000..fb2211e1e --- /dev/null +++ b/example_9/test/test_rrbot_launch.py @@ -0,0 +1,99 @@ +# 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 os +import pytest +import unittest + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_testing.actions import ReadyToTest + +# import launch_testing.markers +import rclpy +from rclpy.node import Node +from ros2_control_demo_testing.test_utils import ( + check_controllers_running, + check_if_js_published, + check_node_running, +) + + +# Executes the given launch file and checks if all nodes can be started +@pytest.mark.rostest +def generate_test_description(): + launch_include = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join( + get_package_share_directory("ros2_control_demo_example_9"), + "launch/rrbot.launch.py", + ) + ), + launch_arguments={"gui": "false"}.items(), + ) + + return LaunchDescription([launch_include, 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, proc_output): + check_node_running(self.node, "robot_state_publisher") + + def test_controller_running(self, proc_output): + + cnames = ["forward_position_controller", "joint_state_broadcaster"] + + check_controllers_running(self.node, cnames) + + def test_check_if_msgs_published(self): + check_if_js_published("/joint_states", ["joint1", "joint2"]) + + +# TODO(anyone): enable this if shutdown of ros2_control_node does not fail anymore +# @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_control_demo_testing/package.xml b/ros2_control_demo_testing/package.xml new file mode 100644 index 000000000..bc16dcf4a --- /dev/null +++ b/ros2_control_demo_testing/package.xml @@ -0,0 +1,17 @@ + + + + ros2_control_demo_testing + 0.0.0 + Utilities for launch testing + Christoph Froehlich + Apache-2.0 + + sensor_msgs + launch_testing_ros + controller_manager + + + ament_python + + diff --git a/ros2_control_demo_testing/resource/ros2_control_demo_testing b/ros2_control_demo_testing/resource/ros2_control_demo_testing new file mode 100644 index 000000000..e69de29bb diff --git a/ros2_control_demo_testing/ros2_control_demo_testing/__init__.py b/ros2_control_demo_testing/ros2_control_demo_testing/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/ros2_control_demo_testing/ros2_control_demo_testing/test_utils.py b/ros2_control_demo_testing/ros2_control_demo_testing/test_utils.py new file mode 100644 index 000000000..5aeab91dd --- /dev/null +++ b/ros2_control_demo_testing/ros2_control_demo_testing/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=""): + + # 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 == "active": + found[cname] = True + break + time.sleep(0.1) + + assert all( + found.values() + ), f"Controller(s) not found or not active: {', '.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() diff --git a/ros2_control_demo_testing/setup.cfg b/ros2_control_demo_testing/setup.cfg new file mode 100644 index 000000000..5d0f85421 --- /dev/null +++ b/ros2_control_demo_testing/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/ros2_control_demo_testing +[install] +install_scripts=$base/lib/ros2_control_demo_testing diff --git a/ros2_control_demo_testing/setup.py b/ros2_control_demo_testing/setup.py new file mode 100644 index 000000000..62f25ff4e --- /dev/null +++ b/ros2_control_demo_testing/setup.py @@ -0,0 +1,53 @@ +# 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 + +from setuptools import find_packages, setup + +package_name = "ros2_control_demo_testing" + +setup( + name=package_name, + version="0.0.0", + packages=find_packages(exclude=["test"]), + data_files=[ + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + ("share/" + package_name, ["package.xml"]), + ], + install_requires=["setuptools"], + zip_safe=True, + maintainer="Christoph Froehlich", + maintainer_email="christoph.froehlich@ait.ac.at", + description="Utilities for launch testing", + license="Apache-2.0", + tests_require=["pytest"], + entry_points={ + "console_scripts": [], + }, +)