Wait for IO controller before checking IOs

Author: fmauch

ur_robot_driver/test/urscript_interface.py

@@ -55,6 +55,7 @@
     Load,
 )
 from ur_msgs.msg import IOStates
+from controller_manager_msgs.srv import ListControllers
 
 
 ROBOT_IP = "192.168.56.101"
@@ -156,27 +157,28 @@ def init_robot(self):
 
         # Connect to all other expected services
         dashboard_interfaces = {
-            "power_off": Trigger,
-            "brake_release": Trigger,
-            "unlock_protective_stop": Trigger,
-            "restart_safety": Trigger,
-            "get_robot_mode": GetRobotMode,
-            "load_installation": Load,
-            "load_program": Load,
-            "close_popup": Trigger,
-            "get_loaded_program": GetLoadedProgram,
-            "program_state": GetProgramState,
-            "program_running": IsProgramRunning,
-            "play": Trigger,
-            "stop": Trigger,
+            "/dashboard_client/power_off": Trigger,
+            "/dashboard_client/brake_release": Trigger,
+            "/dashboard_client/unlock_protective_stop": Trigger,
+            "/dashboard_client/restart_safety": Trigger,
+            "/dashboard_client/get_robot_mode": GetRobotMode,
+            "/dashboard_client/load_installation": Load,
+            "/dashboard_client/load_program": Load,
+            "/dashboard_client/close_popup": Trigger,
+            "/dashboard_client/get_loaded_program": GetLoadedProgram,
+            "/dashboard_client/program_state": GetProgramState,
+            "/dashboard_client/program_running": IsProgramRunning,
+            "/dashboard_client/play": Trigger,
+            "/dashboard_client/stop": Trigger,
+            "/controller_manager/list_controllers": ListControllers,
         }
-        self.dashboard_clients = {
-            srv_name: waitForService(self.node, f"/dashboard_client/{srv_name}", srv_type)
+        self.service_clients = {
+            srv_name: waitForService(self.node, f"{srv_name}", srv_type)
             for (srv_name, srv_type) in dashboard_interfaces.items()
         }
 
         # Add first client to dict
-        self.dashboard_clients["power_on"] = power_on_client
+        self.service_clients["/dashboard_client/power_on"] = power_on_client
 
         self.urscript_pub = self.node.create_publisher(
             StringMsg, "/urscript_interface/script_command", 1
@@ -185,8 +187,19 @@ def init_robot(self):
     def setUp(self):
         # Start robot
         empty_req = Trigger.Request()
-        self.dashboard_call("power_on", empty_req)
-        self.dashboard_call("brake_release", empty_req)
+        self.call_service("/dashboard_client/power_on", empty_req)
+        self.call_service("/dashboard_client/brake_release", empty_req)
+
+        io_controller_running = False
+
+        while not io_controller_running:
+            time.sleep(1)
+            response = self.call_service(
+                "/controller_manager/list_controllers", ListControllers.Request()
+            )
+            for controller in response.controller:
+                if controller.name == "io_and_status_controller":
+                    io_controller_running = controller.state == "active"
 
     def test_set_io(self):
         """Test setting an IO using a direct program call."""
@@ -242,11 +255,12 @@ def check_pin_states(self, pins, states):
             if self.io_msg is not None:
                 for i, pin_id in enumerate(pins):
                     pin_states[i] = self.io_msg.digital_out_states[pin_id].state
+        self.assertIsNotNone(self.io_msg, "Did not receive an IO state in requested time.")
         self.assertEqual(pin_states, states)
 
-    def dashboard_call(self, srv_name, request):
+    def call_service(self, srv_name, request):
         self.node.get_logger().info(f"Calling service '{srv_name}' with request {request}")
-        future = self.dashboard_clients[srv_name].call_async(request)
+        future = self.service_clients[srv_name].call_async(request)
         rclpy.spin_until_future_complete(self.node, future)
         if future.result() is not None:
             self.node.get_logger().info(f"Received result {future.result()}")