Move robot starting to dashboard interface

Author: RobertWilbrandt

ur_robot_driver/test/robot_driver.py

@@ -41,13 +41,12 @@
 from rclpy.node import Node
 from sensor_msgs.msg import JointState
 from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
-from ur_dashboard_msgs.msg import RobotMode
 from ur_msgs.msg import IOStates
 
 sys.path.append(os.path.dirname(__file__))
 from test_common import (  # noqa: E402
-    ControllerManagerInterface,
     ActionInterface,
+    ControllerManagerInterface,
     DashboardInterface,
     IoStatusInterface,
     generate_driver_test_description,
@@ -101,17 +100,7 @@ def init_robot(self):
         )
 
     def setUp(self):
-        # Start robot
-        self.assertTrue(self._dashboard_interface.power_on().success)
-        self.assertTrue(self._dashboard_interface.brake_release().success)
-
-        time.sleep(1)
-
-        robot_mode = self._dashboard_interface.get_robot_mode()
-        self.assertTrue(robot_mode.success)
-        self.assertEqual(robot_mode.robot_mode.mode, RobotMode.RUNNING)
-
-        self.assertTrue(self._dashboard_interface.stop().success)
+        self._dashboard_interface.start_robot()
         time.sleep(1)
         self.assertTrue(self._io_status_controller_interface.resend_robot_program().success)
 

ur_robot_driver/test/test_common.py

@@ -44,6 +44,7 @@
 from launch_testing.actions import ReadyToTest
 from rclpy.action import ActionClient
 from std_srvs.srv import Trigger
+from ur_dashboard_msgs.msg import RobotMode
 from ur_dashboard_msgs.srv import (
     GetLoadedProgram,
     GetProgramState,
@@ -199,7 +200,24 @@ class DashboardInterface(
         "stop": Trigger,
     },
 ):
-    pass
+    def start_robot(self):
+        self._check_call(self.power_on())
+        self._check_call(self.brake_release())
+
+        time.sleep(1)
+
+        robot_mode = self.get_robot_mode()
+        self._check_call(robot_mode)
+        if robot_mode.robot_mode.mode != RobotMode.RUNNING:
+            raise Exception(
+                f"Incorrect robot mode: Expected {RobotMode.RUNNING}, got {robot_mode.robot_mode.mode}"
+            )
+
+        self._check_call(self.stop())
+
+    def _check_call(self, result):
+        if not result.success:
+            raise Exception("Service call not successful")
 
 
 class ControllerManagerInterface(

ur_robot_driver/test/urscript_interface.py

@@ -35,15 +35,14 @@
 import rclpy
 import rclpy.node
 from std_msgs.msg import String as StringMsg
-from ur_dashboard_msgs.msg import RobotMode
 from ur_msgs.msg import IOStates
 
 sys.path.append(os.path.dirname(__file__))
 from test_common import (  # noqa: E402
+    ControllerManagerInterface,
     DashboardInterface,
     IoStatusInterface,
     generate_driver_test_description,
-    ControllerManagerInterface,
 )
 
 ROBOT_IP = "192.168.56.101"
@@ -78,17 +77,7 @@ def init_robot(self):
         )
 
     def setUp(self):
-        # Start robot
-        self.assertTrue(self._dashboard_interface.power_on().success)
-        self.assertTrue(self._dashboard_interface.brake_release().success)
-
-        time.sleep(1)
-
-        robot_mode = self._dashboard_interface.get_robot_mode()
-        self.assertTrue(robot_mode.success)
-        self.assertEqual(robot_mode.robot_mode.mode, RobotMode.RUNNING)
-
-        self.assertTrue(self._dashboard_interface.stop().success)
+        self._dashboard_interface.start_robot()
         time.sleep(1)
         self.assertTrue(self._io_status_controller_interface.resend_robot_program().success)