Skip to content

armgit / moss

Public Imported from GitHub
0

MOSS: a small, mostly 3D-printed litter-picking rover. Cleaner streets, one maker at a time.

Python
32.6 KB
Raw Blame
"""BumpImu: contacts felt by the RealSense's accelerometer, published on
`bump` (a contact in front) or `bump_rear` (a contact behind).

The encoders are a poor bumper: a track that slips keeps turning against a
wall, an oblique contact leaves one track free, and the stall guard needs a
full second of frozen ticks before it calls a stall. The chassis itself
says it at once: hitting something while driving is a short, hard
deceleration along the direction of travel. This module reads it on the
camera's IMU (`imu`, published by moss_dimos.realsense_imu.MossRealSense) and
publishes Bool(True) on the side of the contact:

  * `bump`       in front: the planner's escape backs off 0.20 m in reverse.
                 The explorer's back-off publishes on the same stream; the
                 costmap, the sounds and the lidar odometry listen to it.
  * `bump_rear`  behind: the planner's escape moves 0.20 m FORWARD. A rear
                 contact is not a front wall: the costmap and the sounds do
                 not listen to it.

The two are opposite reflexes. The escape moves away from the contact; a
contact felt while reversing and answered with another reverse drives the
rover back into what it just hit. A stall of the base is not published here
or on either stream: it is inferred, not felt (the coordinator's `stall`).

The rule (ImuContactDetector, pure, benched with known traces):
  * forward = one signed accel axis of the frame the camera publishes in
    (MOSS_BUMP_AXIS, default "+z": librealsense's optical axes, z out of the
    lens, the rover's forward; docs/imu.md);
  * high-pass: the sample's forward component minus the mean of the samples
    of the last 0.5 s before it, so gravity, a tilted camera or a slope cancel;
  * a contact is |high-pass| >= MOSS_BUMP_ACCEL (default 4.0 m/s^2), with a
    non-zero track command younger than 1 s, decelerating AGAINST that command
    (driving forward: <= -threshold, in front; reversing: >= +threshold,
    behind; pivoting, the tracks of opposite signs: either sign, and the sign
    gives the side: a deceleration is in front, a forward kick behind);
  * once per contact, one debounce per side: nothing more on the same side
    for MOSS_BUMP_DEBOUNCE_S (default 0.7 s). A front contact and a rear one
    are opposite reflexes, not repeats of each other: the other side counts
    at once. One exception, which an accelerometer has and a switch would
    not: a kick of the other sign within REBOUND_S (0.1 s) of a contact is
    that impact's rebound, whatever the command says by then. It is logged
    once ("MOSS IMU: ... taken as its rebound"), never published.

The command is the firmware's target_pct from the coordinator's
`base_telemetry` (2 Hz), aged by the time since it arrived plus the
firmware's own age of that line.

  In  imu             : Imu      the camera's accel + gyro (every sample: a plain subscription, not the
                                 latest-only async handler, so a burst of samples is never thinned)
  In  base_telemetry  : String   JSON with target_pct [l, r] (percent, + = forward) and firmware_age_s
  Out bump            : Bool     True on a contact in front
  Out bump_rear       : Bool     True on a contact behind

Each contact is logged as one line, e.g.
    IMU contact: forward -4.8 m/s^2 (threshold 4.0) while commanded L=+70% R=+60%, front contact -> bump
(the cockpit's decision feed matches its start, "IMU contact"). The other
lines start with "MOSS IMU:": the knobs at start, the first sample, and the
watchdog's (ImuWatchdog). A contact sensor must never go quiet unnoticed:
when no sample has come for 1 s while the stack runs, one warning says so,
    MOSS IMU: no sample for 1 s - IMU contacts and gyro prior OFF
and one line says when the samples resume, at what rate,
    MOSS IMU: samples back (199 Hz) after 3.2 s quiet
(a stream that keeps dropping out says it at most once every 5 s, with a
count of the spells in between). Every 10 s a status line gives the state
(alive, QUIET, or waiting for the first sample), the sample rate, where
gravity sits and the contacts so far. All of it goes through the dimOS
logger, into the run's log.

MOSS_IMU_LOG=<path> appends every sample to a CSV (ts, ax, ay, az, gx, gy,
gz, hp, cmd_l, cmd_r), flushed every 2 s: the signature of real contacts,
real braking and real floors, to read before tightening the threshold.
"""
from __future__ import annotations

import json
import math
import os
import re
import threading
import time
from collections import deque
from typing import Any

from dimos.core.core import rpc
from dimos.core.module import Module
from dimos.core.stream import In, Out
from dimos.msgs.sensor_msgs.Imu import Imu
from dimos.msgs.std_msgs.String import String
from dimos.utils.logging_config import setup_logger
from dimos_lcm.std_msgs import Bool   # the SAME Bool class as the explorer's bump and the coordinator's stall

logger = setup_logger()

DEFAULT_AXIS = "+z"             # librealsense optical frame: x right, y down, z forward (docs/imu.md)
DEFAULT_THRESHOLD = 4.0         # m/s^2 of high-passed forward deceleration
DEFAULT_DEBOUNCE_S = 0.7        # one contact, then silence for this long
WINDOW_S = 0.5                  # the running mean the high-pass subtracts
WARMUP_S = 0.25                 # history the window needs before a high-pass value is trusted
COMMAND_MAX_AGE_S = 1.0         # a track command older than this is no command
CSV_FLUSH_S = 2.0
CSV_HEADER = "ts,ax,ay,az,gx,gy,gz,hp,cmd_l,cmd_r\n"
QUIET_S = 1.0                   # no sample for this long while the stack runs: the stream is quiet
QUIET_REPEAT_S = 5.0            # at most one "no sample" line this often; the spells in between are counted
RATE_WINDOW_S = 1.0             # "samples back" measures the rate over this much of the resumed stream
STATUS_PERIOD_S = 10.0          # one status line this often, quiet or not
WATCH_PERIOD_S = 0.1            # how often the watchdog thread looks
FLOWING_S = 2 * WATCH_PERIOD_S  # a resumed stream whose last sample is older than this has stopped again
# An impact rings: right after the hit the accelerometer can feel a kick of the
# other sign, and by then the telemetry may already carry the escape's
# reverse. The contact measured on this robot lasted 20 ms (four samples past
# -4 m/s^2, docs/imu.md): 0.1 s covers it five times over, while two real
# contacts on opposite sides (a rover wedging into a corner) benched 0.18 s
# apart stay two. The figure comes from that impact length, not from a
# recorded rebound: a MOSS_IMU_LOG pivot contact is the recording that sets it.
REBOUND_S = 0.1
THRESHOLD_MAX = 100.0
DEBOUNCE_MAX_S = 10.0
_AXIS_RE = re.compile(r"^([+-]?)([xyz])$")


def parse_axis(text: str) -> tuple[int, float]:
    """"+z" / "z" / "-y" -> (axis index, sign). Raises ValueError otherwise."""
    m = _AXIS_RE.match(str(text).strip().lower())
    if not m:
        raise ValueError(f"not an axis: {text!r} (x, y or z with an optional sign, e.g. +z)")
    return "xyz".index(m.group(2)), (-1.0 if m.group(1) == "-" else 1.0)


def bump_settings(env: dict | None = None) -> dict:
    """The detector's knobs from the environment: {"axis", "threshold",
    "debounce_s", "log_path"}. A value that does not parse, or is out of range,
    is refused with a warning and the default is kept."""
    env = os.environ if env is None else env
    out = {"axis": DEFAULT_AXIS, "threshold": DEFAULT_THRESHOLD,
           "debounce_s": DEFAULT_DEBOUNCE_S, "log_path": None}
    raw = str(env.get("MOSS_BUMP_AXIS", "")).strip()
    if raw:
        try:
            parse_axis(raw)
            out["axis"] = raw.lower() if raw[0] in "+-" else "+" + raw.lower()
        except ValueError as exc:
            logger.warning(f"MOSS_BUMP_AXIS: {exc}; keeping {DEFAULT_AXIS}")
    for name, key, low, high in (("MOSS_BUMP_ACCEL", "threshold", 0.0, THRESHOLD_MAX),
                                 ("MOSS_BUMP_DEBOUNCE_S", "debounce_s", 0.0, DEBOUNCE_MAX_S)):
        raw = str(env.get(name, "")).strip()
        if not raw:
            continue
        try:
            value = float(raw)
        except ValueError:
            value = math.nan
        positive = key == "threshold"
        if not math.isfinite(value) or value > high or (value <= low if positive else value < low):
            bound = f"{'above' if positive else 'from'} {low:g} {'and up to' if positive else 'to'} {high:g}"
            logger.warning(f"{name}={raw!r} is not a number {bound}: keeping {out[key]:g}")
            continue
        out[key] = value
    path = str(env.get("MOSS_IMU_LOG", "")).strip()
    if path:
        out["log_path"] = os.path.abspath(os.path.expanduser(path))
    return out


def contact_side(direction: int, hp: float) -> str:
    """"front" or "rear" for a contact against the command: reversing is
    behind, driving forward in front, and on a pivot the sign of the
    high-pass decides (a deceleration of the forward axis is in front)."""
    if direction < 0 or (direction == 0 and hp > 0):
        return "rear"
    return "front"


def command_direction(commanded: tuple[int, int] | None) -> int | None:
    """+1 forward (no track backward), -1 reverse (no track forward), 0 a
    pivot (tracks of opposite signs), None no command (None or (0, 0))."""
    if commanded is None:
        return None
    try:
        left, right = float(commanded[0]), float(commanded[1])
    except (TypeError, ValueError, IndexError):
        return None
    if left == 0 and right == 0:
        return None
    if left >= 0 and right >= 0:
        return 1
    if left <= 0 and right <= 0:
        return -1
    return 0


class ImuContactDetector:
    """The contact rule of the module docstring, on raw samples. Pure: no
    clock, no I/O. feed() returns True once per contact, its side in
    last_contact["side"]; last_hp is the high-pass of the last sample (None
    while the window warms up). A rebound is counted in `rebounds` once per
    contact, described in last_rebound, and feed() returns False for it."""

    def __init__(self, axis: str = DEFAULT_AXIS, threshold: float = DEFAULT_THRESHOLD,
                 debounce_s: float = DEFAULT_DEBOUNCE_S, window_s: float = WINDOW_S,
                 warmup_s: float = WARMUP_S, command_max_age_s: float = COMMAND_MAX_AGE_S,
                 rebound_s: float = REBOUND_S) -> None:
        self.axis = axis
        self._index, self._sign = parse_axis(axis)
        self.threshold = float(threshold)
        self.debounce_s = float(debounce_s)
        self.window_s = float(window_s)
        self.warmup_s = float(warmup_s)
        self.command_max_age_s = float(command_max_age_s)
        self.rebound_s = float(rebound_s)
        self._window: deque[tuple[float, float]] = deque()
        self._sum = 0.0
        self._appends = 0
        self.last_hp: float | None = None
        self.last_contact_ts: float | None = None
        self.last_contact: dict | None = None
        self._last_by_side: dict[str, float] = {}   # side -> ts of its last contact
        self.contacts = 0
        self.rebounds = 0
        self.last_rebound: dict | None = None
        self._rebound_side: str | None = None       # a rebound in progress: its samples are past the threshold
        self._rebound_of: float | None = None       # ts of the contact whose rebound was counted

    @classmethod
    def from_env(cls, env: dict | None = None) -> "ImuContactDetector":
        s = bump_settings(env)
        return cls(axis=s["axis"], threshold=s["threshold"], debounce_s=s["debounce_s"])

    def forward(self, accel_xyz: Any) -> float:
        """The forward component of an accel sample (the signed axis)."""
        return self._sign * float(accel_xyz[self._index])

    def _high_pass(self, ts: float, value: float) -> float | None:
        """value minus the mean of the samples of the last window_s before ts;
        None while the window holds less than warmup_s of history. Then the
        sample joins the window."""
        if self._window and ts < self._window[-1][0]:
            self._window.clear()          # the clock went back: start over
            self._sum = 0.0
        while self._window and self._window[0][0] < ts - self.window_s:
            self._sum -= self._window.popleft()[1]
        hp = None
        if self._window and ts - self._window[0][0] >= self.warmup_s:
            hp = value - self._sum / len(self._window)
        self._window.append((ts, value))
        self._sum += value
        self._appends += 1
        if self._appends % 4096 == 0:       # no float drift over hours of samples
            self._sum = math.fsum(v for _, v in self._window)
        return hp

    def feed(self, ts: float, accel_xyz: Any, commanded: tuple[int, int] | None,
             command_age_s: float = 0.0) -> bool:
        """One accel sample (m/s^2, the camera's frame) at ts (s), with the
        track command in force (percent pair, or None) and its age. True when
        this sample is a new contact."""
        try:
            ts = float(ts)
            value = self.forward(accel_xyz)
        except (TypeError, ValueError, IndexError):
            return False
        if not (math.isfinite(ts) and math.isfinite(value)):
            return False
        hp = self._high_pass(ts, value)
        self.last_hp = hp
        if hp is None or abs(hp) < self.threshold:
            self._rebound_side = None     # whatever rang has ended
            return False
        try:
            fresh = float(command_age_s) <= self.command_max_age_s
        except (TypeError, ValueError):
            fresh = False
        direction = command_direction(commanded) if fresh else None
        if direction is None:
            return False
        if direction > 0 and hp > -self.threshold:
            return False                  # driving forward: only a deceleration is a contact
        if direction < 0 and hp < self.threshold:
            return False                  # reversing: only a forward kick is a contact
        side = contact_side(direction, hp)
        of = self._rebound_target(side, ts)
        if of is not None:
            self._rebound_side = side
            if of != self._rebound_of:    # counted once per contact, however many samples it lasts
                self._rebound_of = of
                self.rebounds += 1
                self.last_rebound = {"ts": ts, "hp": hp, "commanded": (int(commanded[0]), int(commanded[1])),
                                     "side": side, "after_s": ts - of}
            return False
        self._rebound_side = None
        if self._repeats(side, ts):
            return False
        self._last_by_side[side] = ts
        self.last_contact_ts = ts
        self.contacts += 1
        self.last_contact = {"ts": ts, "hp": hp, "commanded": (int(commanded[0]), int(commanded[1])),
                             "side": side}
        return True

    def _rebound_target(self, side: str, ts: float) -> float | None:
        """The ts of the contact this sample is the rebound of, or None. A
        rebound is a sample of the other side within rebound_s of that side's
        last contact, whatever the command; once one has started, its samples
        stay the rebound while they are past the threshold."""
        other = "front" if side == "rear" else "rear"
        last = self._last_by_side.get(other)
        if last is None:
            return None
        if 0.0 <= ts - last < self.rebound_s or (side == self._rebound_side and self._rebound_of == last):
            return last
        return None

    def _repeats(self, side: str, ts: float) -> bool:
        """True when a contact at ts repeats the last one of the SAME side
        within debounce_s. One debounce per side: a contact on the other side
        is never a repeat (past the rebound, it is a new contact)."""
        last = self._last_by_side.get(side)
        return last is not None and 0.0 <= ts - last < self.debounce_s


def contact_stream(side: str) -> str:
    """The stream a contact on that side is published on."""
    return "bump_rear" if side == "rear" else "bump"


def contact_line(hp: float, threshold: float, commanded: tuple[int, int], side: str) -> str:
    """The log line of one contact (the cockpit's decision feed reads it)."""
    return (f"IMU contact: forward {hp:+.1f} m/s^2 (threshold {threshold:.1f}) "
            f"while commanded L={int(commanded[0]):+d}% R={int(commanded[1]):+d}%, "
            f"{side} contact -> {contact_stream(side)}")


def rebound_line(hp: float, commanded: tuple[int, int], side: str, after_s: float) -> str:
    """The log line of a kick taken as the rebound of the last contact: said
    once, so no felt kick is silent, and kept off the "IMU contact" lines the
    cockpit reads as contacts."""
    other = "front" if side == "rear" else "rear"
    return (f"MOSS IMU: forward {hp:+.1f} m/s^2 while commanded L={int(commanded[0]):+d}% "
            f"R={int(commanded[1]):+d}%, {after_s:.2f} s after the {other} contact: taken as its rebound, "
            f"not published on {contact_stream(side)}")


class ImuWatchdog:
    """Says when the imu stream goes quiet and when it comes back, and gives a
    status line every status_s (module docstring). Pure: the caller passes a
    monotonic clock. sample() runs on the subscription's thread, tick() on the
    watchdog's; one lock between them. tick() returns the lines to log, as
    (level, text) pairs.

    Quiet means no sample for quiet_s after at least one arrived: before the
    first sample the status line says "waiting" instead (the camera takes a
    few seconds to start). A quiet spell is said once, at most once per
    repeat_s (the spells in between are counted in the next line), and its
    end once the resumed stream has run for rate_window_s and still flows
    (its last sample younger than flowing_s), with its rate. A burst that has
    stopped again by then ends with no line: the spell goes on."""

    def __init__(self, quiet_s: float = QUIET_S, repeat_s: float = QUIET_REPEAT_S,
                 rate_window_s: float = RATE_WINDOW_S, status_s: float = STATUS_PERIOD_S,
                 flowing_s: float = FLOWING_S) -> None:
        self.quiet_s = float(quiet_s)
        self.repeat_s = float(repeat_s)
        self.rate_window_s = float(rate_window_s)
        self.status_s = float(status_s)
        self.flowing_s = float(flowing_s)
        self._lock = threading.Lock()
        self._started: float | None = None
        self._last: float | None = None          # arrival of the last sample
        self.quiet = False
        self.spells = 0                          # quiet spells since start
        self._quiet_from: float | None = None    # arrival of the last sample before the spell
        self._spell_said = False                 # this spell's "no sample" line was logged
        self._unsaid = 0                         # spells not logged since the last "no sample" line
        self._said_t = -math.inf                 # when the last "no sample" line was logged
        self._back: list[float] | None = None    # [first, last arrival, count] of the resumed stream
        self._status_t: float | None = None
        self._n = 0                              # samples since the last status line
        self._n_mean = 0                         # ... of them finite, in the mean accel
        self._sum = [0.0, 0.0, 0.0]

    def start(self, now: float) -> None:
        with self._lock:
            self._started = self._status_t = float(now)

    def sample(self, now: float, accel: tuple) -> None:
        """One sample arrived at `now`."""
        with self._lock:
            self._last = now
            self._n += 1
            if all(math.isfinite(v) for v in accel):
                self._n_mean += 1
                for i in range(3):
                    self._sum[i] += accel[i]
            if self.quiet:
                if self._back is None:
                    self._back = [now, now, 1]
                else:
                    self._back[1] = now
                    self._back[2] += 1

    def tick(self, now: float, contacts: int = 0) -> list[tuple[str, str]]:
        lines: list[tuple[str, str]] = []
        with self._lock:
            if self._started is None:
                return lines
            if not self.quiet and self._last is not None and now - self._last >= self.quiet_s:
                self.quiet, self._quiet_from, self._back = True, self._last, None
                self.spells += 1
                self._spell_said = now - self._said_t >= self.repeat_s
                if self._spell_said:
                    more = (f" ({self._unsaid} more quiet spell(s) since the last line)" if self._unsaid else "")
                    lines.append(("warning", f"MOSS IMU: no sample for {self.quiet_s:g} s - "
                                             f"IMU contacts and gyro prior OFF{more}"))
                    self._said_t, self._unsaid = now, 0
                else:
                    self._unsaid += 1
            if self.quiet and self._back is not None and now - self._back[0] >= self.rate_window_s:
                first, last, count = self._back
                if now - last > self.flowing_s:
                    self._back = None             # a burst that stopped again: the same spell, nothing said
                else:
                    rate = (count - 1) / (last - first) if last > first else 0.0
                    if self._spell_said:
                        lines.append(("info", f"MOSS IMU: samples back ({rate:.0f} Hz) after "
                                              f"{first - self._quiet_from:.1f} s quiet"))
                    self.quiet, self._back = False, None
            if now - self._status_t >= self.status_s:
                lines.append(self._status(now, contacts))
                self._status_t, self._n, self._n_mean, self._sum = now, 0, 0, [0.0, 0.0, 0.0]
        return lines

    def _status(self, now: float, contacts: int) -> tuple[str, str]:
        """The periodic line (under the lock): alive while samples arrive."""
        tail = f"{contacts} contact(s) so far, {self.spells} quiet spell(s)"
        if self._last is None:
            return ("warning", f"MOSS IMU: status waiting, no sample {now - self._started:.0f} s after start "
                               "- IMU contacts and gyro prior OFF")
        if now - self._last >= self.quiet_s:
            return ("warning", f"MOSS IMU: status QUIET, no sample for {now - self._last:.0f} s "
                               f"- IMU contacts and gyro prior OFF, {tail}")
        span, n = now - self._status_t, self._n
        mean = [v / self._n_mean for v in self._sum] if self._n_mean else [math.nan] * 3
        return ("info", f"MOSS IMU: status alive, {n} samples in {span:.0f} s ({n / span:.0f} Hz), mean accel "
                        f"({mean[0]:+.2f}, {mean[1]:+.2f}, {mean[2]:+.2f}) m/s^2, {tail}")


class ImuCsvLog:
    """Every sample appended to a CSV, flushed every CSV_FLUSH_S of wall
    time and on close. Never raises: an I/O error is logged once and the rows
    of that flush are dropped."""

    def __init__(self, path: str, flush_s: float = CSV_FLUSH_S) -> None:
        self.path = path
        self.flush_s = float(flush_s)
        self._rows: list[str] = []
        self._lock = threading.Lock()        # the row buffer
        self._io_lock = threading.Lock()     # one writer at a time (the sample thread, or close at stop)
        self._last_flush = time.monotonic()
        self._error_logged = False
        self.rows_written = 0

    @staticmethod
    def _cell(v: Any, fmt: str) -> str:
        return "" if v is None else format(v, fmt)

    def add(self, ts: float, accel: tuple, gyro: tuple, hp: float | None,
            commanded: tuple[int, int] | None) -> None:
        cmd = commanded if commanded is not None else (None, None)
        row = ",".join([format(ts, ".4f")] + [format(float(v), ".4f") for v in accel]
                       + [format(float(v), ".5f") for v in gyro]
                       + [self._cell(hp, ".4f"), self._cell(cmd[0], "d"), self._cell(cmd[1], "d")])
        with self._lock:
            self._rows.append(row + "\n")
        if time.monotonic() - self._last_flush >= self.flush_s:
            self.flush()

    def flush(self) -> None:
        with self._lock:
            rows, self._rows = self._rows, []
            self._last_flush = time.monotonic()
        if not rows:
            return
        with self._io_lock:
            self._write(rows)

    def _write(self, rows: list[str]) -> None:
        try:
            folder = os.path.dirname(self.path)
            if folder:
                os.makedirs(folder, exist_ok=True)
            new = not os.path.exists(self.path) or os.path.getsize(self.path) == 0
            with open(self.path, "a", encoding="ascii") as fh:
                if new:
                    fh.write(CSV_HEADER)
                fh.writelines(rows)
            self.rows_written += len(rows)
        except Exception as exc:  # noqa: BLE001 - the log is a study, never a reason to stop
            if not self._error_logged:
                self._error_logged = True
                logger.warning(f"MOSS_IMU_LOG: cannot write {self.path} ({type(exc).__name__}: {exc}); "
                               "samples are dropped, the contacts go on")

    def close(self) -> None:
        self.flush()


class BumpImu(Module):
    """IMU contacts on `bump` (in front) and `bump_rear` (behind), and the
    watchdog on the imu stream; see the module docstring."""

    imu: In[Imu]
    base_telemetry: In[String]
    bump: Out[Bool]
    bump_rear: Out[Bool]

    def __init__(self, axis: str | None = None, threshold: float | None = None,
                 debounce_s: float | None = None, log_path: str | None = None, **kwargs: Any) -> None:
        super().__init__(**kwargs)
        # the blueprint passes the four values read when it was built; a module built
        # without them (all None) reads the same environment itself
        if axis is None and threshold is None and debounce_s is None and log_path is None:
            env = bump_settings()
            axis, threshold, debounce_s, log_path = env["axis"], env["threshold"], env["debounce_s"], env["log_path"]
        self._detector = ImuContactDetector(
            axis=DEFAULT_AXIS if axis is None else axis,
            threshold=DEFAULT_THRESHOLD if threshold is None else threshold,
            debounce_s=DEFAULT_DEBOUNCE_S if debounce_s is None else debounce_s)
        self._csv = ImuCsvLog(log_path) if log_path else None
        self._command: tuple[tuple[int, int], float] | None = None   # (target_pct, monotonic instant it was true)
        self._unsubscribe: Any = None
        self._samples = 0
        self._error: str | None = None
        self._watchdog = ImuWatchdog()
        self._watch_stop = threading.Event()
        self._watch_thread: threading.Thread | None = None
        self._watch_error: str | None = None

    @property
    def detector(self) -> ImuContactDetector:
        return self._detector

    @property
    def watchdog(self) -> ImuWatchdog:
        return self._watchdog

    @rpc
    def start(self) -> None:
        super().start()
        try:
            self._unsubscribe = self.imu.subscribe(self.on_imu)
        except Exception as exc:  # noqa: BLE001 - no imu stream: no IMU contacts, the stack goes on
            logger.warning(f"MOSS IMU: cannot subscribe to imu ({type(exc).__name__}: {exc}); no IMU contacts")
        d = self._detector
        logger.info(f"MOSS IMU: contacts on (front on bump, rear on bump_rear), forward axis {d.axis}, "
                    f"threshold {d.threshold:.1f} m/s^2, debounce {d.debounce_s:.1f} s, command younger than "
                    f"{d.command_max_age_s:.1f} s, csv {self._csv.path if self._csv else 'off'}, "
                    f"watchdog: quiet after {self._watchdog.quiet_s:g} s, status every {self._watchdog.status_s:g} s")
        self._start_watchdog()

    def _start_watchdog(self) -> None:
        """The watchdog's clock starts now, and its thread unless it runs."""
        self._watchdog.start(time.monotonic())
        if self._watch_thread is None:
            self._watch_stop.clear()
            self._watch_thread = threading.Thread(target=self._watch_loop, name="moss-imu-watchdog", daemon=True)
            self._watch_thread.start()

    @rpc
    def stop(self) -> None:
        self._watch_stop.set()
        thread, self._watch_thread = self._watch_thread, None
        if thread is not None:
            thread.join(timeout=1.0)
        unsubscribe, self._unsubscribe = self._unsubscribe, None
        if unsubscribe is not None:
            try:
                unsubscribe()
            except Exception:  # noqa: BLE001
                pass
        if self._csv is not None:
            self._csv.close()
        super().stop()

    def _watch_loop(self) -> None:
        while not self._watch_stop.wait(WATCH_PERIOD_S):
            self.watch(time.monotonic())

    def watch(self, now: float) -> list[str]:
        """One look of the watchdog at `now` (monotonic): its lines are logged
        through the dimOS logger and returned. Never raises: the thread that
        says a sensor went quiet must not go quiet itself."""
        try:
            lines = self._watchdog.tick(now, self._detector.contacts)
        except Exception as exc:  # noqa: BLE001
            cause = f"{type(exc).__name__}: {exc}"
            if cause != self._watch_error:
                self._watch_error = cause
                logger.warning(f"MOSS IMU: watchdog look failed ({cause})")
            return []
        for level, text in lines:
            (logger.warning if level == "warning" else logger.info)(text)
        return [text for _, text in lines]

    async def handle_base_telemetry(self, msg: String) -> None:
        try:
            doc = json.loads(getattr(msg, "data", "") or "{}")
            target = doc.get("target_pct")
            pair = (int(target[0]), int(target[1]))
        except (TypeError, ValueError, AttributeError, IndexError, KeyError):
            return
        try:
            age = max(0.0, float(doc.get("firmware_age_s") or 0.0))
        except (TypeError, ValueError):
            age = 0.0
        self._command = (pair, time.monotonic() - age)

    def command_now(self) -> tuple[tuple[int, int] | None, float]:
        """(the last target_pct, its age in s), or (None, inf) before any."""
        cmd = self._command
        if cmd is None:
            return None, math.inf
        return cmd[0], max(0.0, time.monotonic() - cmd[1])

    def on_imu(self, msg: Imu) -> bool:
        """Every sample (the subscription's callback). True on a contact."""
        try:
            ts = float(msg.ts)
            a, g = msg.linear_acceleration, msg.angular_velocity
            accel = (float(a.x), float(a.y), float(a.z))
            gyro = (float(g.x), float(g.y), float(g.z))
            self._watchdog.sample(time.monotonic(), accel)
            self._first_sample(accel, msg)
            commanded, age = self.command_now()
            rebounds = self._detector.rebounds
            contact = self._detector.feed(ts, accel, commanded, command_age_s=age)
            if self._csv is not None:
                self._csv.add(ts, accel, gyro, self._detector.last_hp,
                              commanded if age <= self._detector.command_max_age_s else None)
            if contact:
                self._publish_contact()
            elif self._detector.rebounds != rebounds:
                r = self._detector.last_rebound or {}
                logger.info(rebound_line(r.get("hp", 0.0), r.get("commanded", (0, 0)), r.get("side", "rear"),
                                         r.get("after_s", 0.0)))
            return contact
        except Exception as exc:  # noqa: BLE001 - one bad sample never kills the subscription
            cause = f"{type(exc).__name__}: {exc}"
            if cause != self._error:
                self._error = cause
                logger.warning(f"MOSS IMU: sample skipped ({cause})")
            return False

    def _publish_contact(self) -> None:
        """Log first, publish second: a contact that was felt is on the record
        even when its stream refuses it."""
        c = self._detector.last_contact or {}
        side = c.get("side", "front")
        logger.warning(contact_line(c.get("hp", 0.0), self._detector.threshold, c.get("commanded", (0, 0)), side))
        name = contact_stream(side)
        out = getattr(self, name)
        if getattr(out, "transport", None) is None:
            return
        try:
            out.publish(Bool(data=True))
        except Exception as exc:  # noqa: BLE001 - said, not swallowed; the next contact tries again
            logger.warning(f"MOSS IMU: the contact was not published on {name} ({type(exc).__name__}: {exc})")

    def _first_sample(self, accel: tuple, msg: Imu) -> None:
        """The IMU announced alive at its first sample, with where gravity sits."""
        self._samples += 1
        if self._samples == 1:
            logger.info(f"MOSS IMU: first sample in {getattr(msg, 'frame_id', '?')}: accel "
                        f"({accel[0]:+.2f}, {accel[1]:+.2f}, {accel[2]:+.2f}) m/s^2")