66 lines
1.5 KiB
Python
66 lines
1.5 KiB
Python
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
|