236 lines
7.1 KiB
Python
236 lines
7.1 KiB
Python
"""Motion guards: stale state, unsynchronized scene, failures and stop."""
|
|
from pathlib import Path
|
|
|
|
import pytest
|
|
|
|
from iiwa_experement.geometry import Transform, load_geometry
|
|
from iiwa_experement.motion import MotionAdapter
|
|
|
|
|
|
PACKAGE = Path(__file__).resolve().parents[1]
|
|
GEOMETRY = PACKAGE / 'config' / 'geometry.yaml'
|
|
|
|
|
|
class FakeFuture:
|
|
def __init__(self, value=None, done=True):
|
|
self._value = value
|
|
self._done = done
|
|
|
|
def done(self):
|
|
return self._done
|
|
|
|
def result(self):
|
|
return self._value
|
|
|
|
|
|
class FakeResult:
|
|
def __init__(self, success=True, message='ok'):
|
|
self.success = success
|
|
self.message = message
|
|
|
|
|
|
class FakeWrapped:
|
|
def __init__(self, result):
|
|
self.result = result
|
|
|
|
|
|
class FakeGoalHandle:
|
|
def __init__(self, accepted=True, result=None):
|
|
self.accepted = accepted
|
|
self._result = result or FakeResult()
|
|
|
|
def get_result_async(self):
|
|
return FakeFuture(FakeWrapped(self._result))
|
|
|
|
|
|
class FakeActionClient:
|
|
def __init__(self, *, available=True, accepted=True, result=None, timeout=False):
|
|
self.available = available
|
|
self.accepted = accepted
|
|
self.result = result
|
|
self.timeout = timeout
|
|
self.requests = []
|
|
|
|
def wait_for_server(self, timeout_sec=0.0):
|
|
return self.available
|
|
|
|
def send_goal_async(self, request):
|
|
self.requests.append(request)
|
|
if self.timeout:
|
|
return FakeFuture(None, done=False)
|
|
return FakeFuture(FakeGoalHandle(self.accepted, self.result))
|
|
|
|
|
|
class FakeService:
|
|
def __init__(self, *, available=True, success=True, message='ok', timeout=False):
|
|
self.available = available
|
|
self.success = success
|
|
self.message = message
|
|
self.timeout = timeout
|
|
self.requests = []
|
|
|
|
def wait_for_service(self, timeout_sec=0.0):
|
|
return self.available
|
|
|
|
def call_async(self, request):
|
|
self.requests.append(request)
|
|
if self.timeout:
|
|
return FakeFuture(None, done=False)
|
|
return FakeFuture(FakeResult(self.success, self.message))
|
|
|
|
|
|
class StopService(FakeService):
|
|
def __init__(self):
|
|
super().__init__(success=True, message='stopped')
|
|
self.calls = 0
|
|
|
|
def call_async(self, request):
|
|
self.calls += 1
|
|
return super().call_async(request)
|
|
|
|
|
|
def geometry():
|
|
return load_geometry(GEOMETRY)
|
|
|
|
|
|
def goal():
|
|
return Transform.from_xyz_rpy(
|
|
[0.20, 0.10, 0.90], [180.0, 0.0, 45.0], parent='world', child='tcp',
|
|
)
|
|
|
|
|
|
def make_adapter(
|
|
*,
|
|
action=None,
|
|
named=None,
|
|
stop=None,
|
|
fresh=True,
|
|
scene=True,
|
|
actual=None,
|
|
spin=None,
|
|
):
|
|
g = geometry()
|
|
action = action or FakeActionClient()
|
|
named = named or FakeService()
|
|
stop = stop or StopService()
|
|
adapter = MotionAdapter(
|
|
node=None,
|
|
geometry=g,
|
|
action_client=action,
|
|
named_client=named,
|
|
stop_client=stop,
|
|
tf_buffer=object(),
|
|
scene_check=lambda: (scene, 'scene is stale' if not scene else 'ok'),
|
|
freshness_check=lambda: (fresh, 'joint state is stale' if not fresh else 'ok'),
|
|
pose_provider=lambda: actual if actual is not None else adapter.to_base_goal(goal()),
|
|
spin_until=spin or (lambda future, timeout: future.done()),
|
|
)
|
|
return adapter, action, named, stop
|
|
|
|
|
|
def test_successful_move_populates_abc_and_verifies_actual_pose():
|
|
adapter, action, _, _ = make_adapter()
|
|
result = adapter.move_pose(goal(), planner='ptp', speed=0.2, label='test')
|
|
assert result.success, result.message
|
|
assert len(action.requests) == 1
|
|
request = action.requests[0]
|
|
expected = adapter.to_base_goal(goal())
|
|
assert (request.x, request.y, request.z) == pytest.approx(expected.xyz)
|
|
assert request.speed == pytest.approx(0.2)
|
|
assert request.planner == 'ptp'
|
|
assert request.frame_id == 'base_link'
|
|
assert result.goal.xyz == pytest.approx(expected.xyz)
|
|
|
|
|
|
def test_stale_joint_state_blocks_before_sending_and_stops():
|
|
stop = StopService()
|
|
adapter, action, _, _ = make_adapter(fresh=False, stop=stop)
|
|
result = adapter.move_pose(goal(), label='stale')
|
|
assert not result.success
|
|
assert 'stale' in result.message
|
|
assert action.requests == []
|
|
assert adapter.stopped is True
|
|
assert stop.calls == 1
|
|
|
|
|
|
def test_unsynchronized_scene_blocks_before_sending_and_stops():
|
|
adapter, action, _, _ = make_adapter(scene=False)
|
|
result = adapter.move_pose(goal(), label='scene')
|
|
assert not result.success
|
|
assert 'scene' in result.message
|
|
assert action.requests == []
|
|
assert adapter.stopped is True
|
|
|
|
|
|
def test_invalid_speed_and_planner_are_rejected_without_stopping():
|
|
adapter, action, _, _ = make_adapter()
|
|
assert not adapter.move_pose(goal(), speed=0.0).success
|
|
assert not adapter.move_pose(goal(), speed=2.0).success
|
|
assert not adapter.move_pose(goal(), planner='teleport').success
|
|
assert action.requests == []
|
|
assert adapter.stopped is False
|
|
|
|
|
|
def test_rejected_goal_stops_and_blocks_the_next_move():
|
|
action = FakeActionClient(accepted=False)
|
|
adapter, action, _, stop = make_adapter(action=action)
|
|
result = adapter.move_pose(goal(), label='rejected')
|
|
assert not result.success
|
|
assert 'rejected' in result.message
|
|
assert adapter.stopped is True
|
|
assert stop.calls == 1
|
|
count = len(action.requests)
|
|
blocked = adapter.move_pose(goal(), label='again')
|
|
assert not blocked.success
|
|
assert 'blocked' in blocked.message
|
|
assert len(action.requests) == count
|
|
|
|
|
|
def test_timeout_stops_the_robot():
|
|
adapter, _, _, stop = make_adapter(action=FakeActionClient(timeout=True))
|
|
result = adapter.move_pose(goal(), label='timeout')
|
|
assert not result.success
|
|
assert 'timed out' in result.message
|
|
assert adapter.stopped is True
|
|
assert stop.calls == 1
|
|
|
|
|
|
def test_actual_pose_out_of_tolerance_stops_the_robot():
|
|
actual = goal().compose_relaxed(Transform.from_xyz_rpy(
|
|
[0.0, 0.0, 0.05], [0, 0, 0], parent='world', child='tcp',
|
|
))
|
|
adapter, _, _, stop = make_adapter(actual=actual)
|
|
result = adapter.move_pose(goal(), label='mismatch')
|
|
assert not result.success
|
|
assert 'from the goal' in result.message
|
|
assert adapter.stopped is True
|
|
assert stop.calls == 1
|
|
|
|
|
|
def test_named_motion_uses_the_existing_service():
|
|
adapter, _, named, _ = make_adapter()
|
|
result = adapter.move_named('home', speed=0.2)
|
|
assert result.success
|
|
assert len(named.requests) == 1
|
|
assert named.requests[0].name == 'home'
|
|
assert named.requests[0].speed == pytest.approx(0.2)
|
|
|
|
|
|
def test_failed_named_motion_stops():
|
|
named = FakeService(success=False, message='unknown state')
|
|
adapter, _, _, stop = make_adapter(named=named)
|
|
result = adapter.move_named('missing')
|
|
assert not result.success
|
|
assert 'unknown state' in result.message
|
|
assert adapter.stopped is True
|
|
assert stop.calls == 1
|
|
|
|
|
|
def test_unavailable_server_is_reported_without_motion():
|
|
adapter, action, _, _ = make_adapter(action=FakeActionClient(available=False))
|
|
result = adapter.move_pose(goal(), label='no server')
|
|
assert not result.success
|
|
assert 'unavailable' in result.message
|
|
assert action.requests == []
|
|
assert adapter.stopped is False
|