From 0753b76f42f08630e1a2ba35e3f7e96a631e70a4 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 25 Jan 2024 18:49:05 +0000 Subject: [PATCH 1/7] Add Moveit startup test This is a basic test that verifies that all Nodes come up correctly without crashing. --- ur_moveit_config/CMakeLists.txt | 12 ++ ur_moveit_config/package.xml | 3 + ur_moveit_config/test/startup_test.py | 158 ++++++++++++++++++++++++++ 3 files changed, 173 insertions(+) create mode 100644 ur_moveit_config/test/startup_test.py diff --git a/ur_moveit_config/CMakeLists.txt b/ur_moveit_config/CMakeLists.txt index e47ea7642..164777280 100644 --- a/ur_moveit_config/CMakeLists.txt +++ b/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() diff --git a/ur_moveit_config/package.xml b/ur_moveit_config/package.xml index 2b6a026fa..457dc27b8 100644 --- a/ur_moveit_config/package.xml +++ b/ur_moveit_config/package.xml @@ -32,6 +32,9 @@ warehouse_ros_sqlite xacro + launch_testing_ament_cmake + ur_robot_driver + ament_cmake diff --git a/ur_moveit_config/test/startup_test.py b/ur_moveit_config/test/startup_test.py new file mode 100644 index 000000000..44fc66a69 --- /dev/null +++ b/ur_moveit_config/test/startup_test.py @@ -0,0 +1,158 @@ +#!/usr/bin/env python +# Copyright 2024, FZI Forschungszentrum Informatik +# +# 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. + +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) From 3dc5dba83b93cb704036e7add410ed457540c673 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 25 Jan 2024 19:56:28 +0000 Subject: [PATCH 2/7] Attempt to fix moveit servo_node This makes it start but I couldn't get the robot moving. --- ur_moveit_config/config/ur_servo.yaml | 2 +- ur_moveit_config/launch/ur_moveit.launch.py | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/ur_moveit_config/config/ur_servo.yaml b/ur_moveit_config/config/ur_servo.yaml index 767481a8a..ef2c79d3d 100644 --- a/ur_moveit_config/config/ur_servo.yaml +++ b/ur_moveit_config/config/ur_servo.yaml @@ -52,7 +52,7 @@ num_outgoing_halt_msgs_to_publish: 4 ## Configure handling of singularities and joint limits lower_singularity_threshold: 100.0 # Start decelerating when the condition number hits this (close to singularity) hard_stop_singularity_threshold: 200.0 # Stop when the condition number hits this -joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving quickly, make this larger. +joint_limit_margins: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1] # added as a buffer to joint limits [radians]. If moving quickly, make this larger. ## Topic names cartesian_command_in_topic: ~/delta_twist_cmds # Topic for incoming Cartesian twist commands diff --git a/ur_moveit_config/launch/ur_moveit.launch.py b/ur_moveit_config/launch/ur_moveit.launch.py index a73bd73be..a261200ee 100644 --- a/ur_moveit_config/launch/ur_moveit.launch.py +++ b/ur_moveit_config/launch/ur_moveit.launch.py @@ -256,6 +256,7 @@ def launch_setup(context, *args, **kwargs): servo_params, robot_description, robot_description_semantic, + robot_description_kinematics, ], output="screen", ) From 8c5f103457d17e0e8dde5e3f99d97b0aa758571c Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 25 Jan 2024 20:01:23 +0000 Subject: [PATCH 3/7] Disable servo_node in startup_test The servo_node currently doesn't startup correctly until a plan is executed. --- ur_moveit_config/test/startup_test.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/ur_moveit_config/test/startup_test.py b/ur_moveit_config/test/startup_test.py index 44fc66a69..b75216db6 100644 --- a/ur_moveit_config/test/startup_test.py +++ b/ur_moveit_config/test/startup_test.py @@ -115,6 +115,10 @@ def generate_test_description(): launch_arguments={ "ur_type": ur_type, "launch_rviz": "false", + "launch_servo": "false", # the servo node currently doesn't startup correctly, + # it waits for the robot state until the robot has actually + # executed a plan. Therefore, it doesn't exit cleanly which + # breaks this test. }.items(), ) From df1f977e009a1a96976731d28288603f5ad66783 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 25 Jan 2024 20:09:48 +0000 Subject: [PATCH 4/7] Add MoveIt to semi-binary workspace Also enabled CCache, since building MoveIt takes a very long time otherwise. --- .github/workflows/reusable_ici.yml | 36 ++++++++++++++++++++++ Universal_Robots_ROS2_Driver.humble.repos | 9 ++++++ Universal_Robots_ROS2_Driver.iron.repos | 9 ++++++ Universal_Robots_ROS2_Driver.rolling.repos | 9 ++++++ 4 files changed, 63 insertions(+) diff --git a/.github/workflows/reusable_ici.yml b/.github/workflows/reusable_ici.yml index b47569fcd..4e1900d3d 100644 --- a/.github/workflows/reusable_ici.yml +++ b/.github/workflows/reusable_ici.yml @@ -28,6 +28,16 @@ on: default: '' required: false type: string + ccache_dir: + description: 'Local path to store cache (from "github.workspace"). For standard industrial_ci configuration do not have to be changed' + default: '.ccache' + required: false + type: string + basedir: + description: 'Local path to workspace base directory to cache (from "github.workspace"). For standard industrial_ci configuration do not have to be changed' + default: '.work' + required: false + type: string jobs: reusable_ici: @@ -35,6 +45,9 @@ jobs: runs-on: ubuntu-latest env: DOCKER_RUN_OPTS: '-v /var/run/docker.sock:/var/run/docker.sock --network ursim_net' + CCACHE_DIR: ${{ github.workspace }}/${{ inputs.ccache_dir }} + BASEDIR: ${{ github.workspace }}/${{ inputs.basedir }} + CACHE_PREFIX: ${{ inputs.ros_distro }}-${{ inputs.upstream_workspace }}-${{ inputs.ros_repo }}-${{ github.job }} steps: - name: Checkout ${{ inputs.ref }} when build is not scheduled if: ${{ github.event_name != 'schedule' }} @@ -45,9 +58,32 @@ jobs: with: ref: ${{ inputs.ref_for_scheduled_build }} - run: docker network create --subnet=192.168.56.0/24 ursim_net + - name: cache target_ws + if: ${{ ! matrix.env.CCOV }} + uses: pat-s/always-upload-cache@v3.0.11 + with: + path: ${{ env.BASEDIR }}/target_ws + key: target_ws-${{ env.CACHE_PREFIX }}-${{ hashFiles('**/CMakeLists.txt', '**/package.xml') }}-${{ github.run_id }} + restore-keys: | + target_ws-${{ env.CACHE_PREFIX }}-${{ hashFiles('**/CMakeLists.txt', '**/package.xml') }} + - name: cache ccache + uses: pat-s/always-upload-cache@v3.0.11 + with: + path: ${{ env.CCACHE_DIR }} + key: ccache-${{ env.CACHE_PREFIX }}-${{ github.sha }}-${{ github.run_id }} + restore-keys: | + ccache-${{ env.CACHE_PREFIX }}-${{ github.sha }} + ccache-${{ env.CACHE_PREFIX }} - uses: 'ros-industrial/industrial_ci@master' env: UPSTREAM_WORKSPACE: ${{ inputs.upstream_workspace }} ROS_DISTRO: ${{ inputs.ros_distro }} ROS_REPO: ${{ inputs.ros_repo }} CMAKE_ARGS: -DUR_ROBOT_DRIVER_BUILD_INTEGRATION_TESTS=ON + - name: prepare target_ws for cache + if: ${{ always() && ! matrix.env.CCOV }} + run: | + du -sh ${{ env.BASEDIR }}/target_ws + sudo find ${{ env.BASEDIR }}/target_ws -wholename '*/test_results/*' -delete + sudo rm -rf ${{ env.BASEDIR }}/target_ws/src + du -sh ${{ env.BASEDIR }}/target_ws diff --git a/Universal_Robots_ROS2_Driver.humble.repos b/Universal_Robots_ROS2_Driver.humble.repos index 61ee8ef3a..456c5f25b 100644 --- a/Universal_Robots_ROS2_Driver.humble.repos +++ b/Universal_Robots_ROS2_Driver.humble.repos @@ -27,3 +27,12 @@ repositories: type: git url: https://github.com/ros-controls/control_msgs.git version: humble + moveit2: + type: git + url: https://github.com/ros-planning/moveit2.git + version: humble + + moveit_msgs: + type: git + url: https://github.com/ros-planning/moveit_msgs.git + version: humble diff --git a/Universal_Robots_ROS2_Driver.iron.repos b/Universal_Robots_ROS2_Driver.iron.repos index f59c5d0e5..3270a7d02 100644 --- a/Universal_Robots_ROS2_Driver.iron.repos +++ b/Universal_Robots_ROS2_Driver.iron.repos @@ -27,3 +27,12 @@ repositories: type: git url: https://github.com/ros-controls/control_msgs.git version: master + moveit2: + type: git + url: https://github.com/ros-planning/moveit2.git + version: iron + + moveit_msgs: + type: git + url: https://github.com/ros-planning/moveit_msgs.git + version: iron diff --git a/Universal_Robots_ROS2_Driver.rolling.repos b/Universal_Robots_ROS2_Driver.rolling.repos index f7dd6bee1..371895282 100644 --- a/Universal_Robots_ROS2_Driver.rolling.repos +++ b/Universal_Robots_ROS2_Driver.rolling.repos @@ -27,3 +27,12 @@ repositories: type: git url: https://github.com/ros-controls/control_msgs.git version: master + moveit2: + type: git + url: https://github.com/ros-planning/moveit2.git + version: main + + moveit_msgs: + type: git + url: https://github.com/ros-planning/moveit_msgs.git + version: ros2 From efd7052fcee5ca6be0a2afb20916002feeb40b8e Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 25 Jan 2024 20:13:41 +0000 Subject: [PATCH 5/7] Add ur_moveit_config to list of integration-testing packages in ci_status docs --- ci_status.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ci_status.md b/ci_status.md index 376cad650..a364b0b55 100644 --- a/ci_status.md +++ b/ci_status.md @@ -115,5 +115,5 @@ red pipeline there should be a corresponding issue labeled with [ci-failure](htt Each of these stages also performs integration tests using ursim. In order to execute these tests locally, they have to be enabled: ``` - colcon build --packages-select ur_robot_driver --cmake-args -DUR_ROBOT_DRIVER_BUILD_INTEGRATION_TESTS=On + colcon build --packages-select ur_robot_driver ur_moveit_config --cmake-args -DUR_ROBOT_DRIVER_BUILD_INTEGRATION_TESTS=On ``` From f04293be494ca277f516a8565f29026cb3866743 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 25 Jan 2024 22:02:55 +0000 Subject: [PATCH 6/7] Also wait for dashboard server --- ur_moveit_config/test/startup_test.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/ur_moveit_config/test/startup_test.py b/ur_moveit_config/test/startup_test.py index b75216db6..51f754e43 100644 --- a/ur_moveit_config/test/startup_test.py +++ b/ur_moveit_config/test/startup_test.py @@ -154,6 +154,9 @@ 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") + proc_output.assertWaitFor( + "Dashboard server connections are possible.", timeout=120, stream="stdout" + ) @launch_testing.post_shutdown_test() From cb219a0e63617826a49bff78bf22044728023ad3 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Fri, 26 Jan 2024 13:01:45 +0000 Subject: [PATCH 7/7] Add dummy MoveIt test in order to keep it running long enough --- ur_moveit_config/test/startup_test.py | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/ur_moveit_config/test/startup_test.py b/ur_moveit_config/test/startup_test.py index 51f754e43..35effc499 100644 --- a/ur_moveit_config/test/startup_test.py +++ b/ur_moveit_config/test/startup_test.py @@ -44,6 +44,10 @@ from launch_ros.substitutions import FindPackagePrefix, FindPackageShare from launch.event_handlers import OnProcessExit +import rclpy +import rclpy.node +from moveit_msgs.srv import GetPlanningScene + 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. @@ -158,6 +162,15 @@ def test_read_stdout(self, proc_output): "Dashboard server connections are possible.", timeout=120, stream="stdout" ) + def testCallingMoveItService(self): + # Dummy test to make sure MoveIt lives long enough... + rclpy.init() + node = rclpy.node.Node("ur_moveit_test") + + cli = node.create_client(GetPlanningScene, "/get_planning_scene") + while not cli.wait_for_service(timeout_sec=120.0): + node.get_logger().info("service not available, waiting again...") + @launch_testing.post_shutdown_test() class TestProcessExitCode(unittest.TestCase):