[Driver Tests] Unlock protective stop during test case setup (UniversalRobots#1641)

* Use custom logging for ListControllersResponse

* Unlock protective stop

* Use a while loop instead of a fixed wait

---------

Co-authored-by: Rune Søe-Knudsen <41109954+urrsk@users.noreply.github.com>

Author: urfeex

ur_robot_driver/test/test_common.py

@@ -112,7 +112,11 @@ def _call_service(node, client, request):
     rclpy.spin_until_future_complete(node, future)
 
     if future.result() is not None:
-        logging.info("  Received result: %s", future.result())
+        response_str = str(future.result())
+        if type(future.result()).__name__ == "ListControllers_Response":
+            controllers_str_list = [f"{c.name}: {c.state}" for c in future.result().controller]
+            response_str = f"controllers: [{', '.join(controllers_str_list)}]"
+        logging.info("  Received result: %s", response_str)
         return future.result()
 
     raise Exception(f"Error while calling service '{client.srv_name}': {future.exception()}")
@@ -238,17 +242,21 @@ def start_robot(self):
         self._check_call(self.power_off())
         self._check_call(self.power_on())
         self._check_call(self.brake_release())
+        self._check_call(self.unlock_protective_stop())
 
         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())
+        start_time = time.time()
+        while time.time() - start_time < TIMEOUT_WAIT_SERVICE:
+            self._check_call(robot_mode)
+            if robot_mode.robot_mode.mode == RobotMode.RUNNING:
+                self._check_call(self.stop())
+                return
+            time.sleep(0.1)
+        raise Exception(
+            f"Incorrect robot mode: Expected {RobotMode.RUNNING}, got {robot_mode.robot_mode.mode}"
+        )
 
     def _check_call(self, result):
         if not result.success: