dora-ulite6/dora_ulite6/services.py

"""Every operation the node serves, and what each one does to the arm.

A service is one request and one response, correlated by the `request_id` dora
puts in the message metadata. That is the node's whole control surface: there is
no second protocol for the slow ones.

Two groups behave differently enough to be worth naming here:

- The motions in :data:`MOVES_THE_ARM` take as long as the arm does. A caller
  chooses with `wait` whether to be held until the arm stops or answered at
  once; either way the motion's progress is on `motion_state`. A new motion
  preempts one already running, so there is never more than one in play.
- The services in :data:`IMMEDIATE` are run by the event loop rather than queued,
  because queued they would sit behind a motion — which is exactly when they are
  wanted. Putting one there is a promise that it never blocks: `get_state` reads
  the controller's report cache rather than the wire, which is what makes it
  answerable *during* a motion.

`stop` is a service to a caller but has no handler here at all. It cannot queue
(it exists to abort what is at the head of the queue) and it cannot run on the
event loop either: the SDK's `emergency_stop` re-commands the controller for up
to three seconds, and three seconds is far too long to stop routing a dataflow.
It gets a thread of its own — see :mod:`dora_ulite6.control`.
"""

from typing import Any

from dora_ulite6 import payload
from dora_ulite6.driver import ULite6
from dora_ulite6.pose import with_tool_offset

# Field naming the service in a request row.
SERVICE = "service"


def run(robot: ULite6, row: dict) -> dict[str, Any]:
    """Carry out the service ``row`` names and return the response fields.

    ``row`` must name a service with a handler, which `stop` deliberately has
    not; the event loop routes that one before it ever reaches here.

    Blocks for as long as the service takes — seconds, for a motion.

    Raises
    ------
    PayloadError
        The row named no known service, or its arguments were unusable.
    RobotUnavailableError, UnreachablePoseError, EffectorUnsupportedError
        The arm refused; the caller turns it into a failed response.
    """
    return SERVICES[payload.name_of(row, SERVICE, SERVICES)](robot, row)


# --- motions: these block, and they move a real arm ---


def _move_to(robot: ULite6, row: dict) -> dict:
    tip = payload.pose(row)
    # The request names where the tool tip should end up; the controller only
    # takes flange poses, so the tool's own reach comes back off here.
    robot.move_to(
        with_tool_offset(tip, -robot.config.tool_offset_mm),
        payload.optional_number(row, "speed", None),
    )
    return {"message": f"the tool is at ({tip.x:.0f}, {tip.y:.0f}, {tip.z:.0f} mm)"}


def _move_joints(robot: ULite6, row: dict) -> dict:
    angles = payload.numbers(row, "angles")
    robot.set_joint_angles(angles, payload.optional_number(row, "speed", None))
    return {"message": f"the joints are at {angles}"}


def _home(robot: ULite6, row: dict) -> dict:
    robot.go_home()
    return {"message": "the arm is home"}


def _home_and_clear(robot: ULite6, row: dict) -> dict:
    robot.home_and_clear()
    return {"message": "the arm was cleared and driven home"}


# --- everything that answers immediately ---


def _clear(robot: ULite6, row: dict) -> dict:
    robot.clear()
    return {"message": "the controller error is cleared"}


def _recover(robot: ULite6, row: dict) -> dict:
    robot.recover()
    return {"message": "the arm is usable again"}


def _freedrive(robot: ULite6, row: dict) -> dict:
    enabled = payload.flag(row, "enabled", True)
    robot.set_freedrive(enabled)
    return {"message": f"freedrive {'on' if enabled else 'off'}"}


def _grasp(robot: ULite6, row: dict) -> dict:
    robot.engage_tool()
    return {"message": "the tool is holding"}


def _release(robot: ULite6, row: dict) -> dict:
    robot.release_tool()
    return {"message": "the tool has let go"}


def _get_state(robot: ULite6, row: dict) -> dict:
    """Read the arm and hand back everything a caller might otherwise poll for.

    Reads the report cache, not the wire. That is what lets this run on the event
    loop and answer *during* a motion — the point of polling at all — where a
    `get_position()` round trip would block behind the move on the same socket.

    An arm the report cache cannot speak for answers `connected: false` rather
    than handing back the pose it held before the cable was pulled.
    """
    reading = robot.read_report()
    state = {
        "connected": reading is not None,
        "ready": robot.is_ready(),
        "tool_engaged": robot.tool_engaged,
        "freedrive": robot.freedrive_on,
        "pose": [],
        "joints": [],
    }
    if reading is None:
        return {"message": "not connected", **state}
    joints, flange = reading
    state["pose"] = with_tool_offset(flange, robot.config.tool_offset_mm).as_list()
    state["joints"] = joints
    return {"message": "ready" if state["ready"] else "not ready", **state}


SERVICES = {
    "move_to": _move_to,
    "move_joints": _move_joints,
    "home": _home,
    "home_and_clear": _home_and_clear,
    "clear": _clear,
    "recover": _recover,
    "freedrive": _freedrive,
    "grasp": _grasp,
    "release": _release,
    "get_state": _get_state,
}

# The services that drive the arm across the table. They are what `robot_state`
# reports as `moving`, and what a `stop` is there to interrupt.
MOVES_THE_ARM = frozenset({"move_to", "move_joints", "home", "home_and_clear"})

# Served on the event loop rather than the control thread. See the module note.
# Adding to this set is a promise that the service never blocks and is safe from
# a thread other than the control one — check the driver method before you do.
IMMEDIATE = frozenset({"get_state"})

# Named by callers and accepted, but handled by neither of the above. See the
# module note for why it has a thread to itself.
STOP = "stop"

# Every name a request may carry.
NAMES = frozenset(SERVICES) | {STOP}