Add Moveit startup test

This is a basic test that verifies that all Nodes come up correctly without crashing.

Author: Felix Exner

ur_moveit_config/CMakeLists.txt

@@ -13,3 +13,15 @@ ament_python_install_package(${PROJECT_NAME})
 ament_python_install_module(${PROJECT_NAME}/launch_common.py)
 
 ament_package()
+
+if(BUILD_TESTING)
+  find_package(ur_robot_driver REQUIRED)
+  find_package(launch_testing_ament_cmake)
+
+  if(${UR_ROBOT_DRIVER_BUILD_INTEGRATION_TESTS})
+    add_launch_test(test/startup_test.py
+      TIMEOUT
+        180
+    )
+  endif()
+endif()

ur_moveit_config/package.xml

@@ -32,6 +32,9 @@
   <exec_depend>warehouse_ros_sqlite</exec_depend>
   <exec_depend>xacro</exec_depend>
 
+  <test_depend>launch_testing_ament_cmake</test_depend>
+  <test_depend>ur_robot_driver</test_depend>
+
   <export>
     <build_type>ament_cmake</build_type>
   </export>

ur_moveit_config/test/startup_test.py

@@ -0,0 +1,129 @@
+import pytest
+import unittest
+
+from launch import LaunchDescription
+import launch_testing
+from launch_testing.actions import ReadyToTest
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch.actions import (
+    DeclareLaunchArgument,
+    ExecuteProcess,
+    IncludeLaunchDescription,
+    RegisterEventHandler,
+)
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.substitutions import FindPackagePrefix, FindPackageShare
+from launch.event_handlers import OnProcessExit
+
+TIMEOUT_WAIT_SERVICE_INITIAL = 120  # If we download the docker image simultaneously to the tests, it can take quite some time until the dashboard server is reachable and usable.
+
+
+@pytest.mark.launch_test
+def generate_test_description():
+    controller_spawner_timeout = TIMEOUT_WAIT_SERVICE_INITIAL
+    declared_arguments = []
+
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "ur_type",
+            default_value="ur5e",
+            description="Type/series of used UR robot.",
+            choices=[
+                "ur3",
+                "ur3e",
+                "ur5",
+                "ur5e",
+                "ur10",
+                "ur10e",
+                "ur16e",
+                "ur20",
+                "ur30",
+            ],
+        )
+    )
+
+    ur_type = LaunchConfiguration("ur_type")
+
+    robot_driver = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            PathJoinSubstitution(
+                [FindPackageShare("ur_robot_driver"), "launch", "ur_control.launch.py"]
+            )
+        ),
+        launch_arguments={
+            "robot_ip": "192.168.56.101",
+            "ur_type": ur_type,
+            "launch_rviz": "false",
+            "controller_spawner_timeout": str(controller_spawner_timeout),
+            "initial_joint_controller": "scaled_joint_trajectory_controller",
+            "headless_mode": "true",
+            "launch_dashboard_client": "false",
+            "start_joint_controller": "false",
+        }.items(),
+    )
+    wait_dashboard_server = ExecuteProcess(
+        cmd=[
+            PathJoinSubstitution(
+                [
+                    FindPackagePrefix("ur_robot_driver"),
+                    "bin",
+                    "wait_dashboard_server.sh",
+                ]
+            )
+        ],
+        name="wait_dashboard_server",
+        output="screen",
+    )
+    driver_starter = RegisterEventHandler(
+        OnProcessExit(target_action=wait_dashboard_server, on_exit=robot_driver)
+    )
+    moveit_setup = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            PathJoinSubstitution(
+                [FindPackageShare("ur_moveit_config"), "launch", "ur_moveit.launch.py"]
+            )
+        ),
+        launch_arguments={
+            "ur_type": ur_type,
+            "launch_rviz": "false",
+        }.items(),
+    )
+
+    return LaunchDescription(
+        declared_arguments
+        + [ReadyToTest(), wait_dashboard_server, _ursim_action(), driver_starter, moveit_setup]
+    )
+
+
+def _ursim_action():
+    ur_type = LaunchConfiguration("ur_type")
+
+    return ExecuteProcess(
+        cmd=[
+            PathJoinSubstitution(
+                [
+                    FindPackagePrefix("ur_client_library"),
+                    "lib",
+                    "ur_client_library",
+                    "start_ursim.sh",
+                ]
+            ),
+            " ",
+            "-m ",
+            ur_type,
+        ],
+        name="start_ursim",
+        output="screen",
+    )
+
+
+class TestReadyForPlanning(unittest.TestCase):
+    def test_read_stdout(self, proc_output):
+        """Check if 'You can start planning now!' was found in the stdout."""
+        proc_output.assertWaitFor("You can start planning now!", timeout=120, stream="stdout")
+
+
+@launch_testing.post_shutdown_test()
+class TestProcessExitCode(unittest.TestCase):
+    def test_exit_code(self, proc_info):
+        launch_testing.asserts.assertExitCodes(proc_info)