Merge pull request #417 from pollen-robotics/399-robot-is-stuck-in-in…

…finite-loop-using-translate_by-with-wait=true

399 robot is stuck in infinite loop using translate by with wait=true

src/reachy2_sdk/parts/arm.py

@@ -368,11 +368,8 @@ def goto_from_matrix(
         response = self._goto_stub.GoToCartesian(request)
         if response.id == -1:
             self._logger.error(f"Target pose:\n {target} \nwas not reachable. No command sent.")
-        if wait:
-            self._logger.info(f"Waiting for movement with {response}.")
-            while not self._is_goto_finished(response):
-                time.sleep(0.1)
-            self._logger.info(f"Movement with {response} finished.")
+        elif wait:
+            self._wait_goto(response)
         return response
 
     def send_cartesian_interpolation(
@@ -591,11 +588,8 @@ def goto_joints(
         response = self._goto_stub.GoToJoints(request)
         if response.id == -1:
             self._logger.error(f"Position {positions} was not reachable. No command sent.")
-        if wait:
-            self._logger.info(f"Waiting for movement with {response}.")
-            while not self._is_goto_finished(response):
-                time.sleep(0.1)
-            self._logger.info(f"Movement with {response} finished.")
+        elif wait:
+            self._wait_goto(response)
         return response
 
     def get_translation_by(
@@ -763,11 +757,11 @@ def _goto_single_joint(
             interpolation_mode=get_grpc_interpolation_mode(interpolation_mode),
         )
         response = self._goto_stub.GoToJoints(request)
-        if wait:
-            self._logger.info(f"Waiting for movement with {response}.")
-            while not self._is_goto_finished(response):
-                time.sleep(0.1)
-            self._logger.info(f"Movement with {response} finished.")
+
+        if response.id == -1:
+            self._logger.error(f"Position {goal_position} was not reachable. No command sent.")
+        elif wait:
+            self._wait_goto(response)
         return response
 
     def get_joints_positions(self, degrees: bool = True, round: Optional[int] = None) -> List[float]:

src/reachy2_sdk/parts/goto_based_part.py

@@ -1,3 +1,5 @@
+import logging
+import time
 from abc import ABC
 from typing import List, Optional
 
@@ -27,6 +29,7 @@ def __init__(
         """Initialize the common attributes."""
         self.part = part
         self._goto_stub = goto_stub
+        self._logger_goto = logging.getLogger(__name__)  # not using self._logger to avoid name conflict in multiple inheritance
 
     def get_goto_playing(self) -> GoToId:
         """Return the id of the goto currently playing on the part"""
@@ -80,3 +83,30 @@ def _is_goto_finished(self, id: GoToId) -> bool:
             or state.goal_status == GoalStatus.STATUS_SUCCEEDED
         )
         return result
+
+    def _wait_goto(self, id: GoToId) -> None:
+        """Wait for a goto to finish. timeout is in seconds."""
+        self._logger_goto.info(f"Waiting for movement with {id}.")
+
+        id_playing = self.get_goto_playing()
+        info_gotos = [self._get_goto_joints_request(id_playing)]
+        ids_queue = self.get_goto_queue()
+        for id in ids_queue:
+            info_gotos.append(self._get_goto_joints_request(id))
+
+        timeout = 1  # adding one more sec
+        for igoto in info_gotos:
+            if igoto is not None:
+                timeout += igoto.duration
+
+        self._logger_goto.debug(f"timeout is set to {timeout}")
+
+        t_start = time.time()  # timeout for others
+        while not self._is_goto_finished(id):
+            time.sleep(0.1)
+
+            if time.time() - t_start > timeout:
+                self._logger_goto.warning(f"Waiting time for movement with {id} is timeout.")
+                return
+
+        self._logger_goto.info(f"Movement with {id} finished.")

src/reachy2_sdk/parts/head.py

@@ -5,7 +5,6 @@
 - look_at function
 """
 
-import time
 from typing import List
 
 import grpc
@@ -131,11 +130,10 @@ def look_at(
             interpolation_mode=get_grpc_interpolation_mode(interpolation_mode),
         )
         response = self._goto_stub.GoToCartesian(request)
-        if wait:
-            self._logger.info(f"Waiting for movement with {response}.")
-            while not self._is_goto_finished(response):
-                time.sleep(0.1)
-            self._logger.info(f"Movement with {response} finished.")
+        if response.id == -1:
+            self._logger.error(f"Position {x}, {y}, {z} was not reachable. No command sent.")
+        elif wait:
+            self._wait_goto(response)
         return response
 
     def goto_joints(
@@ -177,11 +175,10 @@ def goto_joints(
             interpolation_mode=get_grpc_interpolation_mode(interpolation_mode),
         )
         response = self._goto_stub.GoToJoints(request)
-        if wait:
-            self._logger.info(f"Waiting for movement with {response}.")
-            while not self._is_goto_finished(response):
-                time.sleep(0.1)
-            self._logger.info(f"Movement with {response} finished.")
+        if response.id == -1:
+            self._logger.error(f"Position {positions} was not reachable. No command sent.")
+        elif wait:
+            self._wait_goto(response)
         return response
 
     def _goto_single_joint(
@@ -207,11 +204,10 @@ def _goto_single_joint(
             interpolation_mode=get_grpc_interpolation_mode(interpolation_mode),
         )
         response = self._goto_stub.GoToJoints(request)
-        if wait:
-            self._logger.info(f"Waiting for movement with {response}.")
-            while not self._is_goto_finished(response):
-                time.sleep(0.1)
-            self._logger.info(f"Movement with {response} finished.")
+        if response.id == -1:
+            self._logger.error(f"Position {goal_position} was not reachable. No command sent.")
+        elif wait:
+            self._wait_goto(response)
         return response
 
     def goto_quat(
@@ -235,11 +231,10 @@ def goto_quat(
             interpolation_mode=get_grpc_interpolation_mode(interpolation_mode),
         )
         response = self._goto_stub.GoToJoints(request)
-        if wait:
-            self._logger.info(f"Waiting for movement with {response}.")
-            while not self._is_goto_finished(response):
-                time.sleep(0.1)
-            self._logger.info(f"Movement with {response} finished.")
+        if response.id == -1:
+            self._logger.error(f"Orientation {q} was not reachable. No command sent.")
+        elif wait:
+            self._wait_goto(response)
         return response
 
     def send_goal_positions(self) -> None:

tests/units/online/test_advanced_goto_functions.py

@@ -483,6 +483,12 @@ def test_wait_move(reachy_sdk_zeroed: ReachySDK) -> None:
     elapsed_time = time.time() - tic
     assert elapsed_time < 0.1
 
+    tic = time.time()
+    reachy_sdk_zeroed.goto_posture("default", duration=1.0)
+    reachy_sdk_zeroed.r_arm.goto_from_matrix(A, wait=True, duration=1.0)
+    elapsed_time = time.time() - tic
+    assert elapsed_time >= 2.0
+
 
 @pytest.mark.online
 def test_is_goto_finished(reachy_sdk_zeroed: ReachySDK) -> None: