cristhian
a95d804f0b
`set_cartesian_velocity` passed `duration=velocity_command_ttl_s` on every message, so each one re-armed a timed segment and the controller re-planned the same motion. On a real Lite6 that is not smooth travel: the arm moves in steps. Reducing how often the velocity is sent does not help — re-arming four times a second still steps — so it now uses `duration=0`, the SDK's "always effective, will not stop automatically", and skips a velocity that has not changed. One command stands until a different one replaces it. That removes the controller-side expiry, which `_expire_jog` was relying on: it flipped `_jogging` and reported `ready` without commanding anything, because the controller's own timer had already stopped the arm. With the velocity held indefinitely that would leave an arm travelling while the node reported idle, so `_expire_jog` now queues the stop itself. It goes through the job queue rather than the driver, since it runs on the reporter thread and the control thread owns the driver. `VELOCITY_COMMAND_TTL_S` is unchanged in value and meaning to a caller, but it is now enforced here rather than by the controller. Both halves are one commit on purpose: `duration=0` without the stop in `_expire_jog` is an arm that never stops when its sender dies. Adds tests/test_driver.py — the driver had no direct coverage, because FakeArm replaces it everywhere else, which is why this was invisible to the suite.
618 lines
26 KiB
Python
618 lines
26 KiB
Python
"""The threads that own the arm, and the mailboxes the event loop drains.
|
|
|
|
**Nothing outside this file may call into the driver**, with two exceptions
|
|
written to be safe from another thread: :meth:`ULite6.stop`, and
|
|
:meth:`ULite6.read_report`, which reads the controller's report cache rather
|
|
than the wire.
|
|
|
|
There are two threads because a motion must not stop the arm being watched. The
|
|
xArm SDK blocks: a move waits out the motion, and a read from a controller whose
|
|
cable was pulled blocks until its own socket timeout. So:
|
|
|
|
- The **control thread** runs one job at a time and is allowed to block. It is
|
|
where every service is carried out, including the motions that take seconds.
|
|
- The **reporter thread** never blocks. It reads the report cache at a fixed
|
|
cadence, so `tcp_pose` and `joint_pose` keep flowing right through a ten-second
|
|
move — which is the only way a caller can follow one, since the thread that
|
|
started it is inside it until it ends.
|
|
- The **stopper thread** exists only to call `stop`. It cannot be the control
|
|
thread (a stop must not queue behind the motion it aborts) and it must not be
|
|
the event loop: the SDK's `emergency_stop` re-commands the controller until it
|
|
confirms, for up to three seconds, and a dataflow router cannot go away for
|
|
three seconds. Requests to it coalesce — several stops in a row are one stop.
|
|
|
|
The dora event loop is a third party to both: it drains mailboxes and publishes,
|
|
and never waits for the arm.
|
|
"""
|
|
|
|
import queue
|
|
import threading
|
|
import time
|
|
from dataclasses import dataclass, field
|
|
from typing import Any
|
|
|
|
from dora_ulite6 import services
|
|
from dora_ulite6.config import Config
|
|
from dora_ulite6.driver import ULite6
|
|
from dora_ulite6.errors import ArmNeedsHomingError, RobotUnavailableError
|
|
from dora_ulite6.log import log
|
|
from dora_ulite6.pose import RobotPose, with_tool_offset
|
|
|
|
# The states the node reports on `robot_state`, in the order a healthy start
|
|
# walks through them.
|
|
CONNECTING = "connecting"
|
|
READY = "ready"
|
|
MOVING = "moving"
|
|
RECOVERING = "recovering"
|
|
UNAVAILABLE = "unavailable"
|
|
|
|
|
|
# The motion lifecycle published on `motion_state`. `accepted` exists precisely
|
|
# to close the race a bare moving/idle flag leaves open: between a caller being
|
|
# told its request was taken and the control thread reaching it, a two-valued
|
|
# state still reads "not moving", and a caller polling for completion would
|
|
# conclude the motion it just asked for had already finished.
|
|
ACCEPTED = "accepted"
|
|
DONE = "done"
|
|
FAILED = "failed"
|
|
PREEMPTED = "preempted"
|
|
|
|
|
|
@dataclass
|
|
class ServiceCall:
|
|
"""One service request waiting on the control thread."""
|
|
|
|
request_id: str
|
|
row: dict
|
|
# Whether the caller is held until the arm stops. False means the response
|
|
# was already sent as an acknowledgement, and completion is only reported on
|
|
# `motion_state` — which is what keeps it one response per request.
|
|
wait: bool = True
|
|
# Set when a newer motion took this one's place, before or during its run.
|
|
preempted_by: str | None = None
|
|
|
|
@property
|
|
def service(self) -> str:
|
|
"""The service this request names."""
|
|
return self.row.get(services.SERVICE, "")
|
|
|
|
@property
|
|
def moves_the_arm(self) -> bool:
|
|
"""Whether carrying this out drives the arm, and so preempts and is preemptable."""
|
|
return self.service in services.MOVES_THE_ARM
|
|
|
|
|
|
@dataclass
|
|
class Jog:
|
|
"""One velocity command. Not a service: nobody waits for an answer."""
|
|
|
|
velocity: list[float]
|
|
|
|
|
|
@dataclass
|
|
class Telemetry:
|
|
"""One reading of the arm: joint angles in degrees, tool-tip pose in mm and degrees."""
|
|
|
|
joints: list[float]
|
|
tip: RobotPose
|
|
timestamp_ns: int
|
|
|
|
|
|
@dataclass
|
|
class Message:
|
|
"""Something to publish: an output id, its fields, and its correlation metadata."""
|
|
|
|
output: str
|
|
fields: dict[str, Any]
|
|
metadata: dict[str, str] = field(default_factory=dict)
|
|
|
|
|
|
class Control:
|
|
"""Owns the arm and hands the event loop what there is to publish."""
|
|
|
|
def __init__(self, config: Config, restarted: bool = False):
|
|
self.config = config
|
|
# A restarted node inherits an arm that is wherever it was left. Driving
|
|
# it to INIT_MODE because the process came back is a motion nobody asked
|
|
# for, so a restart skips it.
|
|
self.restarted = restarted
|
|
self.robot: ULite6 | None = None
|
|
self._jobs: queue.Queue[ServiceCall | Jog] = queue.Queue()
|
|
self._lock = threading.Lock()
|
|
self._telemetry: Telemetry | None = None
|
|
self._outbox: list[Message] = []
|
|
self._state = CONNECTING
|
|
self._state_changes: list[str] = [CONNECTING]
|
|
# The motion the control thread is inside right now, and whether the
|
|
# last preemption left a software stop for the next motion to clear.
|
|
self._running_motion: ServiceCall | None = None
|
|
self._stopped_for_preempt = False
|
|
# Set by anyone who needs the arm halted; the stopper thread drains it.
|
|
self._stop_wanted = threading.Event()
|
|
# Whether a velocity command is currently keeping the arm moving, and
|
|
# when the last one arrived — the controller expires them on its own.
|
|
self._jogging = False
|
|
self._last_jog = 0.0
|
|
# Whether the arm has been reached once and put where INIT_MODE wants it.
|
|
self._started = False
|
|
# Set to ask the threads to finish. The sense matters: this is waited on
|
|
# as the threads' sleep, and an event that is *set* while running makes
|
|
# every wait() return at once — a sleep that never sleeps, which is a
|
|
# busy loop rather than a cadence.
|
|
self._stopping = threading.Event()
|
|
self._control = threading.Thread(target=self._run_control, name="ulite6-control", daemon=True)
|
|
self._reporter = threading.Thread(target=self._run_reporter, name="ulite6-reporter", daemon=True)
|
|
self._stopper = threading.Thread(target=self._run_stopper, name="ulite6-stopper", daemon=True)
|
|
|
|
def start(self) -> None:
|
|
"""Start the three threads; they reach the arm themselves, so this never blocks."""
|
|
self._control.start()
|
|
self._reporter.start()
|
|
self._stopper.start()
|
|
|
|
def shutdown(self) -> bool:
|
|
"""Halt the arm, stop the threads, and say whether they really finished.
|
|
|
|
Halting first is the point. A motion lives in the *controller*, not in
|
|
this process: dropping the connection does not abort it, so a node that
|
|
exits mid-move leaves an arm travelling with nobody left to stop it. The
|
|
stop is commanded here on the caller's thread and waited for, because by
|
|
now the event loop has finished routing and there is nothing left to
|
|
starve.
|
|
|
|
Returns False if the control thread is still inside the SDK afterwards,
|
|
which is the caller's cue to leave without waiting for it.
|
|
"""
|
|
if self.robot is not None:
|
|
self._attempt(self.robot.stop)
|
|
self._stopping.set()
|
|
self._stop_wanted.set()
|
|
self._stopper.join(timeout=1.0)
|
|
self._reporter.join(timeout=1.0)
|
|
self._control.join(timeout=self.config.motion.ready_timeout_s)
|
|
if self._control.is_alive():
|
|
log("the control thread is still inside the SDK; leaving it behind")
|
|
return False
|
|
return True
|
|
|
|
# --- what the event loop puts in ---
|
|
|
|
def submit(self, job: ServiceCall | Jog) -> None:
|
|
"""Queue work for the control thread. Returns immediately."""
|
|
self._jobs.put(job)
|
|
|
|
def request_stop(self, request_id: str = "") -> None:
|
|
"""Ask for the arm to be halted, and come straight back.
|
|
|
|
Answering before the arm has actually stopped is deliberate: the reply
|
|
says the stop was commanded, and whether it took is reported where every
|
|
other outcome is, on `motion_state` and `robot_state`. Waiting here
|
|
instead would hand the caller a better answer at the price of the whole
|
|
dataflow, for up to three seconds.
|
|
"""
|
|
self._stop_wanted.set()
|
|
if request_id:
|
|
self._respond(
|
|
ServiceCall(request_id=request_id, row={services.SERVICE: services.STOP}),
|
|
(True, {"message": "stop commanded"}),
|
|
)
|
|
|
|
def _preempt_motions(self, by: str) -> None:
|
|
"""Mark the running motion and any still waiting, and halt the arm if one was moving."""
|
|
with self._lock:
|
|
running = self._running_motion
|
|
waiting = [
|
|
job
|
|
for job in list(self._jobs.queue)
|
|
if isinstance(job, ServiceCall) and job.moves_the_arm and job.preempted_by is None
|
|
]
|
|
for job in [running, *waiting]:
|
|
if job is not None:
|
|
job.preempted_by = by
|
|
# The next motion to actually run clears the software stop this is
|
|
# about to cause. Only ever set here, never unset: preempting a
|
|
# motion that had not started yet stops nothing, and must not clear a
|
|
# flag left by an earlier preemption that nothing has consumed — that
|
|
# would send the next motion at an arm still in its stopped state,
|
|
# where it would be refused the moment it was commanded.
|
|
self._stopped_for_preempt = self._stopped_for_preempt or running is not None
|
|
if running is not None:
|
|
log(f"{running.service} ({running.request_id}) preempted by {by}")
|
|
self.request_stop()
|
|
|
|
def start_jog(self, velocity: list[float]) -> None:
|
|
"""Take one velocity command, preempting a motion the first time it moves the arm.
|
|
|
|
A jog arrives many times a second, so everything here happens on the
|
|
transitions only: a stream of them is one preemption and one state
|
|
change, not hundreds.
|
|
"""
|
|
moving = any(velocity)
|
|
if moving and not self._jogging:
|
|
self._preempt_motions("jog")
|
|
self._set_state(MOVING)
|
|
with self._lock:
|
|
was_jogging, self._jogging = self._jogging, moving
|
|
self._last_jog = time.monotonic()
|
|
# Only a jog that was in fact jogging ends one. A zero velocity from a
|
|
# teleop source that is merely idling says nothing about a motion that
|
|
# owns the arm, and must not answer for it.
|
|
if not moving and was_jogging:
|
|
self._idle_unless_busy()
|
|
self._jobs.put(Jog(velocity=velocity))
|
|
|
|
def start_motion(self, call: ServiceCall) -> None:
|
|
"""Take a motion, preempting whatever motion was already in play.
|
|
|
|
The arm is one resource: there is never more than one motion, so a new
|
|
one replaces the old rather than queueing behind it. The running motion
|
|
is cut short through the arm — the same stop a caller can send — and any
|
|
motion still waiting its turn is dropped without ever moving anything.
|
|
|
|
Each preempted request is answered exactly once, as every request is: if
|
|
its caller is still waiting, by its response; if it was a `wait: false`
|
|
request, it was acknowledged when it arrived, and the preemption shows
|
|
only on `motion_state`.
|
|
"""
|
|
self._preempt_motions(call.request_id)
|
|
self._motion_state(call, ACCEPTED, "queued")
|
|
if not call.wait:
|
|
self._respond(call, (True, {"message": "accepted", "accepted": True}))
|
|
self._jobs.put(call)
|
|
|
|
def post(self, message: Message) -> None:
|
|
"""Add a message for the event loop to publish on the next tick.
|
|
|
|
The loop uses this to answer a request it refused itself, so a malformed
|
|
message is still answered under its own correlation id without the arm
|
|
threads being troubled by it.
|
|
"""
|
|
self._post(message)
|
|
|
|
def call_now(self, request_id: str, row: dict) -> None:
|
|
"""Run a service on the calling thread, for the ones that must not queue.
|
|
|
|
Only `stop` takes this path: queued, it would sit behind the very motion
|
|
it exists to abort. It is safe here because the driver method it reaches
|
|
is the one written for another thread.
|
|
"""
|
|
call = ServiceCall(request_id=request_id, row=row)
|
|
if self.robot is None:
|
|
self._respond(call, (False, {"message": "the arm has not been reached yet"}))
|
|
return
|
|
self._respond(call, self._attempt(lambda: services.run(self.robot, row)))
|
|
|
|
# --- what the event loop drains ---
|
|
|
|
def take_telemetry(self) -> Telemetry | None:
|
|
"""Return the newest reading and clear it, so a tick never republishes a stale pose."""
|
|
with self._lock:
|
|
telemetry, self._telemetry = self._telemetry, None
|
|
return telemetry
|
|
|
|
def take_messages(self) -> list[Message]:
|
|
"""Return every response recorded since the last call."""
|
|
with self._lock:
|
|
outbox, self._outbox = self._outbox, []
|
|
return outbox
|
|
|
|
def take_state_changes(self) -> list[str]:
|
|
"""Return every state transition since the last call."""
|
|
with self._lock:
|
|
changes, self._state_changes = self._state_changes, []
|
|
return changes
|
|
|
|
def _post(self, message: Message) -> None:
|
|
with self._lock:
|
|
self._outbox.append(message)
|
|
|
|
# --- the control thread ---
|
|
|
|
def _run_control(self) -> None:
|
|
while not self._stopping.is_set():
|
|
if not self._started:
|
|
self._start_up()
|
|
continue
|
|
try:
|
|
job = self._jobs.get(timeout=0.1)
|
|
except queue.Empty:
|
|
continue
|
|
if isinstance(job, Jog):
|
|
self._run_jog(job)
|
|
else:
|
|
self._run_service(job)
|
|
|
|
def _start_up(self) -> None:
|
|
"""Reach an arm that answers, put it where INIT_MODE says, and start serving.
|
|
|
|
The driver is built once and then retried through its own ``connect()``,
|
|
which reuses the SDK handle: rebuilding it on every attempt would leak a
|
|
socket and its threads for every second an absent arm stays absent.
|
|
"""
|
|
self._set_state(CONNECTING)
|
|
if self.robot is None:
|
|
self.robot = ULite6(self.config)
|
|
try:
|
|
if not self.robot.is_connected() and not self.robot.connect():
|
|
raise RobotUnavailableError(f"robot at {self.config.robot_ip} is not reachable")
|
|
self._start_position()
|
|
except RobotUnavailableError as error:
|
|
log(f"{error}; retrying in {self.config.reconnect_interval_s:.0f}s")
|
|
self._set_state(UNAVAILABLE)
|
|
self._serve_while_down(self.config.reconnect_interval_s)
|
|
return
|
|
self._started = True
|
|
self._set_state(READY)
|
|
|
|
def _serve_while_down(self, seconds: float) -> None:
|
|
"""Answer what can be answered without a serving arm, until it is time to retry.
|
|
|
|
This is the wait between connection attempts, and it serves the queue
|
|
rather than sleeping through it. Sitting silent is the one thing a caller
|
|
must never get: an arm that is down is an outcome to report, not a reason
|
|
to stop answering, and a request left in the queue here would be answered
|
|
only if the arm ever came back.
|
|
"""
|
|
deadline = time.monotonic() + seconds
|
|
while not self._stopping.is_set():
|
|
remaining = deadline - time.monotonic()
|
|
if remaining <= 0:
|
|
return
|
|
try:
|
|
job = self._jobs.get(timeout=min(remaining, 0.1))
|
|
except queue.Empty:
|
|
continue
|
|
if isinstance(job, ServiceCall):
|
|
self._serve_without_arm(job)
|
|
|
|
def _serve_without_arm(self, call: ServiceCall) -> None:
|
|
"""Answer one request that arrived before the arm was serving.
|
|
|
|
A motion is refused outright. Holding it would be worse than silence:
|
|
the arm is somewhere unknown, nobody is watching, and the motion would
|
|
fire unattended the moment the link came back.
|
|
|
|
Everything else is tried on the arm, because a controller that answers
|
|
while refusing to serve is exactly what `recover` and `clear` are for,
|
|
and refusing them here would leave the one state they exist to escape
|
|
with no way out of it.
|
|
"""
|
|
if call.moves_the_arm:
|
|
self._finish_motion(call, FAILED, False, "the arm is not serving yet")
|
|
return
|
|
self._respond(call, self._attempt(lambda: services.run(self.robot, call.row)))
|
|
|
|
def _start_position(self) -> None:
|
|
"""Put the arm where INIT_MODE says it should start, if anywhere."""
|
|
if self.restarted and self.config.init_mode != "none":
|
|
log(f"restarted: leaving the arm where it is rather than running INIT_MODE={self.config.init_mode}")
|
|
return
|
|
if self.config.init_mode == "home":
|
|
log("moving to home")
|
|
self.robot.go_home()
|
|
elif self.config.init_mode == "pose":
|
|
log(f"moving to the initial pose {self.config.init_pose.as_list()}")
|
|
self.robot.move_to(
|
|
with_tool_offset(self.config.init_pose, -self.config.tool_offset_mm),
|
|
self.config.init_speed,
|
|
)
|
|
|
|
def _run_service(self, call: ServiceCall) -> None:
|
|
"""Carry out one service and post its response."""
|
|
if call.moves_the_arm:
|
|
self._run_motion(call)
|
|
return
|
|
self._respond(call, self._attempt(lambda: services.run(self.robot, call.row)))
|
|
|
|
def _run_motion(self, call: ServiceCall) -> None:
|
|
"""Carry out one motion, reporting its whole life on `motion_state`.
|
|
|
|
Blocks for as long as the arm takes. `robot_state` says `moving`
|
|
meanwhile, so a caller watching it can tell a slow service from a stalled
|
|
one, and `tcp_pose` keeps arriving because the reporter thread is not the
|
|
one in here.
|
|
"""
|
|
if call.preempted_by is not None:
|
|
# Replaced while it waited its turn; it never moved anything.
|
|
self._finish_motion(call, PREEMPTED, False, f"preempted by {call.preempted_by}")
|
|
return
|
|
with self._lock:
|
|
self._running_motion = call
|
|
clear_first = self._stopped_for_preempt
|
|
self._stopped_for_preempt = False
|
|
self._set_state(MOVING)
|
|
self._motion_state(call, MOVING, "")
|
|
if self._leave_jog():
|
|
# A pose command would switch the controller out of velocity mode
|
|
# behind the driver's back, leaving its own bookkeeping saying the
|
|
# arm is still jogging. Leaving through the front door keeps them
|
|
# agreed.
|
|
self._attempt(lambda: self.robot.set_cartesian_velocity([0.0] * 6))
|
|
if clear_first:
|
|
# Preempting leaves the controller in its software-stopped state.
|
|
# The driver's own set_state(0) would probably be enough; clearing
|
|
# first is cheap and does not depend on that being true.
|
|
self._attempt(self.robot.clear)
|
|
outcome = self._attempt(lambda: services.run(self.robot, call.row))
|
|
with self._lock:
|
|
self._running_motion = None
|
|
preempted_by = call.preempted_by
|
|
ok, fields = outcome
|
|
if preempted_by is not None:
|
|
# A preempted move fails through MotionInterruptedError like any
|
|
# aborted one; only the flag says who cut it short.
|
|
self._finish_motion(call, PREEMPTED, False, f"preempted by {preempted_by}")
|
|
else:
|
|
self._finish_motion(call, DONE if ok else FAILED, ok, fields.get("message", ""), fields)
|
|
self._idle_unless_busy()
|
|
|
|
def _leave_jog(self) -> bool:
|
|
"""Clear the jogging flag, reporting whether the arm was in fact jogging."""
|
|
with self._lock:
|
|
was_jogging, self._jogging = self._jogging, False
|
|
return was_jogging
|
|
|
|
def _finish_motion(self, call: ServiceCall, state: str, ok: bool, message: str, fields=None) -> None:
|
|
"""Publish a motion's end, and answer its caller if one is still waiting."""
|
|
self._motion_state(call, state, message)
|
|
if call.wait:
|
|
self._respond(call, (ok, dict(fields or {"message": message})))
|
|
|
|
def _motion_state(self, call: ServiceCall, state: str, message: str) -> None:
|
|
"""Publish where a motion stands.
|
|
|
|
The request_id rides in the *fields*, not the metadata: metadata carrying
|
|
a request_id means "this is the answer to that request", and this is a
|
|
broadcast of the arm's state, which anyone may read. Keeping it in the
|
|
fields is also what lets a caller polling `get_state` see the same value
|
|
as one watching the topic.
|
|
"""
|
|
self._post(
|
|
Message(
|
|
output="motion_state",
|
|
fields={
|
|
"request_id": call.request_id,
|
|
"service": call.service,
|
|
"state": state,
|
|
"message": message,
|
|
},
|
|
)
|
|
)
|
|
|
|
def _run_jog(self, jog: Jog) -> None:
|
|
"""Command one velocity. Nothing is published: a jog has no answer."""
|
|
def command() -> dict:
|
|
self.robot.set_cartesian_velocity(jog.velocity)
|
|
return {"message": "jogging"}
|
|
|
|
ok, fields = self._attempt(command)
|
|
if not ok:
|
|
log(f"jog refused: {fields['message']}")
|
|
|
|
def _respond(self, call: ServiceCall, outcome: tuple[bool, dict]) -> None:
|
|
"""Post the one response this request gets."""
|
|
ok, fields = outcome
|
|
fields.setdefault("message", "")
|
|
self._post(
|
|
Message(
|
|
output="response",
|
|
fields={"ok": ok, "service": call.service, **fields},
|
|
metadata={"request_id": call.request_id},
|
|
)
|
|
)
|
|
|
|
def _attempt(self, work) -> tuple[bool, dict]:
|
|
"""Run ``work``, turning whatever it raises into a failed response."""
|
|
try:
|
|
return True, work()
|
|
except ArmNeedsHomingError as error:
|
|
return False, {"message": f"{error}; call `home_and_clear` once the area is clear"}
|
|
except RobotUnavailableError as error:
|
|
self._go_unavailable(error)
|
|
return False, {"message": str(error)}
|
|
except Exception as error: # noqa: BLE001 — a failed request must never take the thread down
|
|
return False, {"message": str(error)}
|
|
|
|
def _go_unavailable(self, error: Exception) -> None:
|
|
"""Report the arm as unusable and try once to bring it back.
|
|
|
|
A single attempt, not a retry loop: the thread has jobs to serve, and the
|
|
reporter comes round again in milliseconds.
|
|
"""
|
|
self._set_state(RECOVERING)
|
|
log(f"the arm is unusable ({error}); trying to recover")
|
|
try:
|
|
self.robot.recover()
|
|
except RobotUnavailableError as recovery_error:
|
|
log(f"recovery failed: {recovery_error}")
|
|
self._set_state(UNAVAILABLE)
|
|
return
|
|
self._set_state(READY)
|
|
|
|
# --- the stopper thread ---
|
|
|
|
def _run_stopper(self) -> None:
|
|
"""Halt the arm whenever anyone asks, and never make them wait for it.
|
|
|
|
One stop per wake-up however many asked, which is what a stop means:
|
|
several callers wanting the arm halted want the same single thing.
|
|
"""
|
|
while not self._stopping.is_set():
|
|
if not self._stop_wanted.wait(timeout=0.1):
|
|
continue
|
|
self._stop_wanted.clear()
|
|
if self._stopping.is_set():
|
|
return
|
|
if self.robot is not None:
|
|
self._attempt(self.robot.stop)
|
|
|
|
def _set_state(self, state: str) -> None:
|
|
"""Record a state, queueing it for publication only when it is a change."""
|
|
with self._lock:
|
|
if state == self._state:
|
|
return
|
|
self._state = state
|
|
self._state_changes.append(state)
|
|
log(f"state: {state}")
|
|
|
|
def _idle_unless_busy(self) -> None:
|
|
"""Report the arm idle, but only when nothing is still driving it.
|
|
|
|
One thing ending is not the arm stopping. A motion may still be winding
|
|
down inside the SDK after the jog that preempted it went quiet; a jog may
|
|
have taken the arm over from the motion that just finished. Either way
|
|
`ready` published over it is the false idle the state exists to rule out.
|
|
|
|
The state also stops being ours to set once it has left `moving` — that
|
|
is `unavailable` or `recovering` reporting something this does not know
|
|
about, and it must not be overwritten with `ready`.
|
|
"""
|
|
with self._lock:
|
|
if self._running_motion is not None or self._jogging or self._state != MOVING:
|
|
return
|
|
self._set_state(READY)
|
|
|
|
# --- the reporter thread ---
|
|
|
|
def _expire_jog(self) -> None:
|
|
"""Stop the arm once the sender driving it has gone quiet.
|
|
|
|
**This is the deadman, not bookkeeping.** The velocity is set on the
|
|
controller once and held there indefinitely — see
|
|
:meth:`ULite6.set_cartesian_velocity` for why it cannot be given a
|
|
duration — so a sender that dies mid-jog leaves an arm that travels
|
|
until something says otherwise. Nothing tells us; the clock does, and
|
|
this is what says otherwise.
|
|
|
|
The stop goes through the job queue rather than the arm directly: this
|
|
runs on the reporter thread, and the control thread is the only one that
|
|
may touch the driver.
|
|
"""
|
|
with self._lock:
|
|
if not self._jogging or time.monotonic() - self._last_jog < self.config.motion.velocity_command_ttl_s:
|
|
return
|
|
self._jogging = False
|
|
self._jobs.put(Jog(velocity=[0.0] * 6))
|
|
self._idle_unless_busy()
|
|
|
|
def _run_reporter(self) -> None:
|
|
"""Publish telemetry, whatever the control thread is doing.
|
|
|
|
Never touches anything that can block, so it keeps running through a
|
|
ten-second move — which is what lets a caller follow a motion whose
|
|
response will not arrive until it ends.
|
|
"""
|
|
while not self._stopping.is_set():
|
|
self._stopping.wait(self.config.telemetry_interval_s)
|
|
if self.robot is None:
|
|
continue
|
|
self._expire_jog()
|
|
reading = self.robot.read_report()
|
|
if reading is None:
|
|
continue
|
|
joints, flange = reading
|
|
tip = with_tool_offset(flange, self.config.tool_offset_mm)
|
|
with self._lock:
|
|
self._telemetry = Telemetry(joints, tip, time.time_ns())
|
|
|