"""Appliance-owned local locomotion."""
from collections.abc import Callable, Iterator
from typing import Optional
import grpc
from olo_protos.locomotion.v1 import locomotion_pb2
from olo._convert import duration_to_seconds, seconds_to_duration
from olo.errors import wrap_rpc_error
from olo.locomotion.types import (
AppliedVelocity,
LocomotionFeedback,
LocomotionOutcome,
LocomotionResult,
PlanarMotion,
)
from olo.utils.common import normalize_robot_namespace
_DEFAULT_TIMEOUT_SEC = 10.0
_DEFAULT_MOTION_TIMEOUT_SEC = 30.0
_DEFAULT_MAX_LINEAR_SPEED_MPS = 0.5
_DEFAULT_MAX_ANGULAR_SPEED_RADPS = 1.0
_DEFAULT_VELOCITY_LEASE_SEC = 0.5
_OUTCOME = {
locomotion_pb2.LOCOMOTION_OUTCOME_UNSPECIFIED: LocomotionOutcome.UNSPECIFIED,
locomotion_pb2.LOCOMOTION_OUTCOME_SUCCEEDED: LocomotionOutcome.SUCCEEDED,
locomotion_pb2.LOCOMOTION_OUTCOME_STALLED: LocomotionOutcome.STALLED,
locomotion_pb2.LOCOMOTION_OUTCOME_TIMED_OUT: LocomotionOutcome.TIMED_OUT,
locomotion_pb2.LOCOMOTION_OUTCOME_ODOM_STALE: LocomotionOutcome.ODOM_STALE,
locomotion_pb2.LOCOMOTION_OUTCOME_CANCELLED: LocomotionOutcome.CANCELLED,
locomotion_pb2.LOCOMOTION_OUTCOME_ABORTED: LocomotionOutcome.ABORTED,
}
def _motion_from_proto(motion: locomotion_pb2.PlanarMotion) -> PlanarMotion:
return PlanarMotion(x=float(motion.x), y=float(motion.y), yaw=float(motion.yaw))
def _feedback_from_proto(
feedback: locomotion_pb2.LocomotionFeedback,
) -> LocomotionFeedback:
return LocomotionFeedback(
measured=_motion_from_proto(feedback.measured),
remaining=_motion_from_proto(feedback.remaining),
elapsed=duration_to_seconds(feedback.elapsed),
)
def _result_from_proto(result: locomotion_pb2.LocomotionResult) -> LocomotionResult:
return LocomotionResult(
outcome=_OUTCOME.get(result.outcome, LocomotionOutcome.UNSPECIFIED),
reason=result.reason,
requested=_motion_from_proto(result.requested),
measured=_motion_from_proto(result.measured),
residual=_motion_from_proto(result.residual),
elapsed=duration_to_seconds(result.elapsed),
)
class _LocomotionGoal:
"""Internal handle for an accepted local locomotion-motion goal."""
def __init__(self, client: "Locomotion", goal_id: str) -> None:
self._client = client
self.id = goal_id
self._result: LocomotionResult | None = None
def wait(
self,
*,
on_feedback: Callable[[LocomotionFeedback], None] | None = None,
timeout: float | None = None,
) -> LocomotionResult:
if self._result is not None:
return self._result
for event in self._client._watch(self.id, timeout=timeout):
if isinstance(event, LocomotionFeedback):
if on_feedback is not None:
on_feedback(event)
continue
self._result = event
return event
raise TimeoutError(f"Locomotion goal {self.id} ended without a result")
def cancel(self, *, timeout: float = _DEFAULT_TIMEOUT_SEC) -> None:
self._client._cancel(self.id, timeout=timeout)
[docs]
class Locomotion:
"""Sync wrapper for local locomotion."""
[docs]
def __init__(self, session) -> None:
self._session = session
def _call(self, stub_method, request, timeout: float | None):
try:
return stub_method(request, timeout=timeout)
except grpc.RpcError as exc:
raise wrap_rpc_error(exc) from exc
[docs]
def move(
self,
motion: PlanarMotion,
*,
robot_namespace: str = "",
max_linear_speed: float | None = None,
max_angular_speed: float | None = None,
motion_timeout: float = _DEFAULT_MOTION_TIMEOUT_SEC,
cmd_vel_topic: str = "",
odom_topic: str = "",
on_feedback: Callable[[LocomotionFeedback], None] | None = None,
timeout: float = _DEFAULT_TIMEOUT_SEC,
) -> LocomotionResult:
"""Move to a planar pose relative to the starting base frame."""
request = locomotion_pb2.StartMotionRequest(
robot_namespace=normalize_robot_namespace(robot_namespace),
move=locomotion_pb2.PlanarMotion(
x=motion.x,
y=motion.y,
yaw=motion.yaw,
),
motion_timeout=seconds_to_duration(motion_timeout),
max_linear_speed=(
_DEFAULT_MAX_LINEAR_SPEED_MPS
if max_linear_speed is None
else float(max_linear_speed)
),
max_angular_speed=(
_DEFAULT_MAX_ANGULAR_SPEED_RADPS
if max_angular_speed is None
else float(max_angular_speed)
),
cmd_vel_topic=cmd_vel_topic,
odom_topic=odom_topic,
)
response = self._call(
self._session.locomotion_stub.StartMotion,
request,
timeout,
)
goal = _LocomotionGoal(self, response.goal_id)
try:
return goal.wait(on_feedback=on_feedback)
except KeyboardInterrupt:
goal.cancel(timeout=timeout)
try:
goal.wait(timeout=timeout)
except Exception:
pass
raise
[docs]
def stop(
self,
robot_namespace: str = "",
*,
cmd_vel_topic: str = "",
timeout: float = _DEFAULT_TIMEOUT_SEC,
) -> tuple[str, ...]:
"""Cancel owned locomotion and publish zero velocity."""
response = self._call(
self._session.locomotion_stub.Stop,
locomotion_pb2.StopRequest(
robot_namespace=normalize_robot_namespace(robot_namespace),
cmd_vel_topic=cmd_vel_topic,
),
timeout,
)
return tuple(response.cancelled_goal_ids)
[docs]
def send_velocity(
self,
x: float = 0.0,
yaw: float = 0.0,
*,
y: float = 0.0,
lease: float = _DEFAULT_VELOCITY_LEASE_SEC,
robot_namespace: str = "",
cmd_vel_topic: str = "",
timeout: float = _DEFAULT_TIMEOUT_SEC,
) -> AppliedVelocity:
"""Set planar velocity and refresh its appliance-owned lease."""
response = self._call(
self._session.locomotion_stub.SendVelocity,
locomotion_pb2.SendVelocityRequest(
robot_namespace=normalize_robot_namespace(robot_namespace),
velocity=locomotion_pb2.PlanarMotion(x=x, y=y, yaw=yaw),
lease=seconds_to_duration(lease),
cmd_vel_topic=cmd_vel_topic,
),
timeout,
)
return AppliedVelocity(
applied=_motion_from_proto(response.applied),
cmd_vel_topic=response.cmd_vel_topic,
)
def _watch(
self,
goal_id: str,
*,
timeout: float | None = None,
) -> Iterator[LocomotionFeedback | LocomotionResult]:
request = locomotion_pb2.WatchMotionRequest(goal_id=goal_id)
try:
stream = self._session.locomotion_stub.WatchMotion(
request, timeout=timeout
)
for response in stream:
kind = response.WhichOneof("event")
if kind == "feedback":
yield _feedback_from_proto(response.feedback)
elif kind == "result":
yield _result_from_proto(response.result)
return
except grpc.RpcError as exc:
raise wrap_rpc_error(exc) from exc
def _cancel(
self,
goal_id: str,
*,
timeout: float = _DEFAULT_TIMEOUT_SEC,
) -> None:
self._call(
self._session.locomotion_stub.CancelMotion,
locomotion_pb2.CancelMotionRequest(goal_id=goal_id),
timeout,
)
[docs]
def handle(self, namespace: Optional[str] = None) -> "LocomotionHandle":
"""Return a namespace-scoped locomotion-motion handle."""
return LocomotionHandle(self, self._session.resolve_namespace(namespace))
[docs]
class LocomotionHandle:
"""Namespace-scoped local locomotion."""
[docs]
def __init__(self, client: Locomotion, namespace: str) -> None:
self._client = client
self.namespace = namespace
[docs]
def move(
self,
motion: PlanarMotion,
*,
max_linear_speed: float | None = None,
max_angular_speed: float | None = None,
motion_timeout: float = _DEFAULT_MOTION_TIMEOUT_SEC,
cmd_vel_topic: str = "",
odom_topic: str = "",
on_feedback: Callable[[LocomotionFeedback], None] | None = None,
timeout: float = _DEFAULT_TIMEOUT_SEC,
) -> LocomotionResult:
"""Move to a planar pose relative to the starting base frame."""
return self._client.move(
motion,
robot_namespace=self.namespace,
max_linear_speed=max_linear_speed,
max_angular_speed=max_angular_speed,
motion_timeout=motion_timeout,
cmd_vel_topic=cmd_vel_topic,
odom_topic=odom_topic,
on_feedback=on_feedback,
timeout=timeout,
)
[docs]
def stop(
self,
*,
cmd_vel_topic: str = "",
timeout: float = _DEFAULT_TIMEOUT_SEC,
) -> tuple[str, ...]:
"""Cancel owned locomotion and publish zero velocity."""
return self._client.stop(
self.namespace,
cmd_vel_topic=cmd_vel_topic,
timeout=timeout,
)
[docs]
def send_velocity(
self,
x: float = 0.0,
yaw: float = 0.0,
*,
y: float = 0.0,
lease: float = _DEFAULT_VELOCITY_LEASE_SEC,
cmd_vel_topic: str = "",
timeout: float = _DEFAULT_TIMEOUT_SEC,
) -> AppliedVelocity:
"""Set planar velocity and refresh its appliance-owned lease."""
return self._client.send_velocity(
x,
yaw,
y=y,
lease=lease,
robot_namespace=self.namespace,
cmd_vel_topic=cmd_vel_topic,
timeout=timeout,
)
__all__ = [
"AppliedVelocity",
"Locomotion",
"LocomotionHandle",
"LocomotionFeedback",
"LocomotionOutcome",
"LocomotionResult",
"PlanarMotion",
]