dora-teleop/dora_teleop/ui.py

"""Translation between the web console and the dataflow.

The console was written against the monolithic repo's REST API and reads a
telemetry snapshot by key. Those keys are the contract here: `app.js` does
`t.owner === "web"` and `t.estop` directly, so renaming one breaks the page
silently. They are reproduced rather than improved for that reason.

The two vocabularies do not line up, which is most of what this file is for. The
arm node reports `connecting|ready|moving|recovering|unavailable`; the console
draws `idle|moving|jogging|freedrive|estop|stopped|disconnected`. Neither is
wrong — one describes a driver's lifecycle, the other an operator's screen — and
:func:`arm_state` is where the two are reconciled.
"""

from dataclasses import dataclass, field

# What the arm node publishes on `robot_state`.
CONNECTING = "connecting"
READY = "ready"
MOVING = "moving"
RECOVERING = "recovering"
UNAVAILABLE = "unavailable"

# What the console draws.
UI_IDLE = "idle"
UI_MOVING = "moving"
UI_JOGGING = "jogging"
UI_FREEDRIVE = "freedrive"
UI_ESTOP = "estop"
UI_STOPPED = "stopped"
UI_DISCONNECTED = "disconnected"

# An arm the node cannot speak for is `disconnected` whatever the reason, because
# that is the only thing an operator can act on. `recovering` is its own case:
# the arm is there and answering, it just will not move yet.
ARM_STATE = {
    CONNECTING: UI_DISCONNECTED,
    UNAVAILABLE: UI_DISCONNECTED,
    RECOVERING: UI_STOPPED,
    READY: UI_IDLE,
    MOVING: UI_MOVING,
}

POSE_KEYS = ("x", "y", "z", "roll", "pitch", "yaw")


@dataclass
class Command:
    """One thing the console asked for."""

    type: str
    # Normalised, `[-1, 1]` per degree of freedom — a fraction of full speed,
    # exactly what a stick reports. Not a velocity: the console does not know
    # the robot's units and must not have to.
    axes: list[float] = field(default_factory=list)
    action: str = ""
    preempt: bool = False
    enabled: bool = False
    row: dict = field(default_factory=dict)


def command_of(payload: dict) -> Command | None:
    """Read one console message, or None if it is not one we serve.

    A message we do not recognise is dropped rather than refused. The console
    and this node version independently, and a page from a newer build saying
    something we have not learned yet must not take the arm down.
    """
    if not isinstance(payload, dict):
        return None
    kind = payload.get("type")
    if not isinstance(kind, str):
        return None

    if kind == "jog":
        axes = payload.get("axes") or []
        if len(axes) != 6 or not all(isinstance(v, (int, float)) for v in axes):
            return None
        return Command(type="jog", axes=[float(v) for v in axes])
    if kind in ("jog_stop", "estop", "reset", "home"):
        return Command(type=kind)
    if kind == "control":
        return Command(
            type="control",
            action=str(payload.get("action", "")),
            preempt=bool(payload.get("preempt", False)),
        )
    if kind == "gripper":
        return Command(type="gripper", enabled=bool(payload.get("closed", False)))
    if kind == "freedrive":
        return Command(type="freedrive", enabled=bool(payload.get("enabled", False)))
    if kind == "move":
        row = {key: payload[key] for key in POSE_KEYS if isinstance(payload.get(key), (int, float))}
        if not all(key in row for key in ("x", "y", "z")):
            return None
        if isinstance(payload.get("speed"), (int, float)):
            row["speed"] = float(payload["speed"])
        return Command(type="move", row=row)
    return None


@dataclass
class ArmView:
    """What this node currently believes about the arm.

    Assembled from four places — `robot_state`, `tcp_pose`, `joint_pose` and the
    answers to `get_state` — because no single one of them carries everything
    the console draws. Whatever has not arrived yet stays at its default, so a
    page that loads before the arm is reachable renders as disconnected rather
    than as a stale pose.
    """

    state: str = CONNECTING
    pose: list[float] = field(default_factory=list)
    joints: list[float] = field(default_factory=list)
    connected: bool = False
    ready: bool = False
    tool_engaged: bool = False
    freedrive_on: bool = False
    estop: bool = False

    def take_get_state(self, row: dict) -> None:
        """Absorb a `get_state` answer, which carries what telemetry does not."""
        self.connected = bool(row.get("connected", False))
        self.ready = bool(row.get("ready", False))
        self.tool_engaged = bool(row.get("tool_engaged", False))
        self.freedrive_on = bool(row.get("freedrive", False))


def arm_state(view: ArmView, jogging: bool) -> str:
    """Return the state the console should draw.

    Order matters, and it is the order of what an operator most needs to know.
    A latched e-stop wins over everything: the arm may well read `ready`, and
    saying so would invite somebody to push it. Freedrive outranks idle because
    an arm that can be pushed by hand is not the same as one holding station.
    """
    if view.estop:
        return UI_ESTOP
    if view.state in (CONNECTING, UNAVAILABLE):
        return ARM_STATE[view.state]
    if view.freedrive_on:
        return UI_FREEDRIVE
    if jogging and view.state == MOVING:
        return UI_JOGGING
    return ARM_STATE.get(view.state, UI_DISCONNECTED)


def snapshot(view: ArmView, owner: str | None, jogging: bool, timestamp: float) -> dict:
    """Build the telemetry object the console reads.

    Every key here is read by name in `app.js`, so this and the page change
    together. The names are the robot-agnostic ones: `gripper` rather than
    `vacuum`, because a page that says vacuum only fits a robot with one on it,
    and `units` so a page can label a pose without guessing.
    """
    return {
        "type": "telemetry",
        "ts": timestamp,
        "connected": view.connected,
        "ready": view.ready,
        "arm_state": arm_state(view, jogging),
        "owner": owner or "none",
        "estop": view.estop,
        "gripper": view.tool_engaged,
        "freedrive": view.freedrive_on,
        "position": dict(zip(POSE_KEYS, view.pose, strict=False)) if view.pose else None,
        "joints": list(view.joints),
        # What `position` is in, so the page labels it rather than assuming. The
        # telemetry ports carry whatever the robot publishes; only the command
        # path is normalised.
        "units": {"position": "mm", "rotation": "deg"},
    }


def error(message: str) -> dict:
    """Build the message the console shows as a toast."""
    return {"type": "error", "detail": message}