|
| 1 | +#!/usr/bin/env python |
| 2 | +# Copyright 2026, Universal Robots A/S |
| 3 | +# |
| 4 | +# Redistribution and use in source and binary forms, with or without |
| 5 | +# modification, are permitted provided that the following conditions are met: |
| 6 | +# |
| 7 | +# * Redistributions of source code must retain the above copyright |
| 8 | +# notice, this list of conditions and the following disclaimer. |
| 9 | +# |
| 10 | +# * Redistributions in binary form must reproduce the above copyright |
| 11 | +# notice, this list of conditions and the following disclaimer in the |
| 12 | +# documentation and/or other materials provided with the distribution. |
| 13 | +# |
| 14 | +# * Neither the name of the {copyright_holder} nor the names of its |
| 15 | +# contributors may be used to endorse or promote products derived from |
| 16 | +# this software without specific prior written permission. |
| 17 | +# |
| 18 | +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
| 19 | +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
| 20 | +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
| 21 | +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE |
| 22 | +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
| 23 | +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
| 24 | +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
| 25 | +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN |
| 26 | +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
| 27 | +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
| 28 | +# POSSIBILITY OF SUCH DAMAGE. |
| 29 | + |
| 30 | +import logging |
| 31 | +import os |
| 32 | +import socket |
| 33 | +import sys |
| 34 | +import subprocess |
| 35 | +import time |
| 36 | +import unittest |
| 37 | + |
| 38 | +import pytest |
| 39 | +import rclpy |
| 40 | +from control_msgs.action import FollowJointTrajectory |
| 41 | +from controller_manager_msgs.srv import SwitchController |
| 42 | +from rclpy.node import Node |
| 43 | +from std_msgs.msg import Bool |
| 44 | + |
| 45 | +from ur_dashboard_msgs.msg import ProgramState |
| 46 | + |
| 47 | +sys.path.append(os.path.dirname(__file__)) |
| 48 | +from test_common import ( # noqa: E402 |
| 49 | + generate_driver_test_description, |
| 50 | + ActionInterface, |
| 51 | + ControllerManagerInterface, |
| 52 | + DashboardInterface, |
| 53 | + IoStatusInterface, |
| 54 | +) |
| 55 | + |
| 56 | + |
| 57 | +@pytest.mark.launch_test |
| 58 | +def generate_test_description(): |
| 59 | + # When set, use a shared urcap folder to be mounted to the ursim container |
| 60 | + # This is especially useful when running the test in a Docker DooD setup. |
| 61 | + urcap_folder = os.environ.get("UR_CI_URCAP_FOLDER") |
| 62 | + return generate_driver_test_description(headless_mode=False, urcap_folder=urcap_folder) |
| 63 | + |
| 64 | + |
| 65 | +def copy_to_docker_container(container_name, src_path, dest_path): |
| 66 | + print(f"Copying {src_path} to container '{container_name}' at {dest_path}") |
| 67 | + subprocess.run(["docker", "cp", src_path, f"{container_name}:{dest_path}"], check=True) |
| 68 | + |
| 69 | + |
| 70 | +class HandBackControlTest(unittest.TestCase): |
| 71 | + @classmethod |
| 72 | + def setUpClass(cls): |
| 73 | + rclpy.init() |
| 74 | + cls.node = Node("hand_back_control_test") |
| 75 | + time.sleep(1) |
| 76 | + cls.init_robot(cls) |
| 77 | + try: |
| 78 | + copy_to_docker_container( |
| 79 | + "ursim", |
| 80 | + os.path.join( |
| 81 | + os.path.dirname(__file__), |
| 82 | + "resources", |
| 83 | + "ursim", |
| 84 | + "e-series", |
| 85 | + "ur5e", |
| 86 | + "programs", |
| 87 | + "hand_back_control_test_prog.urp", |
| 88 | + ), |
| 89 | + "/ursim/programs/hand_back_control_test_prog.urp", |
| 90 | + ) |
| 91 | + subprocess.run(["docker", "exec", "ursim", "ls", "-l", "/ursim/programs"], check=True) |
| 92 | + |
| 93 | + except Exception as e: |
| 94 | + logging.error(f"Failed to copy program to Docker container: {e}") |
| 95 | + |
| 96 | + @classmethod |
| 97 | + def tearDownClass(cls): |
| 98 | + cls.node.destroy_node() |
| 99 | + rclpy.shutdown() |
| 100 | + |
| 101 | + def init_robot(self): |
| 102 | + self._dashboard_interface = DashboardInterface(self.node) |
| 103 | + self._controller_manager_interface = ControllerManagerInterface(self.node) |
| 104 | + self._io_status_controller_interface = IoStatusInterface(self.node) |
| 105 | + |
| 106 | + self._follow_joint_trajectory = ActionInterface( |
| 107 | + self.node, |
| 108 | + "/scaled_joint_trajectory_controller/follow_joint_trajectory", |
| 109 | + FollowJointTrajectory, |
| 110 | + ) |
| 111 | + |
| 112 | + self._controller_manager_interface.wait_for_controller("scaled_joint_trajectory_controller") |
| 113 | + |
| 114 | + def setUp(self): |
| 115 | + self._dashboard_interface.start_robot() |
| 116 | + time.sleep(1) |
| 117 | + |
| 118 | + # Open a socket server on port 20957 and add a connection callback |
| 119 | + self.serversocket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) |
| 120 | + self.serversocket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) |
| 121 | + self.serversocket.settimeout(0.2) # timeout for listening |
| 122 | + self.serversocket.bind(("0.0.0.0", 20957)) |
| 123 | + self.serversocket.listen(1) |
| 124 | + |
| 125 | + def tearDown(self): |
| 126 | + self.serversocket.close() |
| 127 | + |
| 128 | + def wait_for_connection(self, timeout=5): |
| 129 | + end_time = time.time() + timeout |
| 130 | + conn = None |
| 131 | + while time.time() < end_time and conn is None: |
| 132 | + try: |
| 133 | + conn, address = self.serversocket.accept() |
| 134 | + logging.info(f"Received connection from {address}") |
| 135 | + return True |
| 136 | + except socket.timeout: |
| 137 | + continue |
| 138 | + return False |
| 139 | + |
| 140 | + def test_hand_back_control_stops_reverse_interface_and_program_keeps_running(self): |
| 141 | + """ |
| 142 | + Use hand_back_control to return program flow to the robot. |
| 143 | +
|
| 144 | + We loaded a program that connects to a test socket and waits 10 seconds after |
| 145 | + external_control has finished. Hence, if we hand back control, the program should still |
| 146 | + be running and we should receive a connection on the socket server. |
| 147 | + """ |
| 148 | + program_name = "hand_back_control_test_prog.urp" |
| 149 | + |
| 150 | + self._dashboard_interface.load_program(filename=program_name) |
| 151 | + |
| 152 | + external_control_running = None |
| 153 | + |
| 154 | + def program_state_cb(msg): |
| 155 | + nonlocal external_control_running |
| 156 | + external_control_running = msg.data |
| 157 | + |
| 158 | + program_state_sub = self.node.create_subscription( |
| 159 | + Bool, |
| 160 | + "/io_and_status_controller/robot_program_running", |
| 161 | + program_state_cb, |
| 162 | + rclpy.qos.qos_profile_system_default, |
| 163 | + ) |
| 164 | + |
| 165 | + self._dashboard_interface.play() |
| 166 | + self._controller_manager_interface.wait_for_controller( |
| 167 | + "scaled_joint_trajectory_controller", "active" |
| 168 | + ) |
| 169 | + self.assertTrue( |
| 170 | + self._controller_manager_interface.switch_controller( |
| 171 | + strictness=SwitchController.Request.BEST_EFFORT, |
| 172 | + deactivate_controllers=["passthrough_trajectory_controller"], |
| 173 | + activate_controllers=["scaled_joint_trajectory_controller"], |
| 174 | + ).ok |
| 175 | + ) |
| 176 | + |
| 177 | + self.assertTrue( |
| 178 | + external_control_running, "Robot program should be running before hand_back_control" |
| 179 | + ) |
| 180 | + |
| 181 | + # Call hand_back_control |
| 182 | + logging.info("Calling hand_back_control") |
| 183 | + result = self._io_status_controller_interface.hand_back_control() |
| 184 | + self.assertTrue(result.success, "hand_back_control service call should succeed") |
| 185 | + |
| 186 | + # Verify external control stops running |
| 187 | + end_time = time.time() + 10 |
| 188 | + while external_control_running is not False and time.time() < end_time: |
| 189 | + rclpy.spin_once(self.node, timeout_sec=0.1) |
| 190 | + self.assertFalse( |
| 191 | + external_control_running, "External Control should stop after hand_back_control" |
| 192 | + ) |
| 193 | + |
| 194 | + self.assertTrue( |
| 195 | + self.wait_for_connection(), |
| 196 | + "Robot program should connect back to the socket server after hand_back_control", |
| 197 | + ) |
| 198 | + program_state = self._dashboard_interface.program_state() |
| 199 | + self.assertEqual( |
| 200 | + program_state.state.state, |
| 201 | + ProgramState.PLAYING, |
| 202 | + "Robot program should still be running after hand_back_control", |
| 203 | + ) |
| 204 | + self.assertEqual( |
| 205 | + program_state.program_name, |
| 206 | + program_name, |
| 207 | + "Robot should still be running the same program after hand_back_control", |
| 208 | + ) |
| 209 | + |
| 210 | + self.node.destroy_subscription(program_state_sub) |
![mergify[bot]](https://avatars.githubusercontent.com/in/10562?v=4&size=40){kind=avatar}
0 commit comments