cristhian
fdabf7e241
Replaces the command/status JSON pair with dora's own service pattern: one request, one response, correlated by the request_id dora puts in the message metadata. Motions take a wait flag, and their lifecycle is published as the arm's state on motion_state rather than as a per-goal stream — the arm is one resource, so a new motion preempts the one in play and a state describes that exactly. motion_state is keyed by request_id and publishes accepted before the control thread reaches the motion. Both exist to close one race: a caller polling a bare moving/idle flag right after its request would read "not moving" and conclude the motion had already finished. Three threads, because the SDK blocks. The control thread carries out one job at a time and may block; the reporter thread only ever reads the report cache, so telemetry keeps flowing through a ten-second move; the stopper thread exists because a stop fits nowhere else — it must not queue behind the motion it aborts, and emergency_stop re-commands the controller for up to three seconds, which is far too long to stop routing a dataflow. The event loop never calls anything that can block, get_state included: that one is allowed there only because it reads the cache. The node is built before the arm threads start, so INIT_MODE cannot move a robot before the process has joined the dataflow, and a restart leaves the arm where it is rather than driving it home unasked. Shutdown halts the arm first — a motion lives in the controller, and dropping the connection does not abort it.
92 lines
3.3 KiB
Python
92 lines
3.3 KiB
Python
"""Reading arguments out of the Arrow row that carries a request.
|
|
|
|
Every request arrives as a one-row Arrow struct, and each service
|
|
brings its own schema — dora allows that on an input carrying a correlation id,
|
|
so nothing here assumes a fixed set of fields. `as_py()` hands back a plain dict
|
|
and these helpers read it, naming what was missing or unusable rather than
|
|
letting a `None` reach the arm.
|
|
"""
|
|
|
|
from typing import Any
|
|
|
|
from dora_ulite6.pose import RobotPose
|
|
|
|
|
|
class PayloadError(Exception):
|
|
"""The request could not be understood, so nothing was asked of the arm."""
|
|
|
|
|
|
def read_row(value) -> dict[str, Any]:
|
|
"""Return the single struct row of ``value`` as a dict.
|
|
|
|
Raises
|
|
------
|
|
PayloadError
|
|
The message was empty, or was not a struct this node can read.
|
|
"""
|
|
if value is None or len(value) == 0:
|
|
raise PayloadError("the message carries no data")
|
|
row = value[0].as_py()
|
|
if not isinstance(row, dict):
|
|
raise PayloadError(f"expected a struct with named fields, got {type(row).__name__}")
|
|
return row
|
|
|
|
|
|
def name_of(row: dict, field: str, known) -> str:
|
|
"""Return the operation ``row`` names, or raise listing the ones that exist."""
|
|
name = row.get(field)
|
|
if not isinstance(name, str) or name not in known:
|
|
raise PayloadError(f"unknown {field} {name!r}; available: {', '.join(sorted(known))}")
|
|
return name
|
|
|
|
|
|
def number(row: dict, field: str) -> float:
|
|
"""Return a required numeric field."""
|
|
if field not in row or row[field] is None:
|
|
raise PayloadError(f"missing `{field}`")
|
|
try:
|
|
return float(row[field])
|
|
except (TypeError, ValueError):
|
|
raise PayloadError(f"`{field}` must be a number, not {row[field]!r}") from None
|
|
|
|
|
|
def optional_number(row: dict, field: str, default: float | None) -> float | None:
|
|
"""Return a numeric field, or ``default`` when it is absent or null."""
|
|
if field not in row or row[field] is None:
|
|
return default
|
|
try:
|
|
return float(row[field])
|
|
except (TypeError, ValueError):
|
|
raise PayloadError(f"`{field}` must be a number, not {row[field]!r}") from None
|
|
|
|
|
|
def flag(row: dict, field: str, default: bool) -> bool:
|
|
"""Return a boolean field, or ``default`` when it is absent or null."""
|
|
value = row.get(field)
|
|
return default if value is None else bool(value)
|
|
|
|
|
|
def numbers(row: dict, field: str, count: int | None = None) -> list[float]:
|
|
"""Return a required list of numbers, optionally of an exact length."""
|
|
values = row.get(field)
|
|
if not isinstance(values, list) or not values:
|
|
raise PayloadError(f"`{field}` must be a non-empty list of numbers")
|
|
if count is not None and len(values) != count:
|
|
raise PayloadError(f"`{field}` must hold {count} numbers, got {len(values)}")
|
|
try:
|
|
return [float(value) for value in values]
|
|
except (TypeError, ValueError):
|
|
raise PayloadError(f"`{field}` must hold only numbers") from None
|
|
|
|
|
|
def pose(row: dict) -> RobotPose:
|
|
"""Read a tool-tip pose, defaulting the orientation to tool-down."""
|
|
return RobotPose(
|
|
x=number(row, "x"),
|
|
y=number(row, "y"),
|
|
z=number(row, "z"),
|
|
roll=optional_number(row, "roll", 180.0),
|
|
pitch=optional_number(row, "pitch", 0.0),
|
|
yaw=optional_number(row, "yaw", 0.0),
|
|
)
|