bug #399: adding timeout to wait for goto and factorise function

src/reachy2_sdk/parts/arm.py

@@ -369,10 +369,7 @@ def goto_from_matrix(
         if response.id == -1:
             self._logger.error(f"Target pose:\n {target} \nwas not reachable. No command sent.")
         elif 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.")
+            self._wait_goto(response, duration + 1)
         return response
 
     def send_cartesian_interpolation(
@@ -592,10 +589,7 @@ def goto_joints(
         if response.id == -1:
             self._logger.error(f"Position {positions} was not reachable. No command sent.")
         elif 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.")
+            self._wait_goto(response, duration + 1)
         return response
 
     def get_translation_by(
@@ -767,10 +761,7 @@ def _goto_single_joint(
         if response.id == -1:
             self._logger.error(f"Position {goal_position} was not reachable. No command sent.")
         elif 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.")
+            self._wait_goto(response, duration + 1)
         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__)  # avoid name conflict with class logger
 
     def get_goto_playing(self) -> GoToId:
         """Return the id of the goto currently playing on the part"""
@@ -80,3 +83,14 @@ def _is_goto_finished(self, id: GoToId) -> bool:
             or state.goal_status == GoalStatus.STATUS_SUCCEEDED
         )
         return result
+
+    def _wait_goto(self, id: GoToId, timeout: float = 10) -> None:
+        """Wait for a goto to finish. timeout is in seconds."""
+        self._logger_goto.info(f"Waiting for movement with {id}.")
+        t1 = time.time()
+        while not self._is_goto_finished(id):
+            time.sleep(0.1)
+            if time.time() - t1 > 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
@@ -134,10 +133,7 @@ def look_at(
         if response.id == -1:
             self._logger.error(f"Position {x}, {y}, {z} was not reachable. No command sent.")
         elif 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.")
+            self._wait_goto(response, duration + 1)
         return response
 
     def goto_joints(
@@ -182,10 +178,7 @@ def goto_joints(
         if response.id == -1:
             self._logger.error(f"Position {positions} was not reachable. No command sent.")
         elif 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.")
+            self._wait_goto(response, duration + 1)
         return response
 
     def _goto_single_joint(
@@ -214,10 +207,7 @@ def _goto_single_joint(
         if response.id == -1:
             self._logger.error(f"Position {goal_position} was not reachable. No command sent.")
         elif 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.")
+            self._wait_goto(response, duration + 1)
         return response
 
     def goto_quat(
@@ -244,10 +234,7 @@ def goto_quat(
         if response.id == -1:
             self._logger.error(f"Orientation {q} was not reachable. No command sent.")
         elif 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.")
+            self._wait_goto(response, duration + 1)
         return response
 
     def send_goal_positions(self) -> None: