feat(experement): add guarded motion adapter with pose verification
This commit is contained in:
Даниил Грабарь
@@ -0,0 +1,377 @@
|
||||
"""Safe wrapper around the existing ``cobot/move_to_pose`` motion server.
|
||||
|
||||
Every call performs the same guard sequence:
|
||||
fresh joint state -> synchronized planning scene -> available server ->
|
||||
accepted goal -> successful result -> measured tcp pose. Any failure stops
|
||||
the robot and blocks all later motion.
|
||||
"""
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
import time
|
||||
from dataclasses import dataclass
|
||||
from typing import Any, Callable
|
||||
|
||||
import numpy as np
|
||||
from geometry_msgs.msg import PoseStamped
|
||||
from rclpy.action import ActionClient
|
||||
from rclpy.duration import Duration
|
||||
from rclpy.node import Node
|
||||
from rclpy.time import Time
|
||||
from scipy.spatial.transform import Rotation
|
||||
from sensor_msgs.msg import JointState
|
||||
from std_srvs.srv import Trigger
|
||||
import tf2_ros
|
||||
|
||||
from iiwa_msgs.action import MoveToPose
|
||||
from iiwa_msgs.srv import MoveToNamedPose
|
||||
|
||||
from .geometry import GeometrySpec, Transform
|
||||
|
||||
|
||||
MOVE_ACTION = '/cobot/move_to_pose'
|
||||
NAMED_SERVICE = '/cobot/move_to_named'
|
||||
STOP_SERVICE = '/cobot/stop'
|
||||
PLANNERS = ('ptp', 'lin', 'ompl')
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class MotionResult:
|
||||
success: bool
|
||||
message: str
|
||||
label: str
|
||||
sent: bool = False
|
||||
goal: Transform | None = None
|
||||
actual: Transform | None = None
|
||||
duration_sec: float = 0.0
|
||||
|
||||
|
||||
def transform_from_pose(message, parent: str, child: str) -> Transform:
|
||||
orientation = message.orientation
|
||||
matrix = Rotation.from_quat([
|
||||
orientation.x, orientation.y, orientation.z, orientation.w,
|
||||
]).as_matrix()
|
||||
return Transform(
|
||||
xyz=(
|
||||
float(message.position.x),
|
||||
float(message.position.y),
|
||||
float(message.position.z),
|
||||
),
|
||||
rotation=tuple(tuple(float(value) for value in row) for row in matrix),
|
||||
parent=parent,
|
||||
child=child,
|
||||
)
|
||||
|
||||
|
||||
def transform_from_tf(message, parent: str, child: str) -> Transform:
|
||||
translation = message.transform.translation
|
||||
rotation = message.transform.rotation
|
||||
matrix = Rotation.from_quat([
|
||||
rotation.x, rotation.y, rotation.z, rotation.w,
|
||||
]).as_matrix()
|
||||
return Transform(
|
||||
xyz=(
|
||||
float(translation.x),
|
||||
float(translation.y),
|
||||
float(translation.z),
|
||||
),
|
||||
rotation=tuple(tuple(float(value) for value in row) for row in matrix),
|
||||
parent=parent,
|
||||
child=child,
|
||||
)
|
||||
|
||||
|
||||
def _pose_distance(first: Transform, second: Transform) -> tuple[float, float]:
|
||||
distance = float(np.linalg.norm(first.position - second.position))
|
||||
cosine = (np.trace(first.matrix.T @ second.matrix) - 1.0) / 2.0
|
||||
angle = float(np.arccos(max(-1.0, min(1.0, cosine))))
|
||||
return distance, angle
|
||||
|
||||
|
||||
class MotionAdapter:
|
||||
"""Guarded motion client used by every reference operation."""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
node: Node,
|
||||
geometry: GeometrySpec,
|
||||
*,
|
||||
action_client: Any | None = None,
|
||||
named_client: Any | None = None,
|
||||
stop_client: Any | None = None,
|
||||
tf_buffer: Any | None = None,
|
||||
scene_check: Callable[[], tuple[bool, str]] | None = None,
|
||||
pose_provider: Callable[[], Transform | None] | None = None,
|
||||
freshness_check: Callable[[], tuple[bool, str]] | None = None,
|
||||
spin_until: Callable[[Any, float], bool] | None = None,
|
||||
position_tolerance: float | None = None,
|
||||
orientation_tolerance: float | None = None,
|
||||
command_timeout_sec: float = 90.0,
|
||||
settle_time_sec: float = 0.5,
|
||||
) -> None:
|
||||
self.node = node
|
||||
self.geometry = geometry
|
||||
self.action_client = action_client or ActionClient(
|
||||
node, MoveToPose, MOVE_ACTION,
|
||||
)
|
||||
self.named_client = named_client or node.create_client(
|
||||
MoveToNamedPose, NAMED_SERVICE,
|
||||
)
|
||||
self.stop_client = stop_client or node.create_client(
|
||||
Trigger, STOP_SERVICE,
|
||||
)
|
||||
self.tf_buffer = tf_buffer
|
||||
self._tf_listener = None
|
||||
if tf_buffer is None:
|
||||
self.tf_buffer = tf2_ros.Buffer()
|
||||
self._tf_listener = tf2_ros.TransformListener(self.tf_buffer, node)
|
||||
self._scene_check = scene_check
|
||||
self._pose_provider = pose_provider
|
||||
self._freshness_check = freshness_check
|
||||
self._spin_until = spin_until or self._default_spin_until
|
||||
self.position_tolerance = (
|
||||
geometry.position_tolerance
|
||||
if position_tolerance is None else float(position_tolerance)
|
||||
)
|
||||
self.orientation_tolerance = (
|
||||
geometry.orientation_tolerance_rad
|
||||
if orientation_tolerance is None else float(orientation_tolerance)
|
||||
)
|
||||
self.command_timeout_sec = float(command_timeout_sec)
|
||||
self.settle_time_sec = max(0.0, float(settle_time_sec))
|
||||
self.stopped = False
|
||||
self.stop_reason: str | None = None
|
||||
self._last_joint_time = None
|
||||
if freshness_check is None:
|
||||
node.create_subscription(JointState, '/joint_states', self._on_joint_state, 10)
|
||||
|
||||
# ------------------------------------------------------------------ freshness
|
||||
def _on_joint_state(self, message: JointState) -> None:
|
||||
del message
|
||||
self._last_joint_time = self.node.get_clock().now()
|
||||
|
||||
def _default_freshness(self) -> tuple[bool, str]:
|
||||
if self._last_joint_time is None:
|
||||
return False, 'no /joint_states received yet'
|
||||
age = (self.node.get_clock().now() - self._last_joint_time).nanoseconds / 1e9
|
||||
if age > 1.0:
|
||||
return False, f'/joint_states is stale ({age:.2f} s old)'
|
||||
if self.tf_buffer is not None:
|
||||
try:
|
||||
self.tf_buffer.lookup_transform('base_link', 'tcp', Time())
|
||||
except Exception as error:
|
||||
return False, f'tcp transform is unavailable: {error}'
|
||||
return True, 'ok'
|
||||
|
||||
def _fresh(self) -> tuple[bool, str]:
|
||||
if self._freshness_check is None:
|
||||
return self._default_freshness()
|
||||
return self._freshness_check()
|
||||
|
||||
def _scene(self) -> tuple[bool, str]:
|
||||
if self._scene_check is None:
|
||||
return False, 'planning scene bridge is not configured'
|
||||
return self._scene_check()
|
||||
|
||||
# ------------------------------------------------------------------ transforms
|
||||
def _default_spin_until(self, future, timeout_sec: float) -> bool:
|
||||
import rclpy
|
||||
|
||||
rclpy.spin_until_future_complete(
|
||||
self.node, future, timeout_sec=timeout_sec,
|
||||
)
|
||||
return future.done()
|
||||
|
||||
def world_to_base(self) -> Transform:
|
||||
# The URDF ships a ``world_base_joint`` with an identity origin, so
|
||||
# robot_state_publisher always provides a synthetic world->base_link
|
||||
# transform at zero. The physical Webots placement comes from the
|
||||
# launch setting and is stored in GeometrySpec.world_to_base.
|
||||
return self.geometry.world_to_base
|
||||
|
||||
def update_world_to_base(self, transform: Transform) -> None:
|
||||
self.geometry = GeometrySpec(
|
||||
**{**self.geometry.__dict__, 'world_to_base': transform}
|
||||
)
|
||||
|
||||
def actual_tcp(self) -> Transform | None:
|
||||
if self._pose_provider is not None:
|
||||
return self._pose_provider()
|
||||
if self.tf_buffer is None:
|
||||
return None
|
||||
try:
|
||||
message = self.tf_buffer.lookup_transform('base_link', 'tcp', Time())
|
||||
return transform_from_tf(message, 'base_link', 'tcp')
|
||||
except Exception:
|
||||
return None
|
||||
|
||||
def to_base_goal(self, goal: Transform) -> Transform:
|
||||
return self.world_to_base().inverse().compose_relaxed(goal)
|
||||
|
||||
def _abc(self, goal: Transform) -> tuple[float, float, float]:
|
||||
euler = Rotation.from_matrix(goal.matrix).as_euler('ZYX')
|
||||
return float(euler[0]), float(euler[1]), float(euler[2])
|
||||
|
||||
# ------------------------------------------------------------------ guards
|
||||
def _guard(self, label: str, speed: float, planner: str) -> MotionResult | None:
|
||||
if self.stopped:
|
||||
return MotionResult(
|
||||
False,
|
||||
f'motion is blocked after stop: {self.stop_reason}',
|
||||
label,
|
||||
)
|
||||
if not math.isfinite(float(speed)) or not 0.0 < float(speed) <= 1.0:
|
||||
return MotionResult(False, f'invalid speed {speed!r}', label)
|
||||
if planner not in PLANNERS:
|
||||
return MotionResult(False, f'unknown planner {planner!r}', label)
|
||||
fresh, message = self._fresh()
|
||||
if not fresh:
|
||||
self.stop(message)
|
||||
return MotionResult(False, message, label)
|
||||
synchronized, message = self._scene()
|
||||
if not synchronized:
|
||||
self.stop(message)
|
||||
return MotionResult(False, message, label)
|
||||
return None
|
||||
|
||||
# ------------------------------------------------------------------ motion
|
||||
def move_pose(
|
||||
self,
|
||||
goal: Transform,
|
||||
planner: str = 'ptp',
|
||||
speed: float = 0.2,
|
||||
label: str = '',
|
||||
) -> MotionResult:
|
||||
label = label or 'move_pose'
|
||||
blocked = self._guard(label, speed, planner)
|
||||
if blocked is not None:
|
||||
return blocked
|
||||
if not self.action_client.wait_for_server(timeout_sec=5.0):
|
||||
return MotionResult(False, f'{MOVE_ACTION} is unavailable', label)
|
||||
|
||||
base_goal = self.to_base_goal(goal)
|
||||
a, b, c = self._abc(base_goal)
|
||||
request = MoveToPose.Goal()
|
||||
request.x, request.y, request.z = (float(value) for value in base_goal.xyz)
|
||||
request.a, request.b, request.c = a, b, c
|
||||
request.speed = float(speed)
|
||||
request.planner = planner
|
||||
request.frame_id = 'base_link'
|
||||
|
||||
started = time.monotonic()
|
||||
future = self.action_client.send_goal_async(request)
|
||||
if not self._spin_until(future, self.command_timeout_sec):
|
||||
return self._fail(label, 'goal response timed out', base_goal, started)
|
||||
try:
|
||||
goal_handle = future.result()
|
||||
except Exception as error:
|
||||
return self._fail(label, f'goal call failed: {error}', base_goal, started)
|
||||
if goal_handle is None or not goal_handle.accepted:
|
||||
return self._fail(label, 'motion goal was rejected', base_goal, started)
|
||||
|
||||
result_future = goal_handle.get_result_async()
|
||||
if not self._spin_until(result_future, self.command_timeout_sec):
|
||||
return self._fail(label, 'motion result timed out', base_goal, started)
|
||||
try:
|
||||
wrapped = result_future.result()
|
||||
result = wrapped.result
|
||||
except Exception as error:
|
||||
return self._fail(label, f'motion result failed: {error}', base_goal, started)
|
||||
if not result.success:
|
||||
return self._fail(
|
||||
label, result.message or 'motion server reported failure',
|
||||
base_goal, started,
|
||||
)
|
||||
|
||||
self._settle()
|
||||
actual = self.actual_tcp()
|
||||
if actual is None:
|
||||
return self._fail(label, 'actual tcp pose is unavailable after motion', base_goal, started)
|
||||
distance, angle = _pose_distance(actual, base_goal)
|
||||
if distance > self.position_tolerance or angle > self.orientation_tolerance:
|
||||
return self._fail(
|
||||
label,
|
||||
f'actual tcp is {distance:.4f} m / {angle:.4f} rad from the goal',
|
||||
base_goal,
|
||||
started,
|
||||
actual=actual,
|
||||
)
|
||||
return MotionResult(
|
||||
True, 'motion completed', label, True, base_goal, actual,
|
||||
time.monotonic() - started,
|
||||
)
|
||||
|
||||
def move_named(self, name: str, speed: float = 0.2, label: str = '') -> MotionResult:
|
||||
label = label or f'move_named({name})'
|
||||
blocked = self._guard(label, speed, 'ptp')
|
||||
if blocked is not None:
|
||||
return blocked
|
||||
if not self.named_client.wait_for_service(timeout_sec=5.0):
|
||||
return MotionResult(False, f'{NAMED_SERVICE} is unavailable', label)
|
||||
|
||||
request = MoveToNamedPose.Request()
|
||||
request.name = str(name)
|
||||
request.speed = float(speed)
|
||||
request.accel_scale = float(speed)
|
||||
started = time.monotonic()
|
||||
future = self.named_client.call_async(request)
|
||||
if not self._spin_until(future, self.command_timeout_sec):
|
||||
return self._fail(label, 'named motion timed out', None, started)
|
||||
try:
|
||||
response = future.result()
|
||||
except Exception as error:
|
||||
return self._fail(label, f'named motion failed: {error}', None, started)
|
||||
if not response.success:
|
||||
return self._fail(
|
||||
label, response.message or 'named motion failed', None, started,
|
||||
)
|
||||
self._settle()
|
||||
return MotionResult(
|
||||
True, response.message or 'motion completed', label,
|
||||
True, None, self.actual_tcp(), time.monotonic() - started,
|
||||
)
|
||||
|
||||
def _settle(self) -> None:
|
||||
if self.settle_time_sec <= 0.0 or self.node is None:
|
||||
return
|
||||
import rclpy
|
||||
|
||||
end = time.monotonic() + self.settle_time_sec
|
||||
while time.monotonic() < end:
|
||||
rclpy.spin_once(
|
||||
self.node,
|
||||
timeout_sec=min(0.05, max(0.0, end - time.monotonic())),
|
||||
)
|
||||
|
||||
def _fail(
|
||||
self,
|
||||
label: str,
|
||||
message: str,
|
||||
goal: Transform | None,
|
||||
started: float,
|
||||
*,
|
||||
actual: Transform | None = None,
|
||||
) -> MotionResult:
|
||||
self.stop(message)
|
||||
return MotionResult(
|
||||
False, message, label, True, goal, actual,
|
||||
time.monotonic() - started,
|
||||
)
|
||||
|
||||
def stop(self, reason: str) -> bool:
|
||||
self.stopped = True
|
||||
if self.stop_reason is None:
|
||||
self.stop_reason = reason
|
||||
try:
|
||||
if self.stop_client.wait_for_service(timeout_sec=1.0):
|
||||
future = self.stop_client.call_async(Trigger.Request())
|
||||
self._spin_until(future, 5.0)
|
||||
return bool(future.done() and future.result().success)
|
||||
except Exception:
|
||||
pass
|
||||
return False
|
||||
|
||||
def reset_stop(self) -> None:
|
||||
"""Only for a new run; never used to resume after a failed motion."""
|
||||
self.stopped = False
|
||||
self.stop_reason = None
|
||||
@@ -0,0 +1,235 @@
|
||||
"""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
|
||||
Reference in New Issue
Block a user