diff --git a/.github/workflows/kilted-binary-main.yml b/.github/workflows/kilted-binary-main.yml new file mode 100644 index 0000000..a54866d --- /dev/null +++ b/.github/workflows/kilted-binary-main.yml @@ -0,0 +1,21 @@ +name: Kilted Binary Main +on: + workflow_dispatch: + branches: + - main + pull_request: + branches: + - main + push: + branches: + - main + schedule: + - cron: '34 12 * * 0' + +jobs: + kilted_binary_main: + uses: ./.github/workflows/reusable_ici.yml + with: + ros_distro: kilted + ros_repo: main + ref_for_scheduled_build: main diff --git a/.github/workflows/kilted-binary-testing.yml b/.github/workflows/kilted-binary-testing.yml new file mode 100644 index 0000000..8a524c8 --- /dev/null +++ b/.github/workflows/kilted-binary-testing.yml @@ -0,0 +1,21 @@ +name: Kilted Binary Testing +on: + workflow_dispatch: + branches: + - main + pull_request: + branches: + - main + push: + branches: + - main + schedule: + - cron: '34 12 * * 0' + +jobs: + kilted_binary_testing: + uses: ./.github/workflows/reusable_ici.yml + with: + ros_distro: kilted + ros_repo: testing + ref_for_scheduled_build: main diff --git a/.github/workflows/reusable_ici.yml b/.github/workflows/reusable_ici.yml index f056f41..49f9c92 100644 --- a/.github/workflows/reusable_ici.yml +++ b/.github/workflows/reusable_ici.yml @@ -76,6 +76,8 @@ jobs: UPSTREAM_WORKSPACE: ${{ inputs.upstream_workspace }} ROS_DISTRO: ${{ inputs.ros_distro }} ROS_REPO: ${{ inputs.ros_repo }} + CMAKE_ARGS: -DMY_ROBOT_CELL_CONTROL_BUILD_INTEGRATION_TESTS=ON + ADDITIONAL_DEBS: docker.io netcat-openbsd curl # Needed for integration tests - name: prepare target_ws for cache if: ${{ always() && ! matrix.env.CCOV }} run: | diff --git a/.github/workflows/rolling-binary-main.yml b/.github/workflows/rolling-binary-main.yml index 76f54e4..ae0e8c9 100644 --- a/.github/workflows/rolling-binary-main.yml +++ b/.github/workflows/rolling-binary-main.yml @@ -10,7 +10,7 @@ on: branches: - main schedule: - - cron: '34 12 * * *' + - cron: '34 12 * * 0' jobs: rolling_binary_main: diff --git a/.github/workflows/rolling-binary-testing.yml b/.github/workflows/rolling-binary-testing.yml index 50c787d..32b7aba 100644 --- a/.github/workflows/rolling-binary-testing.yml +++ b/.github/workflows/rolling-binary-testing.yml @@ -10,7 +10,7 @@ on: branches: - main schedule: - - cron: '34 12 * * *' + - cron: '34 12 * * 0' jobs: rolling_binary_testing: diff --git a/README.md b/README.md index b92ed51..255d6eb 100644 --- a/README.md +++ b/README.md @@ -1,10 +1,22 @@ # Universal Robots ROS 2 tutorials This package contains tutorials around the ROS 2 packages for Universal Robots. +## Branches & ROS distributions +This package intends to show use-cases and examples for the ROS 2 packages for Universal Robots. +The main branch of this repository will be used to show examples for the latest ROS 2 distribution, +ROS Rolling. For older ROS 2 distributions. This is valid for all active ROS 2 distributions, as +long as there is no specialized branch for that distribution. The following table shows the +branches and their corresponding ROS 2 distributions: + +| ROS 2 Distro | Branch | Documentation | +|--------------|------------------------------------------------------------------------------------------|---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------| +| **Rolling** | [main](https://github.com/UniversalRobots/Universal_Robots_ROS2_Tutorials/tree/main) | [![Documentation](https://img.shields.io/badge/docs-latest-blue)](https://docs.universal-robots.com/Universal_Robots_ROS_Documentation/rolling/doc/ur_tutorials/tutorial_index.html) | +| **Jazzy** | [jazzy](https://github.com/UniversalRobots/Universal_Robots_ROS2_Tutorials/tree/jazzy) | [![Documentation](https://img.shields.io/badge/docs-jazzy-blue)](https://docs.universal-robots.com/Universal_Robots_ROS_Documentation/jazzy/doc/ur_tutorials/tutorial_index.html) | +| **Humble** | [humble](https://github.com/UniversalRobots/Universal_Robots_ROS2_Tutorials/tree/humble) | -- | + ## Getting started To use the tutorials from this repository, please make sure to [install ROS -2](https://docs.ros.org/en/rolling/Installation.html) on your system. Currently, only ROS Jazzy and -Rolling are supported. +2](https://docs.ros.org/en/rolling/Installation.html) on your system. With that, please create a workspace, clone this repo into the workspace, install the dependencies and build the workspace. diff --git a/my_dual_robot_cell/my_dual_robot_cell_control/CMakeLists.txt b/my_dual_robot_cell/my_dual_robot_cell_control/CMakeLists.txt index 9641ab9..dc7b2c9 100644 --- a/my_dual_robot_cell/my_dual_robot_cell_control/CMakeLists.txt +++ b/my_dual_robot_cell/my_dual_robot_cell_control/CMakeLists.txt @@ -8,4 +8,12 @@ install( DESTINATION share/${PROJECT_NAME} ) +if(BUILD_TESTING) + find_package(launch_testing_ament_cmake REQUIRED) + add_launch_test(test/example_move.py + TIMEOUT + 300 + ) +endif() + ament_package() diff --git a/my_dual_robot_cell/my_dual_robot_cell_control/config/combined_controllers.yaml b/my_dual_robot_cell/my_dual_robot_cell_control/config/combined_controllers.yaml index 2468dfb..6034c08 100644 --- a/my_dual_robot_cell/my_dual_robot_cell_control/config/combined_controllers.yaml +++ b/my_dual_robot_cell/my_dual_robot_cell_control/config/combined_controllers.yaml @@ -24,23 +24,20 @@ controller_manager: alice_joint_trajectory_controller: type: joint_trajectory_controller/JointTrajectoryController - bob_trajectory_controller: + bob_joint_trajectory_controller: type: joint_trajectory_controller/JointTrajectoryController - alice_scaled_joint_trajectory_controller: - type: ur_controllers/ScaledJointTrajectoryController + alice_forward_velocity_controller: + type: forward_command_controller/ForwardCommandController - bob_scaled_joint_trajectory_controller: - type: ur_controllers/ScaledJointTrajectoryController - - forward_velocity_controller: - type: velocity_controllers/JointGroupVelocityController + bob_forward_velocity_controller: + type: forward_command_controller/ForwardCommandController alice_forward_position_controller: - type: position_controllers/JointGroupPositionController + type: forward_command_controller/ForwardCommandController bob_forward_position_controller: - type: position_controllers/JointGroupPositionController + type: forward_command_controller/ForwardCommandController alice_speed_scaling_state_broadcaster: @@ -114,6 +111,8 @@ alice_joint_trajectory_controller: alice_wrist_1_joint: { trajectory: 0.2, goal: 0.1 } alice_wrist_2_joint: { trajectory: 0.2, goal: 0.1 } alice_wrist_3_joint: { trajectory: 0.2, goal: 0.1 } + speed_scaling: + state_interface: alice_speed_scaling/speed_scaling_factor bob_joint_trajectory_controller: ros__parameters: @@ -141,62 +140,8 @@ bob_joint_trajectory_controller: bob_wrist_1_joint: { trajectory: 0.2, goal: 0.1 } bob_wrist_2_joint: { trajectory: 0.2, goal: 0.1 } bob_wrist_3_joint: { trajectory: 0.2, goal: 0.1 } - -alice_scaled_joint_trajectory_controller: - ros__parameters: - joints: - - alice_shoulder_pan_joint - - alice_shoulder_lift_joint - - alice_elbow_joint - - alice_wrist_1_joint - - alice_wrist_2_joint - - alice_wrist_3_joint - command_interfaces: - - position - state_interfaces: - - position - - velocity - state_publish_rate: 100.0 - action_monitor_rate: 20.0 - allow_partial_joints_goal: false - constraints: - stopped_velocity_tolerance: 0.2 - goal_time: 0.0 - alice_shoulder_pan_joint: { trajectory: 0.2, goal: 0.1 } - alice_shoulder_lift_joint: { trajectory: 0.2, goal: 0.1 } - alice_elbow_joint: { trajectory: 0.2, goal: 0.1 } - alice_wrist_1_joint: { trajectory: 0.2, goal: 0.1 } - alice_wrist_2_joint: { trajectory: 0.2, goal: 0.1 } - alice_wrist_3_joint: { trajectory: 0.2, goal: 0.1 } - speed_scaling_interface_name: alice_speed_scaling/speed_scaling_factor - -bob_scaled_joint_trajectory_controller: - ros__parameters: - joints: - - bob_shoulder_pan_joint - - bob_shoulder_lift_joint - - bob_elbow_joint - - bob_wrist_1_joint - - bob_wrist_2_joint - - bob_wrist_3_joint - command_interfaces: - - position - state_interfaces: - - position - - velocity - state_publish_rate: 100.0 - action_monitor_rate: 20.0 - allow_partial_joints_goal: false - constraints: - stopped_velocity_tolerance: 0.2 - goal_time: 0.0 - bob_shoulder_pan_joint: { trajectory: 0.2, goal: 0.1 } - bob_shoulder_lift_joint: { trajectory: 0.2, goal: 0.1 } - bob_elbow_joint: { trajectory: 0.2, goal: 0.1 } - bob_wrist_1_joint: { trajectory: 0.2, goal: 0.1 } - bob_wrist_2_joint: { trajectory: 0.2, goal: 0.1 } - bob_wrist_3_joint: { trajectory: 0.2, goal: 0.1 } - speed_scaling_interface_name: bob_speed_scaling/speed_scaling_factor + speed_scaling: + state_interface: bob_speed_scaling/speed_scaling_factor alice_forward_velocity_controller: ros__parameters: @@ -229,6 +174,7 @@ alice_forward_position_controller: - alice_wrist_1_joint - alice_wrist_2_joint - alice_wrist_3_joint + interface_name: position bob_forward_position_controller: ros__parameters: @@ -239,3 +185,4 @@ bob_forward_position_controller: - bob_wrist_1_joint - bob_wrist_2_joint - bob_wrist_3_joint + interface_name: position diff --git a/my_dual_robot_cell/my_dual_robot_cell_control/launch/start_robots.launch.py b/my_dual_robot_cell/my_dual_robot_cell_control/launch/start_robots.launch.py index 1981d5a..a5b3aed 100644 --- a/my_dual_robot_cell/my_dual_robot_cell_control/launch/start_robots.launch.py +++ b/my_dual_robot_cell/my_dual_robot_cell_control/launch/start_robots.launch.py @@ -157,6 +157,9 @@ def controller_spawner(controllers, active=True): return Node( package="controller_manager", executable="spawner", + parameters=[ + ParameterFile(controllers_file, allow_substs=True), + ], arguments=[ "--controller-manager", "/controller_manager", @@ -189,6 +192,9 @@ def controller_spawner(controllers, active=True): alice_initial_joint_controller_spawner_started = Node( package="controller_manager", executable="spawner", + parameters=[ + ParameterFile(controllers_file, allow_substs=True), + ], arguments=[ alice_initial_joint_controller, "-c", @@ -201,6 +207,9 @@ def controller_spawner(controllers, active=True): bob_initial_joint_controller_spawner_started = Node( package="controller_manager", executable="spawner", + parameters=[ + ParameterFile(controllers_file, allow_substs=True), + ], arguments=[ bob_initial_joint_controller, "-c", @@ -213,6 +222,9 @@ def controller_spawner(controllers, active=True): alice_initial_joint_controller_spawner_stopped = Node( package="controller_manager", executable="spawner", + parameters=[ + ParameterFile(controllers_file, allow_substs=True), + ], arguments=[ alice_initial_joint_controller, "-c", @@ -226,6 +238,9 @@ def controller_spawner(controllers, active=True): bob_initial_joint_controller_spawner_stopped = Node( package="controller_manager", executable="spawner", + parameters=[ + ParameterFile(controllers_file, allow_substs=True), + ], arguments=[ bob_initial_joint_controller, "-c", @@ -396,14 +411,14 @@ def generate_launch_description(): declared_arguments.append( DeclareLaunchArgument( "alice_initial_joint_controller", - default_value="alice_scaled_joint_trajectory_controller", + default_value="alice_joint_trajectory_controller", description="Initially loaded robot controller for the alice robot arm.", ) ) declared_arguments.append( DeclareLaunchArgument( "bob_initial_joint_controller", - default_value="bob_scaled_joint_trajectory_controller", + default_value="bob_joint_trajectory_controller", description="Initially loaded robot controller for the bob robot arm.", ) ) diff --git a/my_dual_robot_cell/my_dual_robot_cell_control/package.xml b/my_dual_robot_cell/my_dual_robot_cell_control/package.xml index 10f9f88..54df3a7 100644 --- a/my_dual_robot_cell/my_dual_robot_cell_control/package.xml +++ b/my_dual_robot_cell/my_dual_robot_cell_control/package.xml @@ -13,9 +13,9 @@ ament_cmake + forward_command_controller joint_state_broadcaster joint_trajectory_controller - position_controllers robot_state_publisher my_dual_robot_cell_description ur_controllers @@ -24,6 +24,8 @@ xacro controller_manager + launch_testing_ament_cmake + ros2run ament_cmake diff --git a/my_dual_robot_cell/my_dual_robot_cell_control/test/example_move.py b/my_dual_robot_cell/my_dual_robot_cell_control/test/example_move.py new file mode 100644 index 0000000..b43b715 --- /dev/null +++ b/my_dual_robot_cell/my_dual_robot_cell_control/test/example_move.py @@ -0,0 +1,102 @@ +#!/usr/bin/env python +# Copyright 2026, Universal Robots A/S +# +# 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 logging +import subprocess +import unittest + +import pytest +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare +from launch_testing.actions import ReadyToTest + + +ROBOTS = { + "alice": { + "tf_prefix": "alice_", + "controller_name": "alice_joint_trajectory_controller", + }, + "bob": { + "tf_prefix": "bob_", + "controller_name": "bob_joint_trajectory_controller", + }, +} + +TIMEOUT_EXAMPLE_MOVE = 120 + + +@pytest.mark.launch_test +def generate_test_description(): + # Mock hardware lets us exercise the tutorial's launch file and controllers without bringing + # up two URSim instances, dashboard clients or the external_control program. + tutorial_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + FindPackageShare("my_dual_robot_cell_control"), + "launch", + "start_robots.launch.py", + ] + ) + ), + launch_arguments={ + "alice_use_mock_hardware": "true", + "bob_use_mock_hardware": "true", + "launch_rviz": "false", + }.items(), + ) + + return LaunchDescription([ReadyToTest(), tutorial_launch]) + + +class DualArmExampleMoveTest(unittest.TestCase): + def _run_example_move(self, name): + cfg = ROBOTS[name] + cmd = [ + "ros2", + "run", + "ur_robot_driver", + "example_move.py", + "--ros-args", + "-p", + f"tf_prefix:={cfg['tf_prefix']}", + "-p", + f"controller_name:={cfg['controller_name']}", + ] + logging.info("Running example_move for '%s': %s", name, " ".join(cmd)) + subprocess.check_call(cmd, timeout=TIMEOUT_EXAMPLE_MOVE) + + def test_example_move_both_arms(self): + # example_move.py waits for the controller's action server itself, so we can simply run + # it once per arm. + for name in ROBOTS: + self._run_example_move(name) diff --git a/my_dual_robot_cell/my_dual_robot_cell_control/urdf/my_dual_robot_cell_controlled.urdf.xacro b/my_dual_robot_cell/my_dual_robot_cell_control/urdf/my_dual_robot_cell_controlled.urdf.xacro index 2e2cdd4..7099115 100644 --- a/my_dual_robot_cell/my_dual_robot_cell_control/urdf/my_dual_robot_cell_controlled.urdf.xacro +++ b/my_dual_robot_cell/my_dual_robot_cell_control/urdf/my_dual_robot_cell_controlled.urdf.xacro @@ -62,6 +62,7 @@ ament_cmake - + forward_command_controller joint_state_broadcaster joint_trajectory_controller - position_controllers robot_state_publisher my_robot_cell_description ur_controllers @@ -21,6 +20,9 @@ xacro controller_manager + launch_testing_ament_cmake + ros2run + ur_dashboard_msgs ament_cmake diff --git a/my_robot_cell/my_robot_cell_control/test/example_move.py b/my_robot_cell/my_robot_cell_control/test/example_move.py new file mode 100644 index 0000000..88e619e --- /dev/null +++ b/my_robot_cell/my_robot_cell_control/test/example_move.py @@ -0,0 +1,218 @@ +#!/usr/bin/env python +# Copyright 2026, Universal Robots A/S +# +# 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 logging +import subprocess +import time +import unittest + +import pytest +import rclpy +from launch import LaunchDescription +from launch.actions import ( + ExecuteProcess, + IncludeLaunchDescription, + RegisterEventHandler, +) +from launch.event_handlers import OnProcessExit +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import PathJoinSubstitution +from launch_ros.substitutions import FindPackagePrefix, FindPackageShare +from launch_testing.actions import ReadyToTest +from rclpy.node import Node as ROSNode + +from std_srvs.srv import Trigger +from ur_dashboard_msgs.msg import RobotMode +from ur_dashboard_msgs.srv import GetRobotMode + + +UR_TYPE = "ur20" +TF_PREFIX = f"{UR_TYPE}_" +ROBOT_IP = "192.168.56.101" + +TIMEOUT_WAIT_SERVICE = 10 +# When the test runs together with downloading the URSim docker image, the dashboard server can +# take quite a while to come up. +TIMEOUT_WAIT_SERVICE_INITIAL = 120 +TIMEOUT_EXAMPLE_MOVE = 120 + + +@pytest.mark.launch_test +def generate_test_description(): + ursim = ExecuteProcess( + cmd=[ + PathJoinSubstitution( + [ + FindPackagePrefix("ur_client_library"), + "lib", + "ur_client_library", + "start_ursim.sh", + ] + ), + "-m", + UR_TYPE, + ], + name="start_ursim", + output="screen", + ) + + wait_dashboard_server = ExecuteProcess( + cmd=[ + PathJoinSubstitution( + [FindPackagePrefix("ur_robot_driver"), "bin", "wait_dashboard_server.sh"] + ) + ], + name="wait_dashboard_server", + output="screen", + ) + + # Use the tutorial's launch file unchanged. Running it in headless mode lets the test drive + # the robot via the dashboard client without manually loading the external_control program. + tutorial_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + FindPackageShare("my_robot_cell_control"), + "launch", + "start_robot.launch.py", + ] + ) + ), + launch_arguments={ + "ur_type": UR_TYPE, + "robot_ip": ROBOT_IP, + "launch_rviz": "false", + "headless_mode": "true", + }.items(), + ) + + driver_starter = RegisterEventHandler( + OnProcessExit(target_action=wait_dashboard_server, on_exit=tutorial_launch) + ) + + return LaunchDescription([ReadyToTest(), wait_dashboard_server, ursim, driver_starter]) + + +def _wait_for_service(node, srv_name, srv_type, timeout): + client = node.create_client(srv_type, srv_name) + logging.info("Waiting for service '%s' with timeout %fs...", srv_name, timeout) + if not client.wait_for_service(timeout): + raise RuntimeError(f"Could not reach service '{srv_name}' within {timeout}s") + return client + + +def _call_service(node, client, request): + future = client.call_async(request) + rclpy.spin_until_future_complete(node, future, timeout_sec=TIMEOUT_WAIT_SERVICE) + if future.result() is None: + raise RuntimeError(f"Error while calling service '{client.srv_name}': {future.exception()}") + return future.result() + + +class ExampleMoveTest(unittest.TestCase): + @classmethod + def setUpClass(cls): + rclpy.init() + cls.node = ROSNode("custom_workcell_example_move_test") + + cls.power_on = _wait_for_service( + cls.node, "/dashboard_client/power_on", Trigger, TIMEOUT_WAIT_SERVICE_INITIAL + ) + cls.power_off = _wait_for_service( + cls.node, "/dashboard_client/power_off", Trigger, TIMEOUT_WAIT_SERVICE + ) + cls.brake_release = _wait_for_service( + cls.node, "/dashboard_client/brake_release", Trigger, TIMEOUT_WAIT_SERVICE + ) + cls.unlock_protective_stop = _wait_for_service( + cls.node, + "/dashboard_client/unlock_protective_stop", + Trigger, + TIMEOUT_WAIT_SERVICE, + ) + cls.stop = _wait_for_service( + cls.node, "/dashboard_client/stop", Trigger, TIMEOUT_WAIT_SERVICE + ) + cls.get_robot_mode = _wait_for_service( + cls.node, "/dashboard_client/get_robot_mode", GetRobotMode, TIMEOUT_WAIT_SERVICE + ) + cls.resend_robot_program = _wait_for_service( + cls.node, + "/io_and_status_controller/resend_robot_program", + Trigger, + TIMEOUT_WAIT_SERVICE_INITIAL, + ) + + @classmethod + def tearDownClass(cls): + cls.node.destroy_node() + rclpy.shutdown() + + def _check_success(self, response): + self.assertTrue(response.success, msg=getattr(response, "message", "")) + + def _start_robot_once(self): + """Power the robot on, release the brakes and wait until it reports RUNNING.""" + self._check_success(_call_service(self.node, self.power_off, Trigger.Request())) + self._check_success(_call_service(self.node, self.power_on, Trigger.Request())) + self._check_success(_call_service(self.node, self.brake_release, Trigger.Request())) + # unlock_protective_stop is not guaranteed to succeed if the robot is not in a + # protective stop, so we don't assert success here. + _call_service(self.node, self.unlock_protective_stop, Trigger.Request()) + + start_time = time.time() + while time.time() - start_time < TIMEOUT_WAIT_SERVICE_INITIAL: + robot_mode = _call_service(self.node, self.get_robot_mode, GetRobotMode.Request()) + self._check_success(robot_mode) + if robot_mode.robot_mode.mode == RobotMode.RUNNING: + _call_service(self.node, self.stop, Trigger.Request()) + return + time.sleep(0.5) + raise AssertionError( + f"Robot did not reach RUNNING mode within {TIMEOUT_WAIT_SERVICE_INITIAL}s " + f"(last mode: {robot_mode.robot_mode.mode})" + ) + + def test_example_move(self): + # Start the robot exactly once before invoking example_move. + self._start_robot_once() + time.sleep(1) + self._check_success(_call_service(self.node, self.resend_robot_program, Trigger.Request())) + + cmd = [ + "ros2", + "run", + "ur_robot_driver", + "example_move.py", + "--ros-args", + "-p", + f"tf_prefix:={TF_PREFIX}", + ] + logging.info("Running example_move: %s", " ".join(cmd)) + subprocess.check_call(cmd, timeout=TIMEOUT_EXAMPLE_MOVE) diff --git a/my_robot_cell/my_robot_cell_control/urdf/my_robot_cell_controlled.urdf.xacro b/my_robot_cell/my_robot_cell_control/urdf/my_robot_cell_controlled.urdf.xacro index d55fa1d..2049316 100644 --- a/my_robot_cell/my_robot_cell_control/urdf/my_robot_cell_controlled.urdf.xacro +++ b/my_robot_cell/my_robot_cell_control/urdf/my_robot_cell_controlled.urdf.xacro @@ -39,6 +39,7 @@