new control robot

src/iiwa_web/test/test_ros_action_timeout.py

@@ -0,0 +1,65 @@
+import itertools
+
+import pytest
+
+from iiwa_web.ros_node import CobotWebNode
+from iiwa_web import ros_node
+
+
+class DoneFuture:
+    def __init__(self, result):
+        self._result = result
+
+    def done(self):
+        return True
+
+    def result(self):
+        return self._result
+
+
+class PendingFuture:
+    def done(self):
+        return False
+
+    def result(self):
+        return None
+
+
+class GoalHandle:
+    accepted = True
+
+    def __init__(self):
+        self.cancel_calls = 0
+
+    def get_result_async(self):
+        return PendingFuture()
+
+    def cancel_goal_async(self):
+        self.cancel_calls += 1
+        return DoneFuture(None)
+
+
+class ActionClientStub:
+    def __init__(self, goal_handle):
+        self.goal_handle = goal_handle
+
+    def wait_for_server(self, timeout_sec):
+        return True
+
+    def send_goal_async(self, goal):
+        return DoneFuture(self.goal_handle)
+
+
+def test_send_action_cancels_goal_when_result_times_out(monkeypatch):
+    node = object.__new__(CobotWebNode)
+    goal_handle = GoalHandle()
+    node._action_clients = {"cobot/move_to_pose": ActionClientStub(goal_handle)}
+
+    timestamps = itertools.chain([0.0, 0.3, 0.6], itertools.repeat(0.9))
+    monkeypatch.setattr(ros_node.time, "monotonic", lambda: next(timestamps))
+    monkeypatch.setattr(ros_node.time, "sleep", lambda _seconds: None)
+
+    with pytest.raises(TimeoutError):
+        node.send_action(object, "cobot/move_to_pose", object(), timeout=0.5)
+
+    assert goal_handle.cancel_calls == 1