Added arguments to test launch

urmahp · urmahp · commit e7e47d5dbe84 · 2023-08-28T15:58:05.000+02:00
Added arguments when launching the test:
 - Added robot ip
 - Added launch_ursim to choose whether or not to launch ursim when running the test.
 - If the arguments are not provided the behavior is the same as before adding the arguments.

Ensure that the scaled joint trajectory controller is running, before sending the motion.
diff --git a/ur_robot_driver/test/dashboard_client.py b/ur_robot_driver/test/dashboard_client.py
@@ -35,6 +35,7 @@
 import rclpy
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription
+from launch.conditions import IfCondition
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
 from launch_ros.substitutions import FindPackagePrefix, FindPackageShare
@@ -69,7 +70,25 @@ def generate_test_description():
         )
     )
 
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "robot_ip",
+            default_value="192.168.56.101",
+            description="IP address of used UR robot.",
+        )
+    )
+
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "launch_ursim",
+            default_value="true",
+            description="Launches the ursim when running the test if yes",
+        )
+    )
+
     ur_type = LaunchConfiguration("ur_type")
+    robot_ip = LaunchConfiguration("robot_ip")
+    launch_ursim = LaunchConfiguration("launch_ursim")
 
     dashboard_client = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
@@ -82,9 +101,10 @@ def generate_test_description():
             )
         ),
         launch_arguments={
-            "robot_ip": "192.168.56.101",
+            "robot_ip": robot_ip,
         }.items(),
     )
+
     ursim = ExecuteProcess(
         cmd=[
             PathJoinSubstitution(
@@ -101,6 +121,7 @@ def generate_test_description():
         ],
         name="start_ursim",
         output="screen",
+        condition=IfCondition(launch_ursim),
     )
 
     return LaunchDescription(declared_arguments + [ReadyToTest(), dashboard_client, ursim])
@@ -159,7 +180,10 @@ def test_switch_on(self):
         # Wait until the robot is booted completely
         end_time = time.time() + 10
         mode = RobotMode.DISCONNECTED
-        while mode != RobotMode.POWER_OFF and time.time() < end_time:
+        while (
+            mode not in (RobotMode.POWER_OFF, RobotMode.IDLE, RobotMode.RUNNING)
+            and time.time() < end_time
+        ):
             time.sleep(0.1)
             result = self.call_dashboard_service("get_robot_mode", GetRobotMode.Request())
             self.assertTrue(result.success)
diff --git a/ur_robot_driver/test/robot_driver.py b/ur_robot_driver/test/robot_driver.py
@@ -44,6 +44,7 @@
     IncludeLaunchDescription,
     RegisterEventHandler,
 )
+from launch.conditions import IfCondition, UnlessCondition
 from launch.event_handlers import OnProcessExit
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
@@ -57,6 +58,7 @@
 from ur_dashboard_msgs.srv import GetRobotMode
 from ur_msgs.msg import IOStates
 from ur_msgs.srv import SetIO
+from controller_manager_msgs.srv import ListControllers
 
 TIMEOUT_WAIT_SERVICE = 10
 TIMEOUT_WAIT_SERVICE_INITIAL = 60
@@ -90,7 +92,25 @@ def generate_test_description(tf_prefix):
         )
     )
 
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "robot_ip",
+            default_value="192.168.56.101",
+            description="IP address of used UR robot.",
+        )
+    )
+
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "launch_ursim",
+            default_value="true",
+            description="Launches the ursim when running the test if True",
+        )
+    )
+
     ur_type = LaunchConfiguration("ur_type")
+    robot_ip = LaunchConfiguration("robot_ip")
+    launch_ursim = LaunchConfiguration("launch_ursim")
 
     robot_driver = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
@@ -109,7 +129,9 @@ def generate_test_description(tf_prefix):
             "start_joint_controller": "false",
             "tf_prefix": tf_prefix,
         }.items(),
+        condition=IfCondition(launch_ursim),
     )
+
     ursim = ExecuteProcess(
         cmd=[
             PathJoinSubstitution(
@@ -126,22 +148,48 @@ def generate_test_description(tf_prefix):
         ],
         name="start_ursim",
         output="screen",
+        condition=IfCondition(launch_ursim),
     )
+
     wait_dashboard_server = ExecuteProcess(
         cmd=[
             PathJoinSubstitution(
                 [FindPackagePrefix("ur_robot_driver"), "bin", "wait_dashboard_server.sh"]
-            )
+            ),
         ],
         name="wait_dashboard_server",
         output="screen",
+        condition=IfCondition(launch_ursim),
     )
+
     driver_starter = RegisterEventHandler(
-        OnProcessExit(target_action=wait_dashboard_server, on_exit=robot_driver)
+        OnProcessExit(target_action=wait_dashboard_server, on_exit=robot_driver),
+        condition=IfCondition(launch_ursim),
+    )
+
+    robot_driver_no_wait = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            PathJoinSubstitution(
+                [FindPackageShare("ur_robot_driver"), "launch", "ur_control.launch.py"]
+            )
+        ),
+        launch_arguments={
+            "robot_ip": robot_ip,
+            "ur_type": ur_type,
+            "launch_rviz": "false",
+            "controller_spawner_timeout": str(TIMEOUT_WAIT_SERVICE_INITIAL),
+            "initial_joint_controller": "scaled_joint_trajectory_controller",
+            "headless_mode": "true",
+            "launch_dashboard_client": "false",
+            "start_joint_controller": "false",
+            "tf_prefix": tf_prefix,
+        }.items(),
+        condition=UnlessCondition(launch_ursim),
     )
 
     return LaunchDescription(
-        declared_arguments + [ReadyToTest(), wait_dashboard_server, ursim, driver_starter]
+        declared_arguments
+        + [ReadyToTest(), wait_dashboard_server, ursim, driver_starter, robot_driver_no_wait]
     )
 
 
@@ -184,6 +232,7 @@ def init_robot(self):
             "/dashboard_client/get_robot_mode": GetRobotMode,
             "/controller_manager/switch_controller": SwitchController,
             "/io_and_status_controller/resend_robot_program": Trigger,
+            "/controller_manager/list_controllers": ListControllers,
         }
         self.service_clients.update(
             {
@@ -282,6 +331,9 @@ def io_msg_cb(msg):
 
     def test_trajectory(self, tf_prefix):
         """Test robot movement."""
+        # Wait for controller to be active
+        self.waitForController("scaled_joint_trajectory_controller")
+
         # Construct test trajectory
         test_trajectory = [
             (Duration(sec=6, nanosec=0), [0.0 for j in ROBOT_JOINTS]),
@@ -320,6 +372,9 @@ def test_illegal_trajectory(self, tf_prefix):
 
         This is more of a validation test that the testing suite does the right thing
         """
+        # Wait for controller to be active
+        self.waitForController("scaled_joint_trajectory_controller")
+
         # Construct test trajectory, the second point wrongly starts before the first
         test_trajectory = [
             (Duration(sec=6, nanosec=0), [0.0 for j in ROBOT_JOINTS]),
@@ -347,6 +402,9 @@ def test_illegal_trajectory(self, tf_prefix):
 
     def test_trajectory_scaled(self, tf_prefix):
         """Test robot movement."""
+        # Wait for controller to be active
+        self.waitForController("scaled_joint_trajectory_controller")
+
         # Construct test trajectory
         test_trajectory = [
             (Duration(sec=6, nanosec=0), [0.0 for j in ROBOT_JOINTS]),
@@ -442,6 +500,25 @@ def get_result(self, action_name, goal_response, timeout):
         else:
             raise Exception(f"Exception while calling action: {future_res.exception()}")
 
+    def waitForController(
+        self, controller_name, controller_status="active", timeout=TIMEOUT_WAIT_SERVICE
+    ):
+        controller_running = False
+        end_time = time.time() + timeout
+        while controller_running is False and time.time() < end_time:
+            time.sleep(1)
+            response = self.call_service(
+                "/controller_manager/list_controllers", ListControllers.Request()
+            )
+            for controller in response.controller:
+                if controller.name == controller_name:
+                    controller_running = controller.state == controller_status
+
+        if controller_running is False:
+            raise Exception(
+                f"Controller {controller_name} did not reach controller state {controller_status} within timeout of {timeout}"
+            )
+
 
 def waitForService(node, srv_name, srv_type, timeout=TIMEOUT_WAIT_SERVICE):
     client = node.create_client(srv_type, srv_name)
diff --git a/ur_robot_driver/test/urscript_interface.py b/ur_robot_driver/test/urscript_interface.py
@@ -40,6 +40,7 @@
     IncludeLaunchDescription,
     RegisterEventHandler,
 )
+from launch.conditions import IfCondition, UnlessCondition
 from launch.event_handlers import OnProcessExit
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
@@ -79,7 +80,25 @@ def generate_test_description():
         )
     )
 
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "robot_ip",
+            default_value="192.168.56.101",
+            description="IP address of used UR robot.",
+        )
+    )
+
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "launch_ursim",
+            default_value="true",
+            description="Launches the ursim when running the test if True",
+        )
+    )
+
     ur_type = LaunchConfiguration("ur_type")
+    robot_ip = LaunchConfiguration("robot_ip")
+    launch_ursim = LaunchConfiguration("launch_ursim")
 
     robot_driver = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
@@ -97,6 +116,7 @@ def generate_test_description():
             "launch_dashboard_client": "false",
             "start_joint_controller": "false",
         }.items(),
+        condition=IfCondition(launch_ursim),
     )
 
     ursim = ExecuteProcess(
@@ -115,24 +135,47 @@ def generate_test_description():
         ],
         name="start_ursim",
         output="screen",
+        condition=IfCondition(launch_ursim),
     )
 
     wait_dashboard_server = ExecuteProcess(
         cmd=[
             PathJoinSubstitution(
                 [FindPackagePrefix("ur_robot_driver"), "bin", "wait_dashboard_server.sh"]
-            )
+            ),
         ],
         name="wait_dashboard_server",
         output="screen",
+        condition=IfCondition(launch_ursim),
     )
 
     driver_starter = RegisterEventHandler(
-        OnProcessExit(target_action=wait_dashboard_server, on_exit=robot_driver)
+        OnProcessExit(target_action=wait_dashboard_server, on_exit=robot_driver),
+        condition=IfCondition(launch_ursim),
+    )
+
+    robot_driver_no_wait = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            PathJoinSubstitution(
+                [FindPackageShare("ur_robot_driver"), "launch", "ur_control.launch.py"]
+            )
+        ),
+        launch_arguments={
+            "robot_ip": robot_ip,
+            "ur_type": ur_type,
+            "launch_rviz": "false",
+            "controller_spawner_timeout": str(TIMEOUT_WAIT_SERVICE_INITIAL),
+            "initial_joint_controller": "scaled_joint_trajectory_controller",
+            "headless_mode": "true",
+            "launch_dashboard_client": "false",
+            "start_joint_controller": "false",
+        }.items(),
+        condition=UnlessCondition(launch_ursim),
     )
 
     return LaunchDescription(
-        declared_arguments + [ReadyToTest(), wait_dashboard_server, driver_starter, ursim]
+        declared_arguments
+        + [ReadyToTest(), wait_dashboard_server, ursim, driver_starter, robot_driver_no_wait]
     )
 
 
