dora-ulite6/tests/conftest.py

"""Shared fakes. Nothing here needs a robot, a dataflow or a daemon."""

import time

import pytest

from dora_ulite6.pose import RobotPose

TIMEOUT = 2.0


class FakeArm:
    """Stands in for the driver: records what it was asked and answers instantly.

    ``block`` lets a test hold a motion open the way a real one does, which is
    the only way to observe feedback and cancellation at all.
    """

    def __init__(self, config):
        self.config = config
        self.calls = []
        self.ready = True
        self.tool_engaged = False
        self.freedrive_on = False
        self.pose = RobotPose(300, 0, 250)
        self.joints = [0.0] * 6
        self.read_error = None
        self.move_error = None
        self.command_error = None
        self.recover_error = None
        self.block = None
        self.stopped = False
        self.connected = True

    # --- reads ---

    def is_connected(self):
        return self.connected

    def connect(self):
        self.calls.append("connect")
        return self.connected

    def is_ready(self):
        return self.ready

    def read_report(self):
        # A report cache that cannot speak for the arm returns nothing, the way
        # the real one does when the link is down.
        if self.read_error:
            return None
        return list(self.joints), self.pose

    def get_joint_angles(self):
        if self.read_error:
            raise self.read_error
        return list(self.joints)

    def get_current_position(self):
        if self.read_error:
            raise self.read_error
        return self.pose

    # --- motion ---

    def _move(self, what):
        self.calls.append(what)
        if self.block is not None:
            # Hold the "motion" open until the test releases it or stops the arm.
            deadline = time.monotonic() + TIMEOUT
            while not self.block.is_set() and not self.stopped and time.monotonic() < deadline:
                time.sleep(0.005)
        if self.stopped:
            from dora_ulite6.errors import MotionInterruptedError

            raise MotionInterruptedError("aborted by stop")
        if self.move_error:
            raise self.move_error

    def go_home(self):
        self._move("go_home")

    def home_and_clear(self):
        self._move("home_and_clear")

    def move_to(self, pose, speed=None):
        self._move(("move_to", pose.as_list(), speed))

    def set_joint_angles(self, angles, speed=None):
        self._move(("set_joint_angles", angles, speed))

    def set_cartesian_velocity(self, velocity):
        self.calls.append(("jog", velocity))

    def stop(self):
        self.calls.append("stop")
        self.stopped = True

    # --- the rest ---

    def clear(self):
        self.calls.append("clear")
        self.stopped = False

    def recover(self):
        self.calls.append("recover")
        if self.recover_error:
            raise self.recover_error
        self.read_error = None

    def engage_tool(self):
        self.calls.append("engage_tool")
        self.tool_engaged = True

    def release_tool(self):
        self.calls.append("release_tool")
        self.tool_engaged = False

    def set_freedrive(self, enabled):
        self.calls.append(("set_freedrive", enabled))
        if self.command_error:
            raise self.command_error
        self.freedrive_on = enabled


class RecordingNode:
    """A dora node that records outputs *with* their metadata.

    `dora.testing.MockNode` drops the metadata dict on `send_output`, and the
    metadata is precisely what this node's service and action patterns are built
    on — the request_id and goal_id a caller correlates by. So the tests record
    it themselves.
    """

    def __init__(self, events=()):
        self._events = iter(list(events))
        self.sent = []

    def is_restart(self):
        return False

    def __iter__(self):
        return self

    def __next__(self):
        return next(self._events)

    def send_output(self, output_id, data, metadata=None):
        self.sent.append((output_id, data, dict(metadata or {})))

    def outputs(self, output_id):
        """Return every message sent on ``output_id`` as (row, metadata) pairs."""
        return [(data[0].as_py(), metadata) for name, data, metadata in self.sent if name == output_id]


def input_event(event_id, value, metadata=None):
    """Build the event dict dora hands a node for one input."""
    return {"type": "INPUT", "id": event_id, "value": value, "metadata": dict(metadata or {})}


def wait_for(predicate):
    """Poll until ``predicate`` holds, or fail the test rather than hang it."""
    deadline = time.monotonic() + TIMEOUT
    while time.monotonic() < deadline:
        result = predicate()
        if result:
            return result
        time.sleep(0.005)
    pytest.fail("the control threads never reached the expected state")