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": [],
+ },
+)