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

cockpit: what it decides, in words, and the IMU as a bumper

metrox38a8669a8137
diff --git a/benches/bench_bump_imu_cold.py b/benches/bench_bump_imu_cold.py
new file mode 100644
index 0000000..bb6ef8f
--- /dev/null
+++ b/benches/bench_bump_imu_cold.py
@@ -0,0 +1,560 @@
+"""Cold bench: contacts from the IMU (moss_dimos/bump_imu.py) and the camera
+that opens its motion module in-process (moss_dimos/realsense_imu.py).
+
+Known input -> known output in physical units. Synthetic accel at 200 Hz in
+the frame the camera publishes (x right, y down, z forward), gravity on the
+down axis (y = -9.81 m/s^2), a little fixed-seed noise:
+
+ A. the rule - a 6 m/s^2 deceleration pulse of 60 ms while the tracks are
+ commanded (70, 60) -> exactly one contact at the pulse, its
+ high-pass about -6.0 m/s^2; no command, a (0, 0) command or
+ a stale one -> none; a 1.5 m/s^2 wobble -> none; two pulses
+ 0.3 s apart -> one, 1.0 s apart -> two; an acceleration
+ (wrong sign) while driving forward -> none, the same kick
+ while reversing -> one; a pivot takes either sign; a 2 m/s^2
+ tilt held 3 s, a camera pitched 10 or 30 deg, a slope of
+ 5 m/s^2 reached over 2 s -> none; a pulse before 0.25 s of
+ history -> none
+ B. the knobs - MOSS_BUMP_ACCEL=8 -> the 6 pulse is not a contact, a 9 one
+ is; MOSS_BUMP_AXIS=-z on a camera turned round -> the
+ contact again; bad values refused, defaults kept
+ C. the module - base_telemetry then the pulse sample by sample -> one
+ Bool(True) on bump and the log line with its numbers;
+ no command, a stale firmware line, an unwired Out, a NaN
+ sample, a bad telemetry line -> no bump and no exception
+ D. the CSV - MOSS_IMU_LOG writes a header and one row per sample with
+ the high-pass and the command; an unwritable path warns
+ once and never raises
+ E. the camera - MossRealSense on a fake pyrealsense2: the motion sensor
+ is found by name on the pipeline's own device, opened with
+ accel at its highest rate + gyro at imu_hz, started on the
+ parent's handler; stop() stops and closes it; no motion
+ module / a gyro rate not offered / a start that fails ->
+ a warning and the camera goes on; the parent publishes the
+ motion axes unrotated in <camera>_accel_optical_frame
+
+No hardware, no pyrealsense2 (a fake stands in), no bus.
+
+Run: PYTHONDONTWRITEBYTECODE=1 PYTHONPATH=. .venv/bin/python benches/bench_bump_imu_cold.py
+"""
+import asyncio
+import json
+import math
+import os
+import random
+import sys
+import tempfile
+import types
+from pathlib import Path
+
+ROOT = Path(__file__).resolve().parent.parent
+sys.path.insert(0, str(ROOT))
+
+OK = KO = 0
+
+
+def check(label, ok, detail=""):
+ global OK, KO
+ if ok:
+ OK += 1
+ else:
+ KO += 1
+ print(f" {'OK ' if ok else 'KO '} {label}{' - ' + str(detail) if detail and not ok else ''}")
+
+
+def finish():
+ print()
+ print(f"{OK} OK, {KO} KO")
+ print("TEST PASSED" if KO == 0 else "TEST FAILED")
+ sys.exit(1 if KO else 0)
+
+
+for var in [k for k in os.environ if k.startswith("MOSS_")]:
+ os.environ.pop(var, None)
+
+try:
+ from moss_dimos import bump_imu as bi
+except Exception as exc: # noqa: BLE001
+ check("moss_dimos.bump_imu imports (needs dimOS: run in the robot's venv)", False, f"{type(exc).__name__}: {exc}")
+ finish()
+
+HZ = 200.0
+G = 9.81
+T0 = 1_760_000_000.0 # an epoch second: the camera's timestamps are on the host clock
+
+
+def trace(duration_s=3.0, pulses=(), offset=None, wobble=None, pitch_deg=0.0, flip=False, seed=7):
+ """[(ts, (ax, ay, az))] at 200 Hz. pulses: (start s, length s, m/s^2 on the
+ forward axis); offset: (start s, length s, m/s^2) held; wobble: (amplitude,
+ Hz) on the forward axis; pitch_deg: camera nose down (gravity leaks onto
+ z); flip: the camera turned round (forward = -z)."""
+ rng = random.Random(seed)
+ out = []
+ n = int(round(duration_s * HZ))
+ pitch = math.radians(pitch_deg)
+ for i in range(n):
+ t = i / HZ
+ fwd = 0.0
+ for start, length, amp in pulses:
+ if start <= t < start + length - 1e-9:
+ fwd += amp
+ if offset and offset[0] <= t < offset[0] + offset[1]:
+ fwd += offset[2]
+ if wobble:
+ fwd += wobble[0] * math.sin(2.0 * math.pi * wobble[1] * t)
+ ay = -G * math.cos(pitch) + rng.uniform(-0.05, 0.05)
+ az = -G * math.sin(pitch) + fwd + rng.uniform(-0.05, 0.05)
+ ax = rng.uniform(-0.05, 0.05)
+ out.append((T0 + t, (ax, ay, -az if flip else az)))
+ return out
+
+
+def run(samples, commanded=(70, 60), age=0.0, det=None):
+ """Feed a trace; -> (detector, [(ts, hp)] of the contacts)."""
+ det = det or bi.ImuContactDetector()
+ hits = []
+ for ts, acc in samples:
+ if det.feed(ts, acc, commanded, command_age_s=age):
+ hits.append((ts, det.last_hp))
+ return det, hits
+
+
+PULSE = (1.5, 0.06, -6.0) # 60 ms of -6 m/s^2 on the forward axis, 1.5 s in
+
+# ── A. the rule ──────────────────────────────────────────────────────────
+print("A. ImuContactDetector on known traces (200 Hz, gravity on y)")
+det, hits = run(trace(pulses=[PULSE]))
+check("(a) a 6 m/s^2 deceleration pulse of 60 ms while commanded (70, 60) -> exactly one contact",
+ len(hits) == 1, f"{len(hits)} contacts")
+check(" ... at the first sample of the pulse (t = 1.500 s)",
+ len(hits) == 1 and abs(hits[0][0] - (T0 + 1.5)) < 1e-6, f"{[round(h[0] - T0, 3) for h in hits]}")
+check(" ... its high-pass is the pulse: -6.0 m/s^2 within 0.15",
+ len(hits) == 1 and abs(hits[0][1] - (-6.0)) <= 0.15, f"{[round(h[1], 3) for h in hits]}")
+check(" ... the detector counts one contact and remembers the command",
+ det.contacts == 1 and det.last_contact["commanded"] == (70, 60), f"{det.contacts} {det.last_contact}")
+check("(b) the same pulse with commanded (0, 0) -> none", run(trace(pulses=[PULSE]), commanded=(0, 0))[1] == [])
+check(" ... with no command at all -> none", run(trace(pulses=[PULSE]), commanded=None)[1] == [])
+check(" ... with a command 1.5 s old (older than 1 s) -> none", run(trace(pulses=[PULSE]), age=1.5)[1] == [])
+check(" ... with a command 0.9 s old -> one", len(run(trace(pulses=[PULSE]), age=0.9)[1]) == 1)
+check("(c) a 1.5 m/s^2 wobble at 8 Hz for 3 s, commanded -> none",
+ run(trace(wobble=(1.5, 8.0)))[1] == [])
+check(" ... at 2 Hz -> none", run(trace(wobble=(1.5, 2.0)))[1] == [])
+two_close = trace(pulses=[PULSE, (1.8, 0.06, -6.0)])
+check("(d) two pulses 0.3 s apart -> one contact (debounce 0.7 s)", len(run(two_close)[1]) == 1,
+ f"{len(run(two_close)[1])}")
+two_far = trace(pulses=[PULSE, (2.5, 0.06, -6.0)])
+far_hits = run(two_far)[1]
+check(" ... 1.0 s apart -> two, at 1.5 s and 2.5 s",
+ [round(h[0] - T0, 3) for h in far_hits] == [1.5, 2.5], f"{[round(h[0] - T0, 3) for h in far_hits]}")
+kick = trace(pulses=[(1.5, 0.06, +6.0)])
+check("(e) a +6 m/s^2 pulse (an acceleration) while commanded forward -> none", run(kick)[1] == [])
+check(" ... the same +6 kick while reversing (-70, -60) -> one (a contact behind)",
+ len(run(kick, commanded=(-70, -60))[1]) == 1)
+check(" ... a -6 pulse while reversing -> none (that is the reverse taking off)",
+ run(trace(pulses=[PULSE]), commanded=(-70, -60))[1] == [])
+check(" ... a pivot (70, -70) takes either sign: -6 -> one, +6 -> one",
+ len(run(trace(pulses=[PULSE]), commanded=(70, -70))[1]) == 1 and len(run(kick, commanded=(70, -70))[1]) == 1)
+check(" ... an arc with one track stopped (70, 0) is forward: -6 -> one, +6 -> none",
+ len(run(trace(pulses=[PULSE]), commanded=(70, 0))[1]) == 1 and run(kick, commanded=(70, 0))[1] == [])
+check("(f) tilt: +2 m/s^2 held on the forward axis for 3 s -> none (the running mean eats it)",
+ run(trace(duration_s=5.0, offset=(1.0, 3.0, 2.0)))[1] == [])
+check(" ... -2 m/s^2 held 3 s -> none", run(trace(duration_s=5.0, offset=(1.0, 3.0, -2.0)))[1] == [])
+check(" ... a camera pitched 10 deg nose down (1.70 m/s^2 of gravity on z), no pulse -> none",
+ run(trace(pitch_deg=10.0))[1] == [])
+check(" ... and the pulse on that pitched camera -> one",
+ len(run(trace(pitch_deg=10.0, pulses=[PULSE]))[1]) == 1)
+check(" ... a camera pitched 30 deg (4.9 m/s^2 of gravity on z, above the threshold raw) -> none",
+ run(trace(pitch_deg=30.0))[1] == [])
+ramp = [(ts, (a[0], a[1], a[2] - 5.0 * min(1.0, max(0.0, (ts - T0 - 0.5) / 2.0)))) for ts, a in trace(duration_s=4.0)]
+check(" ... onto a slope: -5 m/s^2 on the forward axis reached over 2 s and held -> none",
+ run(ramp)[1] == [])
+check("a pulse 0.1 s after the first sample (less than 0.25 s of history) -> none",
+ run(trace(pulses=[(0.1, 0.06, -6.0)]))[1] == [])
+d = bi.ImuContactDetector()
+check("a NaN sample or a short tuple is ignored, never raises",
+ d.feed(T0, (0.0, -G, float("nan")), (70, 60)) is False and d.feed(T0, (0.0,), (70, 60)) is False)
+check("command_direction: (70, 60) +1, (-70, -60) -1, (70, -70) 0, (0, 0) None, None None",
+ [bi.command_direction(c) for c in ((70, 60), (-70, -60), (70, -70), (0, 0), None)] == [1, -1, 0, None, None])
+
+# ── B. the knobs ─────────────────────────────────────────────────────────
+print("B. the knobs: MOSS_BUMP_ACCEL, MOSS_BUMP_AXIS, MOSS_BUMP_DEBOUNCE_S, MOSS_IMU_LOG")
+
+
+class LogSpy:
+ def __init__(self):
+ self.lines = []
+
+ def _record(self, msg, *args, **kwargs):
+ self.lines.append(msg % args if args else msg)
+
+ info = warning = error = debug = exception = _record
+
+
+real_logger = bi.logger
+spy = LogSpy()
+bi.logger = spy
+s = bi.bump_settings({})
+check("defaults: axis +z, 4.0 m/s^2, debounce 0.7 s, no CSV",
+ s == {"axis": "+z", "threshold": 4.0, "debounce_s": 0.7, "log_path": None}, f"{s}")
+eight = bi.ImuContactDetector.from_env({"MOSS_BUMP_ACCEL": "8"})
+check("(g) MOSS_BUMP_ACCEL=8 -> threshold 8.0 and the 6 m/s^2 pulse is not a contact",
+ eight.threshold == 8.0 and run(trace(pulses=[PULSE]), det=eight)[1] == [])
+check(" ... a 9 m/s^2 pulse is", len(run(trace(pulses=[(1.5, 0.06, -9.0)]),
+ det=bi.ImuContactDetector.from_env({"MOSS_BUMP_ACCEL": "8"}))[1]) == 1)
+flipped = bi.ImuContactDetector.from_env({"MOSS_BUMP_AXIS": "-z"})
+check("MOSS_BUMP_AXIS=-z on a camera turned round (forward = -z) -> the pulse is one contact again",
+ flipped.axis == "-z" and len(run(trace(pulses=[PULSE], flip=True), det=flipped)[1]) == 1)
+check(" ... and the default +z on that camera sees an acceleration instead -> none",
+ run(trace(pulses=[PULSE], flip=True))[1] == [])
+check("MOSS_BUMP_AXIS=x -> +x, Y -> +y", bi.bump_settings({"MOSS_BUMP_AXIS": "x"})["axis"] == "+x"
+ and bi.bump_settings({"MOSS_BUMP_AXIS": "Y"})["axis"] == "+y")
+spy.lines.clear()
+bad = bi.bump_settings({"MOSS_BUMP_AXIS": "w", "MOSS_BUMP_ACCEL": "-1", "MOSS_BUMP_DEBOUNCE_S": "abc"})
+check("MOSS_BUMP_AXIS=w, MOSS_BUMP_ACCEL=-1, MOSS_BUMP_DEBOUNCE_S=abc -> defaults kept, three warnings",
+ bad["axis"] == "+z" and bad["threshold"] == 4.0 and bad["debounce_s"] == 0.7 and len(spy.lines) == 3, f"{bad} {spy.lines}")
+check("MOSS_BUMP_ACCEL=0 and =1e9 refused (the threshold is above 0, up to 100)",
+ bi.bump_settings({"MOSS_BUMP_ACCEL": "0"})["threshold"] == 4.0 and bi.bump_settings({"MOSS_BUMP_ACCEL": "1e9"})["threshold"] == 4.0)
+check("MOSS_BUMP_DEBOUNCE_S=0 accepted (no debounce), =0.3 -> 0.3",
+ bi.bump_settings({"MOSS_BUMP_DEBOUNCE_S": "0"})["debounce_s"] == 0.0
+ and bi.bump_settings({"MOSS_BUMP_DEBOUNCE_S": "0.3"})["debounce_s"] == 0.3)
+nodeb = bi.ImuContactDetector(debounce_s=0.0)
+check(" ... with debounce 0 the 60 ms pulse is still one contact per sample above threshold (12 samples)",
+ len(run(trace(pulses=[PULSE]), det=nodeb)[1]) == 12, f"{len(run(trace(pulses=[PULSE]), det=bi.ImuContactDetector(debounce_s=0.0))[1])}")
+check("MOSS_IMU_LOG=~/imu.csv -> an absolute path under the home",
+ bi.bump_settings({"MOSS_IMU_LOG": "~/imu.csv"})["log_path"] == os.path.join(os.path.expanduser("~"), "imu.csv"))
+
+# ── C. the module ────────────────────────────────────────────────────────
+print("C. BumpImu: base_telemetry, then the samples -> bump and the log line")
+from dimos.msgs.geometry_msgs.Vector3 import Vector3 # noqa: E402
+from dimos.msgs.sensor_msgs.Imu import Imu # noqa: E402
+from dimos.msgs.std_msgs.String import String # noqa: E402
+from dimos_lcm.std_msgs import Bool # noqa: E402
+
+
+class BumpOut:
+ """The `bump` Out, recording what the module publishes; transport None =
+ a module built outside a blueprint."""
+
+ def __init__(self, wired=True):
+ self.transport = object() if wired else None
+ self.sent = []
+
+ def publish(self, msg):
+ self.sent.append((type(msg), bool(getattr(msg, "data", False))))
+
+
+def imu_msgs(samples, gyro=(0.01, -0.02, 0.03)):
+ return [Imu(angular_velocity=Vector3(*gyro), linear_acceleration=Vector3(*acc),
+ frame_id="camera_accel_optical_frame", ts=ts) for ts, acc in samples]
+
+
+def telemetry(target, age=0.1):
+ return String(data=json.dumps({"ts": 0.0, "target_pct": list(target), "firmware_age_s": age, "stall_count": 0}))
+
+
+MODULES = []
+
+
+def module(wired=True, **kwargs):
+ m = bi.BumpImu(**kwargs)
+ m.bump = BumpOut(wired)
+ MODULES.append(m)
+ return m
+
+
+check("streams: In imu (Imu), In base_telemetry (String), Out bump (Bool), nothing else",
+ set(bi.BumpImu.__annotations__) == {"imu", "base_telemetry", "bump"}, f"{sorted(bi.BumpImu.__annotations__)}")
+import moss_dimos.blueprints as _bp_mod # noqa: E402
+check("bump carries the SAME Bool class as the coordinator's (one stream type on the bus)", bi.Bool is _bp_mod.Bool)
+check("no async handle_imu: every sample goes through a plain subscription (the async handler keeps only the latest)",
+ not hasattr(bi.BumpImu, "handle_imu") and callable(getattr(bi.BumpImu, "on_imu", None)))
+check("start/stop keep @rpc", {"start", "stop"} <= set(bi.BumpImu.rpcs))
+
+spy.lines.clear()
+mod = module()
+check("a module built bare reads the environment's defaults: +z, 4.0, 0.7, no CSV",
+ (mod.detector.axis, mod.detector.threshold, mod.detector.debounce_s, mod._csv) == ("+z", 4.0, 0.7, None))
+asyncio.run(mod.handle_base_telemetry(telemetry((70, 60))))
+cmd, age = mod.command_now()
+check("(h) base_telemetry target_pct [70, 60], firmware age 0.1 s -> command (70, 60), about 0.1 s old",
+ cmd == (70, 60) and 0.09 <= age < 0.5, f"{cmd} {age:.3f}")
+results = [mod.on_imu(m) for m in imu_msgs(trace(pulses=[PULSE]))]
+check(" ... then the pulse sample by sample -> on_imu True exactly once", results.count(True) == 1, f"{results.count(True)}")
+check(" ... one Bool(True) published on bump", mod.bump.sent == [(Bool, True)], f"{mod.bump.sent}")
+contact_lines = [ln for ln in spy.lines if ln.startswith("IMU contact:")]
+check(" ... the log line: 'IMU contact: forward -6.0 m/s^2 (threshold 4.0) while commanded L=+70% R=+60%'",
+ contact_lines == ["IMU contact: forward -6.0 m/s^2 (threshold 4.0) while commanded L=+70% R=+60%"], f"{contact_lines}")
+check(" ... and the IMU is announced alive at its first sample (gravity on y)",
+ any(ln.startswith("MOSS IMU: first sample") and "-9.8" in ln for ln in spy.lines), f"{spy.lines[:3]}")
+check("every other line of the module starts with 'MOSS IMU:' (the launcher's log filter keeps MOSS lines), "
+ "none of them with 'IMU contact'",
+ all(ln.startswith("MOSS IMU:") for ln in spy.lines if not ln.startswith("IMU contact:"))
+ and len([ln for ln in spy.lines if ln.startswith("IMU contact")]) == 1, f"{spy.lines}")
+check("contact_line on a reverse contact: 'forward +5.2 ... L=-70% R=-60%'",
+ bi.contact_line(5.24, 4.0, (-70, -60)) == "IMU contact: forward +5.2 m/s^2 (threshold 4.0) while commanded L=-70% R=-60%")
+
+idle = module()
+asyncio.run(idle.handle_base_telemetry(telemetry((0, 0))))
+check("target_pct [0, 0] (tracks stopped) then the pulse -> no bump",
+ not any(idle.on_imu(m) for m in imu_msgs(trace(pulses=[PULSE]))) and idle.bump.sent == [])
+silent = module()
+check("no telemetry at all then the pulse -> no bump",
+ not any(silent.on_imu(m) for m in imu_msgs(trace(pulses=[PULSE]))) and silent.bump.sent == [])
+stale = module()
+asyncio.run(stale.handle_base_telemetry(telemetry((70, 60), age=2.0)))
+check("a firmware line 2 s old -> no command (age >= 2 s), no bump",
+ stale.command_now()[1] >= 2.0 and not any(stale.on_imu(m) for m in imu_msgs(trace(pulses=[PULSE]))) and stale.bump.sent == [])
+garbled = module()
+asyncio.run(garbled.handle_base_telemetry(telemetry((70, 60))))
+for bad_line in ("not json", json.dumps({"target_pct": "fast"}), json.dumps({"target_pct": [1]}), json.dumps([1, 2])):
+ asyncio.run(garbled.handle_base_telemetry(String(data=bad_line)))
+check("bad telemetry lines (not JSON, no pair, a list) leave the last good command in force",
+ garbled.command_now()[0] == (70, 60))
+unwired = module(wired=False)
+asyncio.run(unwired.handle_base_telemetry(telemetry((70, 60))))
+check("an unwired bump (no transport): the contact is detected and logged, nothing published, nothing raised",
+ [unwired.on_imu(m) for m in imu_msgs(trace(pulses=[PULSE]))].count(True) == 1 and unwired.bump.sent == [])
+weird = module()
+check("a sample with NaN or a message without fields -> False, never raises",
+ weird.on_imu(Imu(linear_acceleration=Vector3(float("nan"), 0.0, 0.0), ts=T0)) is False
+ and weird.on_imu(types.SimpleNamespace(ts="x")) is False)
+
+# ── D. the CSV ───────────────────────────────────────────────────────────
+print("D. MOSS_IMU_LOG: every sample to a CSV")
+tmp = tempfile.mkdtemp(prefix="imu_log_")
+csv_path = os.path.join(tmp, "sub", "imu.csv")
+os.environ["MOSS_IMU_LOG"] = csv_path
+logged = module()
+os.environ.pop("MOSS_IMU_LOG")
+check("(i) MOSS_IMU_LOG set -> the module built bare logs to that path", logged._csv is not None and logged._csv.path == csv_path)
+asyncio.run(logged.handle_base_telemetry(telemetry((70, 60))))
+pulse_msgs = imu_msgs(trace(pulses=[PULSE]))
+for m in pulse_msgs:
+ logged.on_imu(m)
+logged.stop()
+rows = Path(csv_path).read_text().splitlines() if os.path.exists(csv_path) else []
+check(" ... header ts,ax,ay,az,gx,gy,gz,hp,cmd_l,cmd_r then one row per sample (600)",
+ rows[:1] == ["ts,ax,ay,az,gx,gy,gz,hp,cmd_l,cmd_r"] and len(rows) == 1 + len(pulse_msgs), f"{len(rows)} lines")
+cells = [r.split(",") for r in rows[1:]]
+check(" ... ten cells per row", bool(cells) and all(len(c) == 10 for c in cells))
+pulse_row = next((c for c in cells if abs(float(c[0]) - (T0 + 1.5)) < 1e-3), None)
+check(" ... the row at 1.500 s: hp about -6.0, gyro (0.01, -0.02, 0.03), command 70 / 60",
+ pulse_row is not None and abs(float(pulse_row[7]) + 6.0) <= 0.15 and pulse_row[4:7] == ["0.01000", "-0.02000", "0.03000"]
+ and pulse_row[8:] == ["70", "60"], f"{pulse_row}")
+check(" ... the first rows (window warming up) leave hp empty", cells[0][7] == "", f"{cells[0]}")
+flushes = bi.ImuCsvLog(os.path.join(tmp, "every.csv"), flush_s=0.0)
+flushes.add(T0, (0.0, -G, 0.0), (0.0, 0.0, 0.0), None, None)
+check("a flush period of 0 writes at once; no command -> empty command cells",
+ Path(tmp, "every.csv").read_text().splitlines()[1].endswith(",,,"), Path(tmp, "every.csv").read_text())
+spy.lines.clear()
+blocked = bi.ImuCsvLog(os.path.join(tmp, "every.csv", "cannot", "be.csv"), flush_s=0.0) # a file stands where a folder must be
+for i in range(3):
+ blocked.add(T0 + i, (0.0, -G, 0.0), (0.0, 0.0, 0.0), 0.1, (70, 60))
+blocked.close()
+check("an unwritable path: no exception, ONE warning naming MOSS_IMU_LOG",
+ len([ln for ln in spy.lines if "MOSS_IMU_LOG" in ln]) == 1 and blocked.rows_written == 0, f"{spy.lines}")
+bi.logger = real_logger
+
+# ── E. the camera: MossRealSense on a fake pyrealsense2 ──────────────────
+print("E. MossRealSense: the motion module on the pipeline's own device (fake pyrealsense2)")
+
+
+class _Enum:
+ def __init__(self, name):
+ self.name = name
+
+ def __repr__(self):
+ return self.name
+
+
+rs = types.ModuleType("pyrealsense2")
+rs.stream = types.SimpleNamespace(accel=_Enum("accel"), gyro=_Enum("gyro"), depth=_Enum("depth"), color=_Enum("color"))
+rs.format = types.SimpleNamespace(motion_xyz32f=_Enum("motion_xyz32f"), motion_raw=_Enum("motion_raw"), z16=_Enum("z16"))
+rs.camera_info = types.SimpleNamespace(name=_Enum("name"))
+
+
+class FakeProfile:
+ def __init__(self, kind, fps, fmt=None):
+ self.kind, self.rate, self.fmt = kind, fps, fmt or rs.format.motion_xyz32f
+
+ def stream_type(self):
+ return self.kind
+
+ def format(self):
+ return self.fmt
+
+ def fps(self):
+ return self.rate
+
+ def is_motion_stream_profile(self):
+ return self.kind in (rs.stream.accel, rs.stream.gyro)
+
+ def as_motion_stream_profile(self):
+ return self
+
+ def __repr__(self):
+ return f"{self.kind}@{self.rate}"
+
+
+class FakeSensor:
+ def __init__(self, name, profiles, fail_start=False):
+ self.name, self.profiles, self.fail_start = name, profiles, fail_start
+ self.opened, self.callback, self.calls = None, None, []
+
+ def get_info(self, key):
+ return self.name
+
+ def get_stream_profiles(self):
+ return list(self.profiles)
+
+ def open(self, profiles):
+ self.calls.append("open")
+ self.opened = list(profiles)
+
+ def start(self, callback):
+ self.calls.append("start")
+ if self.fail_start:
+ raise RuntimeError("failed to set power state")
+ self.callback = callback
+
+ def stop(self):
+ self.calls.append("stop")
+
+ def close(self):
+ self.calls.append("close")
+
+
+class FakeDevice:
+ def __init__(self, sensors):
+ self.sensors = sensors
+
+ def query_sensors(self):
+ return list(self.sensors)
+
+
+class FakePipelineProfile:
+ def __init__(self, device):
+ self.device = device
+
+ def get_device(self):
+ return self.device
+
+
+def d455(fail_start=False, gyro_rates=(200, 400), accel_rates=(100, 200)):
+ motion = FakeSensor("Motion Module",
+ [FakeProfile(rs.stream.accel, r) for r in accel_rates]
+ + [FakeProfile(rs.stream.accel, 400, rs.format.motion_raw)] # a raw format is never picked
+ + [FakeProfile(rs.stream.gyro, r) for r in gyro_rates], fail_start=fail_start)
+ stereo = FakeSensor("Stereo Module", [FakeProfile(rs.stream.depth, 30, rs.format.z16)])
+ rgb = FakeSensor("RGB Camera", [FakeProfile(rs.stream.color, 30)])
+ return FakeDevice([stereo, rgb, motion]), motion
+
+
+sys.modules["pyrealsense2"] = rs
+from moss_dimos import realsense_imu as ri # noqa: E402
+from dimos.hardware.sensors.camera.realsense.camera import RealSenseCamera # noqa: E402
+
+dev, motion = d455()
+check("find_motion_sensor: the sensor named 'Motion Module' among stereo / RGB / motion",
+ ri.find_motion_sensor(dev, rs) is motion)
+check("... and None on a camera without one", ri.find_motion_sensor(FakeDevice(dev.sensors[:2]), rs) is None)
+acc, gyr = ri.motion_profiles(motion, rs, 200)
+check("motion_profiles(imu_hz=200): accel at its highest float rate (200, not the raw 400), gyro at 200",
+ (acc.kind, acc.rate, acc.fmt, gyr.kind, gyr.rate) == (rs.stream.accel, 200, rs.format.motion_xyz32f, rs.stream.gyro, 200),
+ f"{acc} {gyr}")
+try:
+ ri.motion_profiles(motion, rs, 300)
+ check("imu_hz=300 -> ValueError naming the offered gyro rates [200, 400]", False)
+except ValueError as exc:
+ check("imu_hz=300 -> ValueError naming the offered gyro rates [200, 400]", "[200, 400]" in str(exc), str(exc))
+
+spy2 = LogSpy()
+real_ri_logger = ri.logger
+ri.logger = spy2
+cam = ri.MossRealSense(enable_imu=True, imu_hz=200)
+MODULES.append(cam)
+from dimos.core.coordination.blueprints import BlueprintAtom # noqa: E402
+_streams = lambda cls: {(r.name, r.type, r.direction) for r in BlueprintAtom.create(cls, {}).streams} # noqa: E731
+check("MossRealSense is a RealSenseCamera with exactly the parent's streams (imu among them), no new one",
+ issubclass(ri.MossRealSense, RealSenseCamera) and not vars(ri.MossRealSense).get("__annotations__")
+ and _streams(ri.MossRealSense) == _streams(RealSenseCamera) and ("imu", Imu, "out") in _streams(RealSenseCamera))
+check("its stop keeps @rpc", "stop" in ri.MossRealSense.rpcs)
+dev, motion = d455()
+cam._profile = FakePipelineProfile(dev)
+cam._start_imu()
+check("_start_imu: the motion sensor of the pipeline's own device is opened with [accel 200 Hz, gyro 200 Hz]",
+ cam._imu_sensor is motion and [(p.kind, p.rate) for p in motion.opened] == [(rs.stream.accel, 200), (rs.stream.gyro, 200)],
+ f"{motion.opened}")
+check("... and started on the parent's own handler (_on_motion_frame), no second pipeline",
+ motion.calls == ["open", "start"] and motion.callback == cam._on_motion_frame and cam._imu_pipeline is None)
+check("... announced in the log with the rates and the axes",
+ any("accel 200 Hz, gyro 200 Hz" in ln and "z forward" in ln for ln in spy2.lines), f"{spy2.lines}")
+
+
+class FakeMotionFrame:
+ def __init__(self, kind, ms, xyz):
+ self.kind, self.ms, self.xyz = kind, ms, xyz
+
+ def as_motion_frame(self):
+ return self
+
+ def get_motion_data(self):
+ return types.SimpleNamespace(x=self.xyz[0], y=self.xyz[1], z=self.xyz[2])
+
+ def get_timestamp(self):
+ return self.ms
+
+ def get_profile(self):
+ return FakeProfile(self.kind, 200)
+
+
+published = []
+cam.imu = types.SimpleNamespace(publish=published.append)
+base_ms = T0 * 1000.0
+motion.callback(FakeMotionFrame(rs.stream.accel, base_ms, (0.10, -9.81, 0.20)))
+motion.callback(FakeMotionFrame(rs.stream.accel, base_ms + 10.0, (0.30, -9.81, 0.40)))
+motion.callback(FakeMotionFrame(rs.stream.gyro, base_ms + 5.0, (0.01, 0.02, 0.03)))
+imu = published[0] if published else None
+check("the parent publishes one Imu per gyro sample, accel interpolated to it: (0.20, -9.81, 0.30) at +5 ms",
+ imu is not None and len(published) == 1
+ and all(abs(a - b) < 1e-4 for a, b in zip((imu.linear_acceleration.x, imu.linear_acceleration.y, imu.linear_acceleration.z),
+ (0.20, -9.81, 0.30)))
+ and abs(imu.ts - (T0 + 0.005)) < 1e-6, f"{imu!r}")
+check("... unrotated (librealsense's axes as they come: y down = -9.81) in camera_accel_optical_frame",
+ imu is not None and imu.frame_id == "camera_accel_optical_frame"
+ and (imu.angular_velocity.x, imu.angular_velocity.y, imu.angular_velocity.z) == (0.01, 0.02, 0.03))
+cam.stop()
+check("stop(): the motion sensor is stopped then closed, and forgotten",
+ motion.calls == ["open", "start", "stop", "close"] and cam._imu_sensor is None, f"{motion.calls}")
+
+spy2.lines.clear()
+bare = ri.MossRealSense(enable_imu=True, imu_hz=200)
+MODULES.append(bare)
+bare._profile = FakePipelineProfile(FakeDevice(d455()[0].sensors[:2]))
+bare._start_imu()
+bare._start_imu()
+check("a camera without a motion module: ONE warning for two starts, no sensor, no exception",
+ bare._imu_sensor is None and len([ln for ln in spy2.lines if "no motion module" in ln]) == 1, f"{spy2.lines}")
+spy2.lines.clear()
+odd = ri.MossRealSense(enable_imu=True, imu_hz=300)
+MODULES.append(odd)
+odd._profile = FakePipelineProfile(d455()[0])
+odd._start_imu()
+check("imu_hz=300 (not offered): a warning naming the rates, the camera goes on (no exception)",
+ odd._imu_sensor is None and any("[200, 400]" in ln and "colour and depth go on" in ln for ln in spy2.lines), f"{spy2.lines}")
+spy2.lines.clear()
+fdev, fmotion = d455(fail_start=True)
+failing = ri.MossRealSense(enable_imu=True, imu_hz=200)
+MODULES.append(failing)
+failing._profile = FakePipelineProfile(fdev)
+failing._start_imu()
+check("a start that fails ('failed to set power state'): the sensor is closed again, a warning, the camera goes on",
+ failing._imu_sensor is None and fmotion.calls == ["open", "start", "close"]
+ and any("failed to set power state" in ln for ln in spy2.lines), f"{fmotion.calls} {spy2.lines}")
+ri.logger = real_ri_logger
+del sys.modules["pyrealsense2"]
+
+MODULES.clear()
+import shutil # noqa: E402
+shutil.rmtree(tmp, ignore_errors=True)
+finish()
diff --git a/benches/bench_moss_blueprints_cold.py b/benches/bench_moss_blueprints_cold.py
index 516eb4e..e09e4b4 100644
--- a/benches/bench_moss_blueprints_cold.py
+++ b/benches/bench_moss_blueprints_cold.py
@@ -32,6 +32,16 @@ Known input -> known output in physical units. Groups:
MOSS_CAMERA=0 / MOSS_RECORD=0 / MOSS_WRIST=0 / MOSS_RERUN=1
(and off / no / false: robot_profile.env_on);
MossMemoryConfig defaults; base_telemetry on the coordinator
+ J. MOSS_IMU - unset / 0 / off: dimOS's RealSenseCamera with enable_imu False
+ and no BumpImu, the same blueprint as without the switch;
+ MOSS_IMU=1: MossRealSense with enable_imu True and imu_hz
+ 200 and every other camera kwarg unchanged, a BumpImu atom
+ with the knobs read at build time, its bump on the stream
+ the coordinator / explorer publish and the planner / costmap
+ read, imu from the camera, base_telemetry from the
+ coordinator, no stream type conflict; refused without the
+ camera; both classes pickle by reference into a clean
+ interpreter; importing the blueprints imports neither
I. the worker - what a dimOS forkserver worker does: unpickle the
coordinator class in a clean interpreter -> the module is
imported, 'moss' is registered, the registry builds a
@@ -499,6 +509,129 @@ pl = atom(exp, "RecoveringPlanner").kwargs
check("planner footprint = MOSS width 0.34, pivot 0.50",
pl.get("robot_width") == 0.34 and pl.get("robot_rotation_diameter") == 0.50)
+# ── J. MOSS_IMU: contacts from the IMU ───────────────────────────────────
+print("J. MOSS_IMU: the in-process IMU camera and BumpImu")
+from collections import defaultdict # noqa: E402
+from dimos.core.coordination.module_coordinator import _verify_no_name_conflicts # noqa: E402
+from dimos.hardware.sensors.camera.realsense.camera import RealSenseCamera # noqa: E402
+from dimos.msgs.sensor_msgs.Imu import Imu # noqa: E402
+from dimos.msgs.std_msgs.String import String # noqa: E402
+
+
+def wiring(bp):
+ """{(stream name after remaps, type): {(module class name, direction)}} - the
+ grouping dimOS's _connect_streams gives one transport each."""
+ out = defaultdict(set)
+ for b in bp.active_blueprints:
+ for conn in b.streams:
+ name = bp.remapping_map.get((b.name, conn.name), conn.name)
+ out[(name, conn.type)].add((b.module.__name__, conn.direction))
+ return out
+
+
+def blueprint_text(bp):
+ return [(b.module.__name__, repr(sorted(b.kwargs.items()))) for b in bp.blueprints]
+
+
+cam_default = atom(nav, "RealSenseCamera")
+check("MOSS_IMU unset: the camera atom is dimOS's own RealSenseCamera, enable_imu False, no imu_hz kwarg",
+ cam_default.module is RealSenseCamera and cam_default.kwargs.get("enable_imu") is False
+ and "imu_hz" not in cam_default.kwargs)
+check("MOSS_IMU unset: no BumpImu, no MossRealSense in moss-nav or moss-explore",
+ not ({"BumpImu", "MossRealSense"} & (names(nav) | names(exp))), f"{sorted(names(exp))}")
+for spelled_off in ("0", "off"):
+ os.environ["MOSS_IMU"] = spelled_off
+ same_nav, same_exp = mb.build_moss_nav(), mb.build_moss_explore()
+ check(f"MOSS_IMU={spelled_off}: moss-nav and moss-explore are the blueprints of the unset switch, kwarg for kwarg",
+ blueprint_text(same_nav) == blueprint_text(nav) and blueprint_text(same_exp) == blueprint_text(exp))
+os.environ["MOSS_IMU"] = "1"
+imu_nav, imu_exp = mb.build_moss_nav(), mb.build_moss_explore()
+from moss_dimos.bump_imu import BumpImu # noqa: E402
+from moss_dimos.realsense_imu import MossRealSense # noqa: E402
+check("MOSS_IMU=1: moss-nav has MossRealSense and BumpImu, and no stock RealSenseCamera",
+ {"MossRealSense", "BumpImu"} <= names(imu_nav) and "RealSenseCamera" not in names(imu_nav), f"{sorted(names(imu_nav))}")
+check("MOSS_IMU=1: moss-explore too, with the planner and the explorer still there",
+ {"MossRealSense", "BumpImu", "RecoveringPlanner", "Explorer2"} <= names(imu_exp) and "RealSenseCamera" not in names(imu_exp))
+cam_imu = atom(imu_nav, "MossRealSense")
+check("the camera atom is moss_dimos.realsense_imu.MossRealSense, a RealSenseCamera subclass",
+ cam_imu.module is MossRealSense and issubclass(MossRealSense, RealSenseCamera))
+check("... enable_imu True, imu_hz 200", cam_imu.kwargs.get("enable_imu") is True and cam_imu.kwargs.get("imu_hz") == 200,
+ f"{cam_imu.kwargs.get('enable_imu')} {cam_imu.kwargs.get('imu_hz')}")
+strip = lambda kw: repr(sorted((k, v) for k, v in kw.items() if k not in ("enable_imu", "imu_hz"))) # noqa: E731
+check("... every other camera kwarg as without the switch (640x480 @ 5 fps, depth, no cloud, the mount)",
+ strip(cam_imu.kwargs) == strip(cam_default.kwargs), f"{strip(cam_imu.kwargs)}")
+bk = atom(imu_nav, "BumpImu").kwargs
+check("BumpImu atom: forward axis +z, 4.0 m/s^2, debounce 0.7 s, no CSV (the defaults, read at build time)",
+ bk == {"axis": "+z", "threshold": 4.0, "debounce_s": 0.7, "log_path": None}, f"{bk}")
+w = wiring(imu_exp)
+bump_side = w.get(("bump", mb.Bool), set())
+check("moss-explore: BumpImu's bump is the stream the coordinator and the explorer publish on",
+ {("BumpImu", "out"), ("MossControlCoordinator", "out"), ("Explorer2", "out")} <= bump_side, f"{sorted(bump_side)}")
+check("... and the one the planner's escape and the costmap's contact memory read",
+ {("RecoveringPlanner", "in"), ("MossCostMap", "in")} <= bump_side, f"{sorted(bump_side)}")
+check("... the sounds read it too when the sound module is in the build",
+ "MossSounds" not in names(imu_exp) or ("MossSounds", "in") in bump_side, f"{sorted(names(imu_exp))}")
+check("... no second bump stream of another type",
+ [k for k in w if k[0] == "bump"] == [("bump", mb.Bool)], f"{[k for k in w if k[0] == 'bump']}")
+check("imu: MossRealSense out -> BumpImu in (and the odometry's In)",
+ {("MossRealSense", "out"), ("BumpImu", "in"), ("LidarOdometry", "in")} <= w.get(("imu", Imu), set()),
+ f"{sorted(w.get(('imu', Imu), set()))}")
+check("base_telemetry: the coordinator out -> BumpImu in",
+ {("MossControlCoordinator", "out"), ("BumpImu", "in")} <= w.get(("base_telemetry", String), set()))
+try:
+ _verify_no_name_conflicts(imu_exp)
+ _verify_no_name_conflicts(imu_nav)
+ check("dimOS's stream type check accepts both MOSS_IMU builds", True)
+except ValueError as exc:
+ check("dimOS's stream type check accepts both MOSS_IMU builds", False, str(exc)[:300])
+check("moss-nav with MOSS_IMU=1: bump reaches the costmap (no planner there)",
+ {("BumpImu", "out"), ("MossCostMap", "in")} <= wiring(imu_nav).get(("bump", mb.Bool), set()))
+check("MOSS_IMU=1 leaves the gyro prior off (MOSS_GYRO unset)", atom(imu_nav, "LidarOdometry").kwargs.get("use_gyro_prior") is False)
+os.environ.update(MOSS_BUMP_ACCEL="6", MOSS_BUMP_AXIS="-z", MOSS_BUMP_DEBOUNCE_S="0.5", MOSS_IMU_LOG="/data/imu.csv", MOSS_GYRO="1")
+tuned = mb.build_moss_nav()
+for k in ("MOSS_BUMP_ACCEL", "MOSS_BUMP_AXIS", "MOSS_BUMP_DEBOUNCE_S", "MOSS_IMU_LOG", "MOSS_GYRO"):
+ os.environ.pop(k)
+check("MOSS_BUMP_ACCEL=6 MOSS_BUMP_AXIS=-z MOSS_BUMP_DEBOUNCE_S=0.5 MOSS_IMU_LOG=/data/imu.csv -> the BumpImu kwargs",
+ atom(tuned, "BumpImu").kwargs == {"axis": "-z", "threshold": 6.0, "debounce_s": 0.5, "log_path": "/data/imu.csv"},
+ f"{atom(tuned, 'BumpImu').kwargs}")
+check("MOSS_IMU=1 MOSS_GYRO=1: the gyro prior rides the in-process IMU (MossRealSense, prior on)",
+ atom(tuned, "MossRealSense").kwargs.get("enable_imu") is True and atom(tuned, "LidarOdometry").kwargs.get("use_gyro_prior") is True)
+os.environ["MOSS_CAMERA"] = "0"
+nocam = mb.build_moss_nav()
+os.environ.pop("MOSS_CAMERA")
+check("MOSS_IMU=1 MOSS_CAMERA=0: no camera, so no BumpImu either (refused with a warning)",
+ not ({"MossRealSense", "RealSenseCamera", "BumpImu"} & names(nocam)), f"{sorted(names(nocam))}")
+os.environ.pop("MOSS_IMU")
+pick = run_child("""
+ import json, pickle, sys
+ import moss_dimos.blueprints as b
+ print(json.dumps({"imported": [m for m in ("moss_dimos.bump_imu", "moss_dimos.realsense_imu") if m in sys.modules]}))
+""")
+try:
+ got_imp = json.loads(last_line(pick))
+except ValueError:
+ got_imp = {}
+ print(pick.stderr.decode(errors="replace")[-600:])
+check("importing moss_dimos.blueprints imports neither bump_imu nor realsense_imu", got_imp.get("imported") == [], f"{got_imp}")
+payload_imu = pickle.dumps((MossRealSense, BumpImu))
+check("MossRealSense and BumpImu pickle by reference to their modules",
+ b"moss_dimos.realsense_imu" in payload_imu and b"moss_dimos.bump_imu" in payload_imu)
+unp = run_child("""
+ import json, pickle, sys
+ cam, bump = pickle.loads(sys.stdin.buffer.read())
+ from dimos.hardware.sensors.camera.realsense.camera import RealSenseCamera
+ print(json.dumps({"cam": f"{cam.__module__}.{cam.__name__}", "bump": f"{bump.__module__}.{bump.__name__}",
+ "camera": issubclass(cam, RealSenseCamera)}))
+""", stdin=payload_imu)
+try:
+ got_unp = json.loads(last_line(unp))
+except ValueError:
+ got_unp = {}
+ print(unp.stderr.decode(errors="replace")[-600:])
+check("... and unpickle in a clean interpreter, as a dimOS worker does",
+ got_unp == {"cam": "moss_dimos.realsense_imu.MossRealSense", "bump": "moss_dimos.bump_imu.BumpImu", "camera": True},
+ f"{got_unp}")
+
os.environ["MOSS_CAMERA"] = "0"
os.environ["MOSS_RECORD"] = "0"
bare = mb.build_moss_nav()
diff --git a/benches/bench_moss_cockpit_cold.py b/benches/bench_moss_cockpit_cold.py
index 3c31db4..a0d6bc5 100644
--- a/benches/bench_moss_cockpit_cold.py
+++ b/benches/bench_moss_cockpit_cold.py
@@ -45,6 +45,22 @@ Every check feeds a value we know and reads back a value in physical units
page's own pose choice (state vs scan, the younger wins), stall view
and stall latch (a stall the 1 Hz read misses still holds the badge
4 s) run in node
+ I. decisions - every decision line of the catalogue (the run's own text) -> its kind,
+ source and a short text that keeps the line's numbers; twenty other
+ lines -> nothing; the planner's replan reasons shown only with their
+ consequence ("Replanning.", "Goal abandoned"), the run's three stuck
+ verdicts the planner ignored -> nothing; one whole real exploration
+ replayed (goal 10, contact 30, waiting 44, no event from its 181 twist /
+ telemetry lines; its 11-line wait folded into one row) and its first
+ contact with every module's lines -> the expected events and words;
+ the state word on known sequences (the 3 s / 4 s holds, BACKING OFF
+ until the escape ends, rank, stop, escape, tapped goals the planner
+ drops, ISO times); echoes; bad records never raise; /decisions.json
+ live on a 1.1 MB main.jsonl (the 1 MiB window, at most 2 Hz, a torn
+ last line, a reason and its consequence in two reads, a rewritten
+ file, the stack gone and its events kept, the next run, a stack with
+ no registry entry) and in the demo (the scripted cycle, STOP, explore,
+ a goal 3 s after the stop, UTC times, the stop keeping the feed)
F. hygiene - no dimos/serial/torch import, no LAN address in the server
C, D and the run-change checks of G need dimos_lcm (pure python, installed with
@@ -1369,6 +1385,1098 @@ console.log(JSON.stringify({
jr.get("holdEdge") == [True, False] and jr.get("stalledNoHold") is True,
json.dumps({k: jr.get(k) for k in ("holdEdge", "stalledNoHold")}))
+# ---------------------------------------------------------------------------
+# One real exploration, every line its decision-making modules printed, in order: minutes:seconds on the
+# run's clock, the module, the event text (lines cut where the run's export cut them; the board's serial
+# id zeroed). Twists and telemetry included: they must make no event.
+RUN_NAV = r"""
+0:27.040 moss_adapter.py MOSS base: firmware ramp set to 5 5 ms per % (rise fall)
+0:27.041 moss_adapter.py MOSS base: connected on /dev/serial/by-id/usb-Adafruit_QT_Py_ESP32-S3_No_PSRAM_00:00:00:00:00:00-if00, firmware mode=0 geometry=MossGeom
+1:25.257 wavefront_frontier Received exploration start command via LCM
+1:25.259 explorer2.py exploration start: distance exp 2, cap 3 m, radius 4 m (0 = no limit; the radius is centred on the first pose)
+1:25.260 wavefront_frontier Started autonomous frontier exploration
+1:25.928 explorer2.py goal 1: (-1.27, 1.28) 2.0 m away, 35 frontier cells, 12 clusters, chosen in 114 ms
+1:25.929 global_planner.py Got new goal
+1:25.929 global_planner.py Cancelling goal.
+1:26.039 global_planner.py Found safe goal.
+1:26.104 moss_adapter.py MOSS base: twist vx=+0.000 vy=+0.000(ignored) wz=+0.000 -> tracks L=+0% R=+0% (limit 100%)
+1:26.111 local_planner.py changed state
+1:26.214 moss_adapter.py MOSS base: 'drive' refused not_armed (firmware reason: stop) - re-arming once
+1:26.216 moss_adapter.py MOSS base: twist vx=+0.000 vy=+0.000(ignored) wz=+0.385 -> tracks L=-100% R=+100% (limit 100%)
+1:26.311 moss_adapter.py MOSS telemetry: out=[0, 0] target=[0, 0] bus=10972 mV ticks=[387605, 451441] mode=0
+1:27.525 local_planner.py changed state
+1:27.532 moss_adapter.py MOSS base: twist vx=+0.282 vy=+0.000(ignored) wz=+0.211 -> tracks L=+56% R=+69% (limit 100%)
+1:27.633 moss_adapter.py MOSS base: twist vx=+0.307 vy=+0.000(ignored) wz=+0.200 -> tracks L=+62% R=+74% (limit 100%)
+1:27.734 moss_adapter.py MOSS base: twist vx=+0.329 vy=+0.000(ignored) wz=+0.200 -> tracks L=+67% R=+79% (limit 100%)
+1:27.835 moss_adapter.py MOSS base: twist vx=+0.352 vy=+0.000(ignored) wz=+0.200 -> tracks L=+72% R=+84% (limit 100%)
+1:27.937 moss_adapter.py MOSS base: twist vx=+0.359 vy=+0.000(ignored) wz=+0.200 -> tracks L=+74% R=+86% (limit 100%)
+1:28.038 moss_adapter.py MOSS base: twist vx=+0.370 vy=+0.000(ignored) wz=+0.200 -> tracks L=+76% R=+88% (limit 100%)
+1:28.130 moss_adapter.py MOSS base: twist vx=+0.378 vy=+0.000(ignored) wz=+0.200 -> tracks L=+78% R=+90% (limit 100%)
+1:28.231 moss_adapter.py MOSS base: twist vx=+0.381 vy=+0.000(ignored) wz=-0.200 -> tracks L=+91% R=+79% (limit 100%)
+1:28.340 moss_adapter.py MOSS base: twist vx=+0.370 vy=+0.000(ignored) wz=-0.200 -> tracks L=+88% R=+76% (limit 100%)
+1:28.358 moss_adapter.py MOSS telemetry: out=[36, 79] target=[91, 79] bus=10804 mV ticks=[386244, 454604] mode=2
+1:28.441 moss_adapter.py MOSS base: twist vx=+0.363 vy=+0.000(ignored) wz=-0.200 -> tracks L=+87% R=+75% (limit 100%)
+1:28.534 moss_adapter.py MOSS base: twist vx=+0.367 vy=+0.000(ignored) wz=+0.200 -> tracks L=+76% R=+88% (limit 100%)
+1:28.636 moss_adapter.py MOSS base: twist vx=+0.372 vy=+0.000(ignored) wz=+0.200 -> tracks L=+77% R=+89% (limit 100%)
+1:28.853 moss_adapter.py MOSS base: twist vx=+0.331 vy=+0.000(ignored) wz=+0.200 -> tracks L=+68% R=+79% (limit 100%)
+1:28.929 local_planner.py Obstacle detected ahead, stopping local planner.
+1:28.929 local_planner.py changed state
+1:28.930 global_planner.py Replanning path due to obstacle found.
+1:28.932 global_planner.py Replanning.
+1:28.932 global_planner.py Cancelling goal.
+1:28.936 global_planner.py Found safe goal.
+1:28.937 moss_adapter.py MOSS base: twist vx=+0.000 vy=+0.000(ignored) wz=+0.000 -> tracks L=+0% R=+0% (limit 100%)
+1:28.973 local_planner.py changed state
+1:28.978 moss_adapter.py MOSS base: twist vx=+0.000 vy=+0.000(ignored) wz=+0.385 -> tracks L=-100% R=+100% (limit 100%)
+1:30.380 moss_adapter.py MOSS telemetry: out=[-100, 100] target=[-100, 100] bus=10492 mV ticks=[387720, 460484] mode=2
+1:31.415 local_planner.py changed state
+1:31.424 moss_adapter.py MOSS base: twist vx=+0.276 vy=+0.000(ignored) wz=-0.223 -> tracks L=+68% R=+55% (limit 100%)
+1:31.518 moss_adapter.py MOSS base: twist vx=+0.256 vy=+0.000(ignored) wz=-0.262 -> tracks L=+65% R=+50% (limit 100%)
+1:31.620 moss_adapter.py MOSS base: twist vx=+0.240 vy=+0.000(ignored) wz=-0.295 -> tracks L=+62% R=+50% (limit 100%)
+1:31.719 moss_adapter.py MOSS base: twist vx=+0.227 vy=+0.000(ignored) wz=-0.322 -> tracks L=+60% R=+50% (limit 100%)
+1:31.820 moss_adapter.py MOSS base: twist vx=+0.217 vy=+0.000(ignored) wz=-0.342 -> tracks L=+58% R=+50% (limit 100%)
+1:31.924 moss_adapter.py MOSS base: twist vx=+0.218 vy=+0.000(ignored) wz=-0.341 -> tracks L=+59% R=+50% (limit 100%)
+1:32.129 moss_adapter.py MOSS base: twist vx=+0.214 vy=+0.000(ignored) wz=-0.348 -> tracks L=+58% R=+50% (limit 100%)
+1:32.331 moss_adapter.py MOSS base: twist vx=+0.212 vy=+0.000(ignored) wz=-0.353 -> tracks L=+57% R=+50% (limit 100%)
+1:32.399 moss_adapter.py MOSS telemetry: out=[58, 50] target=[58, 50] bus=10816 mV ticks=[385182, 463840] mode=2
+1:32.530 moss_adapter.py MOSS base: twist vx=+0.204 vy=+0.000(ignored) wz=-0.370 -> tracks L=+56% R=+50% (limit 100%)
+1:34.429 moss_adapter.py MOSS telemetry: out=[56, 50] target=[56, 50] bus=10800 mV ticks=[388105, 466783] mode=2
+1:35.142 moss_adapter.py MOSS base: twist vx=+0.000 vy=+0.000(ignored) wz=-0.385 -> tracks L=+100% R=-100% (limit 100%)
+1:35.935 moss_adapter.py MOSS base: twist vx=+0.200 vy=+0.000(ignored) wz=-0.385 -> tracks L=+56% R=+50% (limit 100%)
+1:36.464 moss_adapter.py MOSS telemetry: out=[56, -9] target=[56, 50] bus=10828 mV ticks=[391710, 468411] mode=2
+1:37.848 moss_adapter.py MOSS base: twist vx=+0.000 vy=+0.000(ignored) wz=-0.385 -> tracks L=+100% R=-100% (limit 100%)
+1:38.490 moss_adapter.py MOSS telemetry: out=[100, -25] target=[100, -100] bus=10728 mV ticks=[395223, 471199] mode=2
+1:38.658 moss_adapter.py MOSS base: twist vx=+0.200 vy=+0.000(ignored) wz=-0.385 -> tracks L=+56% R=+50% (limit 100%)
+1:40.511 moss_adapter.py MOSS telemetry: out=[56, 50] target=[56, 50] bus=10720 mV ticks=[396064, 471064] mode=2
+1:40.814 moss_adapter.py MOSS base: tracks frozen at L=+56% R=+50% - pushing at 75 % for 1 s before calling it a contact
+1:40.815 moss_adapter.py MOSS base: twist vx=+0.200 vy=+0.000(ignored) wz=-0.385 -> tracks L=+75% R=+75% (limit 100%)
+1:41.833 moss_adapter.py MOSS base: twist vx=+0.200 vy=+0.000(ignored) wz=-0.385 -> tracks L=+56% R=+50% (limit 100%)
+1:42.537 moss_adapter.py MOSS telemetry: out=[56, 50] target=[56, 50] bus=10708 mV ticks=[396196, 471761] mode=2
+1:42.947 moss_adapter.py MOSS base: tracks frozen at L=+56% R=+50% - pushing at 75 % for 1 s before calling it a contact
+1:42.953 moss_adapter.py MOSS base: twist vx=+0.200 vy=+0.000(ignored) wz=-0.385 -> tracks L=+75% R=+75% (limit 100%)
+1:43.962 moss_adapter.py MOSS base: twist vx=+0.200 vy=+0.000(ignored) wz=-0.385 -> tracks L=+56% R=+50% (limit 100%)
+1:44.270 moss_adapter.py MOSS base: twist vx=+0.000 vy=+0.000(ignored) wz=-0.385 -> tracks L=+100% R=-100% (limit 100%)
+1:44.565 moss_adapter.py MOSS telemetry: out=[100, 0] target=[100, -100] bus=10548 mV ticks=[396329, 472266] mode=2
+1:44.668 moss_adapter.py MOSS base: twist vx=+0.200 vy=+0.000(ignored) wz=-0.385 -> tracks L=+56% R=+50% (limit 100%)
+1:45.171 moss_adapter.py MOSS base: tracks frozen at L=+56% R=+50% - pushing at 75 % for 1 s before calling it a contact
+1:45.177 moss_adapter.py MOSS base: twist vx=+0.200 vy=+0.000(ignored) wz=-0.385 -> tracks L=+75% R=+75% (limit 100%)
+1:45.371 moss_adapter.py MOSS base: twist vx=+0.000 vy=+0.000(ignored) wz=-0.385 -> tracks L=+100% R=-100% (limit 100%)
+1:46.171 moss_adapter.py MOSS base: twist vx=+0.200 vy=+0.000(ignored) wz=-0.385 -> tracks L=+56% R=+50% (limit 100%)
+1:46.589 moss_adapter.py MOSS telemetry: out=[56, 0] target=[56, 50] bus=10808 mV ticks=[396507, 472231] mode=2
+1:47.396 moss_adapter.py MOSS base: tracks frozen at L=+56% R=+50% - pushing at 75 % for 1 s before calling it a contact
+1:47.398 moss_adapter.py MOSS base: twist vx=+0.200 vy=+0.000(ignored) wz=-0.385 -> tracks L=+75% R=+75% (limit 100%)
+1:48.416 moss_adapter.py MOSS base: twist vx=+0.200 vy=+0.000(ignored) wz=-0.385 -> tracks L=+56% R=+50% (limit 100%)
+1:48.611 moss_adapter.py MOSS telemetry: out=[56, 53] target=[56, 50] bus=10712 mV ticks=[396707, 472992] mode=2
+1:48.707 moss_adapter.py MOSS base: twist vx=+0.000 vy=+0.000(ignored) wz=-0.385 -> tracks L=+100% R=-100% (limit 100%)
+1:50.018 moss_adapter.py MOSS base: twist vx=+0.200 vy=+0.000(ignored) wz=-0.385 -> tracks L=+56% R=+50% (limit 100%)
+1:50.631 moss_adapter.py MOSS telemetry: out=[56, 0] target=[56, 50] bus=10812 mV ticks=[397813, 471591] mode=2
+1:50.825 moss_adapter.py MOSS base: twist vx=+0.343 vy=+0.000(ignored) wz=+0.200 -> tracks L=+70% R=+82% (limit 100%)
+1:50.929 moss_adapter.py MOSS base: twist vx=+0.345 vy=+0.000(ignored) wz=+0.200 -> tracks L=+71% R=+82% (limit 100%)
+1:51.027 moss_adapter.py MOSS base: twist vx=+0.343 vy=+0.000(ignored) wz=+0.200 -> tracks L=+70% R=+82% (limit 100%)
+1:51.224 moss_adapter.py MOSS base: twist vx=+0.349 vy=+0.000(ignored) wz=+0.200 -> tracks L=+72% R=+83% (limit 100%)
+1:51.325 moss_adapter.py MOSS base: twist vx=+0.355 vy=+0.000(ignored) wz=+0.200 -> tracks L=+73% R=+85% (limit 100%)
+1:51.427 moss_adapter.py MOSS base: twist vx=+0.363 vy=+0.000(ignored) wz=+0.200 -> tracks L=+75% R=+87% (limit 100%)
+1:51.529 moss_adapter.py MOSS base: twist vx=+0.369 vy=+0.000(ignored) wz=+0.200 -> tracks L=+76% R=+88% (limit 100%)
+1:51.629 moss_adapter.py MOSS base: twist vx=+0.372 vy=+0.000(ignored) wz=+0.200 -> tracks L=+77% R=+89% (limit 100%)
+1:51.721 moss_adapter.py MOSS base: twist vx=+0.378 vy=+0.000(ignored) wz=+0.200 -> tracks L=+78% R=+90% (limit 100%)
+1:51.823 moss_adapter.py MOSS base: twist vx=+0.384 vy=+0.000(ignored) wz=-0.200 -> tracks L=+91% R=+79% (limit 100%)
+1:51.922 moss_adapter.py MOSS base: twist vx=+0.375 vy=+0.000(ignored) wz=-0.200 -> tracks L=+89% R=+77% (limit 100%)
+1:52.024 moss_adapter.py MOSS base: twist vx=+0.370 vy=+0.000(ignored) wz=-0.200 -> tracks L=+88% R=+76% (limit 100%)
+1:52.130 moss_adapter.py MOSS base: twist vx=+0.363 vy=+0.000(ignored) wz=-0.200 -> tracks L=+87% R=+75% (limit 100%)
+1:52.229 moss_adapter.py MOSS base: twist vx=+0.359 vy=+0.000(ignored) wz=-0.200 -> tracks L=+86% R=+74% (limit 100%)
+1:52.332 moss_adapter.py MOSS base: twist vx=+0.356 vy=+0.000(ignored) wz=-0.200 -> tracks L=+85% R=+73% (limit 100%)
+1:52.433 moss_adapter.py MOSS base: twist vx=+0.347 vy=+0.000(ignored) wz=-0.200 -> tracks L=+83% R=+71% (limit 100%)
+1:52.524 moss_adapter.py MOSS base: twist vx=+0.340 vy=+0.000(ignored) wz=-0.200 -> tracks L=+82% R=+70% (limit 100%)
+1:52.627 moss_adapter.py MOSS base: twist vx=+0.000 vy=+0.000(ignored) wz=-0.385 -> tracks L=+100% R=-100% (limit 100%)
+1:52.667 moss_adapter.py MOSS telemetry: out=[82, 70] target=[82, 70] bus=10456 mV ticks=[398810, 474169] mode=2
+1:53.930 moss_adapter.py MOSS base: twist vx=+0.200 vy=+0.000(ignored) wz=-0.385 -> tracks L=+56% R=+50% (limit 100%)
+1:54.435 moss_adapter.py MOSS base: twist vx=+0.374 vy=+0.000(ignored) wz=+0.200 -> tracks L=+77% R=+89% (limit 100%)
+1:54.534 moss_adapter.py MOSS base: twist vx=+0.377 vy=+0.000(ignored) wz=+0.200 -> tracks L=+78% R=+90% (limit 100%)
+1:54.636 moss_adapter.py MOSS base: twist vx=+0.375 vy=+0.000(ignored) wz=+0.200 -> tracks L=+77% R=+89% (limit 100%)
+1:54.691 moss_adapter.py MOSS telemetry: out=[75, 0] target=[77, 89] bus=10688 mV ticks=[399405, 473685] mode=2
+1:54.730 moss_adapter.py MOSS base: twist vx=+0.375 vy=+0.000(ignored) wz=+0.200 -> tracks L=+78% R=+89% (limit 100%)
+1:54.937 moss_adapter.py MOSS base: twist vx=+0.377 vy=+0.000(ignored) wz=+0.200 -> tracks L=+78% R=+90% (limit 100%)
+1:55.038 moss_adapter.py MOSS base: twist vx=+0.381 vy=+0.000(ignored) wz=+0.200 -> tracks L=+79% R=+91% (limit 100%)
+1:55.133 moss_adapter.py MOSS base: twist vx=+0.384 vy=+0.000(ignored) wz=-0.200 -> tracks L=+91% R=+79% (limit 100%)
+1:55.234 moss_adapter.py MOSS base: twist vx=+0.378 vy=+0.000(ignored) wz=-0.200 -> tracks L=+90% R=+78% (limit 100%)
+1:55.336 moss_adapter.py MOSS base: twist vx=+0.369 vy=+0.000(ignored) wz=-0.200 -> tracks L=+88% R=+76% (limit 100%)
+1:55.437 moss_adapter.py MOSS base: twist vx=+0.363 vy=+0.000(ignored) wz=-0.200 -> tracks L=+87% R=+75% (limit 100%)
+1:55.531 moss_adapter.py MOSS base: twist vx=+0.354 vy=+0.000(ignored) wz=-0.200 -> tracks L=+84% R=+73% (limit 100%)
+1:55.632 moss_adapter.py MOSS base: twist vx=+0.344 vy=+0.000(ignored) wz=-0.200 -> tracks L=+82% R=+71% (limit 100%)
+1:55.733 moss_adapter.py MOSS base: twist vx=+0.340 vy=+0.000(ignored) wz=-0.200 -> tracks L=+81% R=+70% (limit 100%)
+1:55.834 moss_adapter.py MOSS base: twist vx=+0.333 vy=+0.000(ignored) wz=-0.200 -> tracks L=+80% R=+68% (limit 100%)
+1:55.937 moss_adapter.py MOSS base: twist vx=+0.330 vy=+0.000(ignored) wz=-0.200 -> tracks L=+79% R=+67% (limit 100%)
+1:56.044 moss_adapter.py MOSS base: twist vx=+0.321 vy=+0.000(ignored) wz=-0.200 -> tracks L=+77% R=+65% (limit 100%)
+1:56.132 moss_adapter.py MOSS base: twist vx=+0.310 vy=+0.000(ignored) wz=-0.200 -> tracks L=+75% R=+63% (limit 100%)
+1:56.233 moss_adapter.py MOSS base: twist vx=+0.303 vy=+0.000(ignored) wz=-0.200 -> tracks L=+73% R=+61% (limit 100%)
+1:56.335 moss_adapter.py MOSS base: twist vx=+0.299 vy=+0.000(ignored) wz=-0.200 -> tracks L=+72% R=+60% (limit 100%)
+1:56.439 moss_adapter.py MOSS base: twist vx=+0.299 vy=+0.000(ignored) wz=-0.200 -> tracks L=+72% R=+61% (limit 100%)
+1:56.638 moss_adapter.py MOSS base: twist vx=+0.304 vy=+0.000(ignored) wz=-0.200 -> tracks L=+73% R=+62% (limit 100%)
+1:56.713 moss_adapter.py MOSS telemetry: out=[72, 61] target=[72, 61] bus=10560 mV ticks=[400033, 475192] mode=2
+1:56.740 moss_adapter.py MOSS base: twist vx=+0.308 vy=+0.000(ignored) wz=-0.200 -> tracks L=+74% R=+63% (limit 100%)
+1:56.849 moss_adapter.py MOSS base: twist vx=+0.311 vy=+0.000(ignored) wz=-0.200 -> tracks L=+75% R=+63% (limit 100%)
+1:56.934 moss_adapter.py MOSS base: twist vx=+0.313 vy=+0.000(ignored) wz=-0.200 -> tracks L=+75% R=+64% (limit 100%)
+1:57.036 moss_adapter.py MOSS base: twist vx=+0.315 vy=+0.000(ignored) wz=-0.200 -> tracks L=+76% R=+64% (limit 100%)
+1:57.220 moss_adapter.py MOSS base: STALL #1: tracks commanded L=+76% R=+64% but the encoders moved L=+1 R=+0 ticks in 1.0 s - stopped (the encoders are the bump
+1:57.225 moss_adapter.py MOSS base: 'drive' refused not_armed (firmware reason: drive) - re-arming once
+1:57.445 moss_adapter.py MOSS base: twist vx=+0.313 vy=+0.000(ignored) wz=-0.200 -> tracks L=+75% R=+64% (limit 100%)
+1:57.503 recovering_planner BUMP received
+1:57.505 local_planner.py changed state
+1:57.505 local_planner.py Local planner loop exited due to stop event.
+1:57.509 moss_adapter.py MOSS base: twist vx=+0.000 vy=+0.000(ignored) wz=+0.000 -> tracks L=+0% R=+0% (limit 100%)
+1:57.570 moss_adapter.py MOSS base: twist vx=-0.100 vy=+0.000(ignored) wz=+0.000 -> tracks L=-50% R=-50% (limit 100%)
+1:58.752 moss_adapter.py MOSS telemetry: out=[-50, -50] target=[-50, -50] bus=10764 mV ticks=[399100, 474264] mode=2
+1:59.605 recovering_planner Contact escape
+1:59.606 global_planner.py Cancelling goal.
+1:59.615 moss_adapter.py MOSS base: twist vx=+0.000 vy=+0.000(ignored) wz=+0.000 -> tracks L=+0% R=+0% (limit 100%)
+1:59.663 explorer2.py planner gave up on that goal: excluded until the map or the viewpoint changes
+1:59.835 explorer2.py goal 2: (0.18, -1.47) 2.6 m away, 20 frontier cells, 19 clusters, chosen in 170 ms
+1:59.836 global_planner.py Got new goal
+1:59.837 global_planner.py Cancelling goal.
+1:59.840 global_planner.py Found safe goal.
+1:59.934 local_planner.py changed state
+1:59.949 moss_adapter.py MOSS base: twist vx=+0.000 vy=+0.000(ignored) wz=-0.385 -> tracks L=+100% R=-100% (limit 100%)
+2:00.772 moss_adapter.py MOSS telemetry: out=[100, -100] target=[100, -100] bus=10260 mV ticks=[398571, 472912] mode=2
+2:00.974 moss_adapter.py MOSS base: STALL #2: tracks commanded L=+100% R=-100% but the encoders moved L=+1259 R=+6 ticks in 1.0 s - stopped (the encoders are the
+2:00.977 moss_adapter.py MOSS base: 'drive' refused not_armed (firmware reason: drive) - re-arming once
+2:01.015 recovering_planner BUMP received
+2:01.017 local_planner.py changed state
+2:01.018 local_planner.py Local planner loop exited due to stop event.
+2:01.024 latest_only.py twist_command: consumer behind, executing the newest command only (1 stale dropped so far)
+2:01.029 moss_adapter.py MOSS base: twist vx=+0.000 vy=+0.000(ignored) wz=+0.000 -> tracks L=+0% R=+0% (limit 100%)
+2:01.079 moss_adapter.py MOSS base: twist vx=-0.100 vy=+0.000(ignored) wz=+0.000 -> tracks L=-50% R=-50% (limit 100%)
+2:02.793 moss_adapter.py MOSS telemetry: out=[-50, -50] target=[-50, -50] bus=10784 mV ticks=[396941, 470728] mode=2
+2:03.199 recovering_planner Contact escape
+2:03.200 global_planner.py Cancelling goal.
+2:03.203 explorer2.py planner gave up on that goal: excluded until the map or the viewpoint changes
+2:03.204 moss_adapter.py MOSS base: twist vx=+0.000 vy=+0.000(ignored) wz=+0.000 -> tracks L=+0% R=+0% (limit 100%)
+2:03.360 explorer2.py goal 3: (-1.67, -0.12) 3.9 m away (beyond the cap: nothing safe within it), 110 frontier cells, 19 clusters, chosen in 156 ms
+2:03.361 global_planner.py Got new goal
+2:03.361 global_planner.py Cancelling goal.
+2:03.372 global_planner.py Found safe goal.
+2:03.412 local_planner.py changed state
+2:03.425 moss_adapter.py MOSS base: twist vx=+0.000 vy=+0.000(ignored) wz=-0.385 -> tracks L=+100% R=-100% (limit 100%)
+2:04.615 moss_adapter.py MOSS base: STALL #3: tracks commanded L=+100% R=-100% but the encoders moved L=+1001 R=-19 ticks in 1.0 s - stopped (the encoders are th
+2:04.618 moss_adapter.py MOSS base: 'drive' refused not_armed (firmware reason: drive) - re-arming once
+2:04.819 moss_adapter.py MOSS telemetry: out=[21, -21] target=[100, -100] bus=10828 mV ticks=[397164, 469780] mode=2
+2:05.032 recovering_planner BUMP received
+2:05.032 local_planner.py changed state
+2:05.033 local_planner.py Local planner loop exited due to stop event.
+2:05.039 moss_adapter.py MOSS base: twist vx=+0.000 vy=+0.000(ignored) wz=+0.000 -> tracks L=+0% R=+0% (limit 100%)
+2:05.100 moss_adapter.py MOSS base: twist vx=-0.100 vy=+0.000(ignored) wz=+0.000 -> tracks L=-50% R=-50% (limit 100%)
+2:06.862 moss_adapter.py MOSS telemetry: out=[-50, -50] target=[-50, -50] bus=10776 mV ticks=[395591, 468248] mode=2
+2:07.299 recovering_planner Contact escape
+2:07.301 global_planner.py Cancelling goal.
+2:07.304 explorer2.py planner gave up on that goal: excluded until the map or the viewpoint changes
+2:07.306 moss_adapter.py MOSS base: twist vx=+0.000 vy=+0.000(ignored) wz=+0.000 -> tracks L=+0% R=+0% (limit 100%)
+2:07.507 explorer2.py goal 4: (1.58, 3.53) 5.1 m away (beyond the cap: nothing safe within it), 27 frontier cells, 20 clusters, chosen in 201 ms
+2:07.508 global_planner.py Got new goal
+2:07.508 global_planner.py Cancelling goal.
+2:07.512 global_planner.py Found safe goal.
+2:07.620 local_planner.py changed state
+2:07.630 moss_adapter.py MOSS base: twist vx=+0.000 vy=+0.000(ignored) wz=-0.385 -> tracks L=+100% R=-100% (limit 100%)
+2:08.680 moss_adapter.py MOSS base: STALL #4: tracks commanded L=+100% R=-100% but the encoders moved L=+1051 R=+8 ticks in 1.0 s - stopped (the encoders are the
+2:08.688 moss_adapter.py MOSS base: 'drive' refused not_armed (firmware reason: drive) - re-arming once
+2:08.883 moss_adapter.py MOSS telemetry: out=[7, -7] target=[100, -100] bus=10852 mV ticks=[395707, 467024] mode=2
+2:09.044 local_planner.py changed state
+2:09.045 recovering_planner BUMP received
+2:09.045 local_planner.py Local planner loop exited due to stop event.
+2:09.058 moss_adapter.py MOSS base: twist vx=-0.100 vy=+0.000(ignored) wz=+0.000 -> tracks L=-50% R=-50% (limit 100%)
+2:09.068 moss_adapter.py MOSS base: twist vx=+0.000 vy=+0.000(ignored) wz=+0.000 -> tracks L=+0% R=+0% (limit 100%)
+2:09.106 moss_adapter.py MOSS base: twist vx=-0.100 vy=+0.000(ignored) wz=+0.000 -> tracks L=-50% R=-50% (limit 100%)
+2:10.907 moss_adapter.py MOSS telemetry: out=[-50, -50] target=[-50, -50] bus=10748 mV ticks=[394273, 465582] mode=2
+2:11.399 recovering_planner Contact escape
+2:11.400 global_planner.py Cancelling goal.
+2:11.406 explorer2.py planner gave up on that goal: excluded until the map or the viewpoint changes
+2:11.406 moss_adapter.py MOSS base: twist vx=+0.000 vy=+0.000(ignored) wz=+0.000 -> tracks L=+0% R=+0% (limit 100%)
+2:11.547 explorer2.py goal 5: (1.03, -2.72) 7.6 m away (beyond the cap: nothing safe within it), 184 frontier cells, 20 clusters, chosen in 139 ms
+2:11.575 global_planner.py Got new goal
+2:11.576 global_planner.py Cancelling goal.
+2:11.581 global_planner.py Found safe goal.
+2:11.637 global_planner.py Robot is stuck. Replanning.
+2:11.654 local_planner.py changed state
+2:11.662 moss_adapter.py MOSS base: twist vx=+0.000 vy=+0.000(ignored) wz=-0.385 -> tracks L=+100% R=-100% (limit 100%)
+2:12.823 moss_adapter.py MOSS base: STALL #5: tracks commanded L=+100% R=-100% but the encoders moved L=+1100 R=-4 ticks in 1.0 s - stopped (the encoders are the
+2:12.837 moss_adapter.py MOSS base: 'drive' refused not_armed (firmware reason: drive) - re-arming once
+2:12.924 moss_adapter.py MOSS telemetry: out=[100, -100] target=[100, -100] bus=10204 mV ticks=[394167, 464360] mode=2
+2:13.065 recovering_planner BUMP received
+2:13.066 local_planner.py changed state
+2:13.067 local_planner.py Local planner loop exited due to stop event.
+2:13.075 moss_adapter.py MOSS base: twist vx=-0.100 vy=+0.000(ignored) wz=+0.000 -> tracks L=-50% R=-50% (limit 100%)
+2:13.085 moss_adapter.py MOSS base: twist vx=+0.000 vy=+0.000(ignored) wz=+0.000 -> tracks L=+0% R=+0% (limit 100%)
+2:13.126 moss_adapter.py MOSS base: twist vx=-0.100 vy=+0.000(ignored) wz=+0.000 -> tracks L=-50% R=-50% (limit 100%)
+2:14.842 recovering_planner Contact escape
+2:14.842 global_planner.py Cancelling goal.
+2:14.852 moss_adapter.py MOSS base: twist vx=+0.000 vy=+0.000(ignored) wz=+0.000 -> tracks L=+0% R=+0% (limit 100%)
+2:14.855 explorer2.py planner gave up on that goal: excluded until the map or the viewpoint changes
+2:15.018 explorer2.py goal 6: (2.18, -2.32) 11.9 m away (beyond the cap: nothing safe within it), 99 frontier cells, 20 clusters, chosen in 162 ms
+2:15.019 global_planner.py Got new goal
+2:15.019 global_planner.py Cancelling goal.
+2:15.080 global_planner.py Travelling to goal 1.2850097467377768m away from requested goal.
+2:15.081 global_planner.py Found safe goal.
+2:15.144 local_planner.py changed state
+2:15.185 moss_adapter.py MOSS base: twist vx=+0.000 vy=+0.000(ignored) wz=-0.385 -> tracks L=+100% R=-100% (limit 100%)
+2:15.259 moss_adapter.py MOSS telemetry: out=[0, 0] target=[0, 0] bus=10876 mV ticks=[392170, 462189] mode=2
+2:16.271 moss_adapter.py MOSS base: STALL #6: tracks commanded L=+100% R=-100% but the encoders moved L=+1041 R=+2 ticks in 1.0 s - stopped (the encoders are the
+2:16.281 moss_adapter.py MOSS base: 'drive' refused not_armed (firmware reason: drive) - re-arming once
+2:16.575 local_planner.py changed state
+2:16.577 recovering_planner BUMP received
+2:16.576 local_planner.py Local planner loop exited due to stop event.
+2:16.586 moss_adapter.py MOSS base: twist vx=+0.000 vy=+0.000(ignored) wz=+0.000 -> tracks L=+0% R=+0% (limit 100%)
+2:16.590 moss_adapter.py MOSS base: twist vx=-0.100 vy=+0.000(ignored) wz=+0.000 -> tracks L=-50% R=-50% (limit 100%)
+2:17.282 moss_adapter.py MOSS telemetry: out=[-32, -50] target=[-50, -50] bus=10752 mV ticks=[393278, 462086] mode=2
+2:18.891 recovering_planner Contact escape
+2:18.891 global_planner.py Cancelling goal.
+2:18.894 explorer2.py planner gave up on that goal: excluded until the map or the viewpoint changes
+2:18.897 moss_adapter.py MOSS base: twist vx=+0.000 vy=+0.000(ignored) wz=+0.000 -> tracks L=+0% R=+0% (limit 100%)
+2:19.016 explorer2.py 10 of 20 clusters are on a recently failed goal: waiting for the map or the viewpoint to change, not stopping
+2:20.112 explorer2.py 10 of 20 clusters are on a recently failed goal: waiting for the map or the viewpoint to change, not stopping
+2:21.216 explorer2.py 10 of 20 clusters are on a recently failed goal: waiting for the map or the viewpoint to change, not stopping
+2:22.310 explorer2.py 10 of 20 clusters are on a recently failed goal: waiting for the map or the viewpoint to change, not stopping
+2:23.458 explorer2.py 10 of 20 clusters are on a recently failed goal: waiting for the map or the viewpoint to change, not stopping
+2:24.534 explorer2.py 10 of 20 clusters are on a recently failed goal: waiting for the map or the viewpoint to change, not stopping
+2:25.632 explorer2.py 10 of 20 clusters are on a recently failed goal: waiting for the map or the viewpoint to change, not stopping
+2:26.734 explorer2.py 10 of 20 clusters are on a recently failed goal: waiting for the map or the viewpoint to change, not stopping
+2:27.865 explorer2.py 10 of 20 clusters are on a recently failed goal: waiting for the map or the viewpoint to change, not stopping
+2:28.960 explorer2.py 10 of 20 clusters are on a recently failed goal: waiting for the map or the viewpoint to change, not stopping
+2:30.066 explorer2.py 9 of 20 clusters are on a recently failed goal: waiting for the map or the viewpoint to change, not stopping
+2:31.155 explorer2.py starvation escape: the world stayed silent for 12 asks - reopening the oldest failed goal at (-1.27, 1.28)
+2:32.291 explorer2.py 9 of 20 clusters are on a recently failed goal: waiting for the map or the viewpoint to change, not stopping
+2:33.382 explorer2.py 9 of 20 clusters are on a recently failed goal: waiting for the map or the viewpoint to change, not stopping
+2:34.476 explorer2.py 9 of 20 clusters are on a recently failed goal: waiting for the map or the viewpoint to change, not stopping
+2:35.549 explorer2.py 9 of 20 clusters are on a recently failed goal: waiting for the map or the viewpoint to change, not stopping
+2:36.686 explorer2.py 9 of 20 clusters are on a recently failed goal: waiting for the map or the viewpoint to change, not stopping
+2:37.806 explorer2.py 9 of 20 clusters are on a recently failed goal: waiting for the map or the viewpoint to change, not stopping
+2:38.912 explorer2.py 9 of 20 clusters are on a recently failed goal: waiting for the map or the viewpoint to change, not stopping
+2:40.051 explorer2.py 9 of 20 clusters are on a recently failed goal: waiting for the map or the viewpoint to change, not stopping
+2:41.151 explorer2.py 9 of 20 clusters are on a recently failed goal: waiting for the map or the viewpoint to change, not stopping
+2:42.291 explorer2.py 9 of 20 clusters are on a recently failed goal: waiting for the map or the viewpoint to change, not stopping
+2:43.380 explorer2.py 9 of 20 clusters are on a recently failed goal: waiting for the map or the viewpoint to change, not stopping
+2:44.493 explorer2.py starvation escape: the world stayed silent for 12 asks - reopening the oldest failed goal at (0.18, -1.47)
+2:45.690 explorer2.py goal 7: (0.18, -1.47) 1.7 m away, 20 frontier cells, 20 clusters, chosen in 195 ms
+2:45.691 global_planner.py Got new goal
+2:45.691 global_planner.py Cancelling goal.
+2:45.693 global_planner.py Found safe goal.
+2:45.748 local_planner.py changed state
+2:45.756 moss_adapter.py MOSS base: twist vx=+0.000 vy=+0.000(ignored) wz=-0.385 -> tracks L=+100% R=-100% (limit 100%)
+2:45.763 moss_adapter.py MOSS telemetry: out=[0, 0] target=[0, 0] bus=10868 mV ticks=[390416, 459210] mode=2
+2:47.786 moss_adapter.py MOSS telemetry: out=[100, -100] target=[100, -100] bus=10112 mV ticks=[392221, 458942] mode=2
+2:47.990 moss_adapter.py MOSS base: STALL #7: tracks commanded L=+100% R=-100% but the encoders moved L=+992 R=-6 ticks in 1.0 s - stopped (the encoders are the
+2:48.002 moss_adapter.py MOSS base: 'drive' refused not_armed (firmware reason: drive) - re-arming once
+2:48.198 recovering_planner BUMP received
+2:48.199 local_planner.py changed state
+2:48.206 local_planner.py Local planner loop exited due to stop event.
+2:48.211 moss_adapter.py MOSS base: twist vx=+0.000 vy=+0.000(ignored) wz=+0.000 -> tracks L=+0% R=+0% (limit 100%)
+2:48.271 moss_adapter.py MOSS base: twist vx=-0.100 vy=+0.000(ignored) wz=+0.000 -> tracks L=-50% R=-50% (limit 100%)
+2:49.807 moss_adapter.py MOSS telemetry: out=[-50, -50] target=[-50, -50] bus=10744 mV ticks=[391050, 457549] mode=2
+2:50.462 recovering_planner Contact escape
+2:50.462 global_planner.py Cancelling goal.
+2:50.468 moss_adapter.py MOSS base: twist vx=+0.000 vy=+0.000(ignored) wz=+0.000 -> tracks L=+0% R=+0% (limit 100%)
+2:50.471 explorer2.py planner gave up on that goal: excluded until the map or the viewpoint changes
+2:50.617 explorer2.py goal 8: (-1.42, 0.78) 2.2 m away, 5 frontier cells, 20 clusters, chosen in 144 ms
+2:50.620 global_planner.py Got new goal
+2:50.622 global_planner.py Cancelling goal.
+2:50.628 global_planner.py Found safe goal.
+2:50.755 local_planner.py changed state
+2:50.771 moss_adapter.py MOSS base: twist vx=+0.251 vy=+0.000(ignored) wz=+0.273 -> tracks L=+50% R=+64% (limit 100%)
+2:50.862 moss_adapter.py MOSS base: twist vx=+0.256 vy=+0.000(ignored) wz=+0.262 -> tracks L=+50% R=+65% (limit 100%)
+2:51.168 moss_adapter.py MOSS base: twist vx=+0.254 vy=+0.000(ignored) wz=+0.268 -> tracks L=+50% R=+64% (limit 100%)
+2:51.280 moss_adapter.py MOSS base: twist vx=+0.245 vy=+0.000(ignored) wz=+0.286 -> tracks L=+50% R=+63% (limit 100%)
+2:51.381 moss_adapter.py MOSS base: twist vx=+0.216 vy=+0.000(ignored) wz=+0.344 -> tracks L=+50% R=+58% (limit 100%)
+2:51.477 moss_adapter.py MOSS base: twist vx=+0.209 vy=+0.000(ignored) wz=+0.358 -> tracks L=+50% R=+57% (limit 100%)
+2:51.578 moss_adapter.py MOSS base: twist vx=+0.202 vy=+0.000(ignored) wz=+0.373 -> tracks L=+50% R=+56% (limit 100%)
+2:51.826 moss_adapter.py MOSS telemetry: out=[50, 56] target=[50, 56] bus=10728 mV ticks=[391317, 457806] mode=2
+2:53.860 moss_adapter.py MOSS telemetry: out=[50, 56] target=[50, 56] bus=10748 mV ticks=[394974, 461521] mode=2
+2:55.001 moss_adapter.py MOSS base: twist vx=+0.000 vy=+0.000(ignored) wz=+0.385 -> tracks L=-100% R=+100% (limit 100%)
+2:55.696 moss_adapter.py MOSS base: twist vx=+0.200 vy=+0.000(ignored) wz=+0.385 -> tracks L=+50% R=+56% (limit 100%)
+2:55.884 moss_adapter.py MOSS telemetry: out=[-60, 90] target=[50, 56] bus=10524 mV ticks=[397677, 465684] mode=2
+2:57.604 moss_adapter.py MOSS base: tracks frozen at L=+50% R=+56% - pushing at 75 % for 1 s before calling it a contact
+2:57.615 moss_adapter.py MOSS base: twist vx=+0.200 vy=+0.000(ignored) wz=+0.385 -> tracks L=+75% R=+75% (limit 100%)
+2:57.908 moss_adapter.py MOSS telemetry: out=[75, 75] target=[75, 75] bus=10400 mV ticks=[398047, 466494] mode=2
+2:58.626 moss_adapter.py MOSS base: twist vx=+0.200 vy=+0.000(ignored) wz=+0.385 -> tracks L=+50% R=+56% (limit 100%)
+2:59.832 moss_adapter.py MOSS base: tracks frozen at L=+50% R=+56% - pushing at 75 % for 1 s before calling it a contact
+2:59.838 moss_adapter.py MOSS base: twist vx=+0.200 vy=+0.000(ignored) wz=+0.385 -> tracks L=+75% R=+75% (limit 100%)
+2:59.935 moss_adapter.py MOSS telemetry: out=[50, 56] target=[50, 56] bus=10608 mV ticks=[398305, 466920] mode=2
+3:00.857 moss_adapter.py MOSS base: STALL #8: tracks commanded L=+75% R=+75% but the encoders moved L=+18 R=+30 ticks in 1.0 s - stopped (the encoders are the bu
+3:00.863 moss_adapter.py MOSS base: 'drive' refused not_armed (firmware reason: drive) - re-arming once
+3:00.866 moss_adapter.py MOSS base: twist vx=+0.200 vy=+0.000(ignored) wz=+0.385 -> tracks L=+50% R=+56% (limit 100%)
+3:01.283 local_planner.py changed state
+3:01.284 recovering_planner BUMP received
+3:01.284 local_planner.py Local planner loop exited due to stop event.
+3:01.289 moss_adapter.py MOSS base: twist vx=+0.000 vy=+0.000(ignored) wz=+0.000 -> tracks L=+0% R=+0% (limit 100%)
+3:01.349 moss_adapter.py MOSS base: twist vx=-0.100 vy=+0.000(ignored) wz=+0.000 -> tracks L=-50% R=-50% (limit 100%)
+3:01.971 moss_adapter.py MOSS telemetry: out=[-50, -50] target=[-50, -50] bus=10664 mV ticks=[398158, 466800] mode=2
+3:03.769 recovering_planner Contact escape
+3:03.770 global_planner.py Cancelling goal.
+3:03.774 explorer2.py planner gave up on that goal: excluded until the map or the viewpoint changes
+3:03.775 moss_adapter.py MOSS base: twist vx=+0.000 vy=+0.000(ignored) wz=+0.000 -> tracks L=+0% R=+0% (limit 100%)
+3:03.982 explorer2.py goal 9: (0.18, -1.47) 2.0 m away, 20 frontier cells, 19 clusters, chosen in 207 ms
+3:03.982 global_planner.py Got new goal
+3:03.983 global_planner.py Cancelling goal.
+3:03.984 global_planner.py Found safe goal.
+3:04.072 global_planner.py Robot is stuck. Replanning.
+3:04.159 local_planner.py changed state
+3:04.175 moss_adapter.py MOSS base: twist vx=+0.000 vy=+0.000(ignored) wz=-0.385 -> tracks L=+100% R=-100% (limit 100%)
+3:04.191 moss_adapter.py MOSS telemetry: out=[0, 0] target=[0, 0] bus=10828 mV ticks=[395601, 464221] mode=2
+3:05.202 moss_adapter.py MOSS base: STALL #9: tracks commanded L=+100% R=-100% but the encoders moved L=+838 R=-7 ticks in 1.0 s - stopped (the encoders are the
+3:05.211 moss_adapter.py MOSS base: 'drive' refused not_armed (firmware reason: drive) - re-arming once
+3:05.276 recovering_planner BUMP received
+3:05.277 local_planner.py changed state
+3:05.277 local_planner.py Local planner loop exited due to stop event.
+3:05.281 moss_adapter.py MOSS base: twist vx=+0.000 vy=+0.000(ignored) wz=+0.000 -> tracks L=+0% R=+0% (limit 100%)
+3:05.333 moss_adapter.py MOSS base: twist vx=-0.100 vy=+0.000(ignored) wz=+0.000 -> tracks L=-50% R=-50% (limit 100%)
+3:06.211 moss_adapter.py MOSS telemetry: out=[-50, -50] target=[-50, -50] bus=10704 mV ticks=[395598, 463303] mode=2
+3:07.039 recovering_planner Contact escape
+3:07.039 global_planner.py Cancelling goal.
+3:07.045 explorer2.py planner gave up on that goal: excluded until the map or the viewpoint changes
+3:07.052 moss_adapter.py MOSS base: twist vx=+0.000 vy=+0.000(ignored) wz=+0.000 -> tracks L=+0% R=+0% (limit 100%)
+3:07.169 explorer2.py 7 of 19 clusters are on a recently failed goal: waiting for the map or the viewpoint to change, not stopping
+3:08.263 explorer2.py 7 of 19 clusters are on a recently failed goal: waiting for the map or the viewpoint to change, not stopping
+3:09.336 explorer2.py 7 of 19 clusters are on a recently failed goal: waiting for the map or the viewpoint to change, not stopping
+3:10.463 explorer2.py 7 of 19 clusters are on a recently failed goal: waiting for the map or the viewpoint to change, not stopping
+3:11.545 explorer2.py 7 of 19 clusters are on a recently failed goal: waiting for the map or the viewpoint to change, not stopping
+3:12.621 explorer2.py 7 of 19 clusters are on a recently failed goal: waiting for the map or the viewpoint to change, not stopping
+3:13.733 explorer2.py 7 of 19 clusters are on a recently failed goal: waiting for the map or the viewpoint to change, not stopping
+3:14.830 explorer2.py 7 of 19 clusters are on a recently failed goal: waiting for the map or the viewpoint to change, not stopping
+3:15.969 explorer2.py 7 of 19 clusters are on a recently failed goal: waiting for the map or the viewpoint to change, not stopping
+3:17.081 explorer2.py 7 of 19 clusters are on a recently failed goal: waiting for the map or the viewpoint to change, not stopping
+3:18.196 explorer2.py 7 of 19 clusters are on a recently failed goal: waiting for the map or the viewpoint to change, not stopping
+3:19.295 explorer2.py starvation escape: the world stayed silent for 12 asks - reopening the oldest failed goal at (-1.67, -0.12)
+3:20.493 explorer2.py goal 10: (-1.27, -0.07) 11.0 m away (beyond the cap: nothing safe within it), 5 frontier cells, 19 clusters, chosen in 197 ms
+3:20.497 global_planner.py Got new goal
+3:20.498 global_planner.py Cancelling goal.
+3:20.501 global_planner.py Found safe goal.
+3:20.505 global_planner.py Robot is stuck. Replanning.
+3:20.559 local_planner.py changed state
+3:20.568 moss_adapter.py MOSS base: twist vx=+0.000 vy=+0.000(ignored) wz=-0.385 -> tracks L=+100% R=-100% (limit 100%)
+3:20.647 moss_adapter.py MOSS telemetry: out=[0, 0] target=[0, 0] bus=10820 mV ticks=[394095, 461793] mode=2
+3:21.666 moss_adapter.py MOSS base: STALL #10: tracks commanded L=+100% R=-100% but the encoders moved L=+968 R=-6 ticks in 1.0 s - stopped (the encoders are the
+3:21.667 moss_adapter.py MOSS base: 'drive' refused not_armed (firmware reason: drive) - re-arming once
+3:21.867 local_planner.py changed state
+3:21.868 recovering_planner BUMP received
+3:21.869 local_planner.py Local planner loop exited due to stop event.
+3:21.873 moss_adapter.py MOSS base: twist vx=-0.100 vy=+0.000(ignored) wz=+0.000 -> tracks L=-50% R=-50% (limit 100%)
+3:21.885 moss_adapter.py MOSS base: twist vx=+0.000 vy=+0.000(ignored) wz=+0.000 -> tracks L=+0% R=+0% (limit 100%)
+3:21.934 moss_adapter.py MOSS base: twist vx=-0.100 vy=+0.000(ignored) wz=+0.000 -> tracks L=-50% R=-50% (limit 100%)
+3:22.676 moss_adapter.py MOSS telemetry: out=[-50, -50] target=[-50, -50] bus=10664 mV ticks=[394769, 461400] mode=2
+3:24.016 recovering_planner Contact escape
+3:24.017 global_planner.py Cancelling goal.
+3:24.021 explorer2.py planner gave up on that goal: excluded until the map or the viewpoint changes
+3:24.026 moss_adapter.py MOSS base: twist vx=+0.000 vy=+0.000(ignored) wz=+0.000 -> tracks L=+0% R=+0% (limit 100%)
+3:24.142 explorer2.py 7 of 20 clusters are on a recently failed goal: waiting for the map or the viewpoint to change, not stopping
+3:25.225 explorer2.py 7 of 19 clusters are on a recently failed goal: waiting for the map or the viewpoint to change, not stopping
+3:26.318 explorer2.py 7 of 19 clusters are on a recently failed goal: waiting for the map or the viewpoint to change, not stopping
+3:27.430 explorer2.py 7 of 19 clusters are on a recently failed goal: waiting for the map or the viewpoint to change, not stopping
+3:28.555 explorer2.py 7 of 19 clusters are on a recently failed goal: waiting for the map or the viewpoint to change, not stopping
+3:29.638 explorer2.py 7 of 19 clusters are on a recently failed goal: waiting for the map or the viewpoint to change, not stopping
+3:30.774 explorer2.py 7 of 19 clusters are on a recently failed goal: waiting for the map or the viewpoint to change, not stopping
+3:31.892 explorer2.py 7 of 19 clusters are on a recently failed goal: waiting for the map or the viewpoint to change, not stopping
+3:32.976 explorer2.py 7 of 19 clusters are on a recently failed goal: waiting for the map or the viewpoint to change, not stopping
+3:34.037 explorer2.py 7 of 19 clusters are on a recently failed goal: waiting for the map or the viewpoint to change, not stopping
+3:35.126 explorer2.py 7 of 19 clusters are on a recently failed goal: waiting for the map or the viewpoint to change, not stopping
+3:35.151 wavefront_frontier WavefrontFrontierExplorer: stop_movement received, stopping exploration
+3:35.152 wavefront_frontier Received exploration stop command via LCM
+3:35.156 wavefront_frontier Stopped autonomous frontier exploration
+3:35.156 global_planner.py Got new goal
+3:35.156 global_planner.py Cancelling goal.
+3:35.158 global_planner.py Found safe goal.
+3:35.206 global_planner.py Cancelling goal.
+3:35.208 local_planner.py changed state
+3:35.210 local_planner.py Reached goal position, starting final rotation
+3:35.210 local_planner.py changed state
+3:35.211 local_planner.py changed state
+3:35.215 local_planner.py Error in local planning
+3:35.217 local_planner.py changed state
+3:35.218 global_planner.py Failure in navigation.
+3:35.219 recovering_planner Replan requested without a goal; ignoring.
+"""
+
+# The same run, three seconds around its first contact, with every module's lines (costmap camera counts,
+# sounds, the coordinator's bump relay).
+RUN_CONTACT = r"""
+1:56.934 moss_adapter.py MOSS base: twist vx=+0.313 vy=+0.000(ignored) wz=-0.200 -> tracks L=+75% R=+64% (limit 100%)
+1:56.998 costmap2d.py costmap: camera obstacles: 1544 points, 9 cell hits over the last 2 revolutions
+1:57.036 moss_adapter.py MOSS base: twist vx=+0.315 vy=+0.000(ignored) wz=-0.200 -> tracks L=+76% R=+64% (limit 100%)
+1:57.183 costmap2d.py costmap: camera obstacles: 1570 points, 8 cell hits over the last 2 revolutions
+1:57.220 moss_adapter.py MOSS base: STALL #1: tracks commanded L=+76% R=+64% but the encoders moved L=+1 R=+0 ticks in 1.0 s - stopped (the encoders are the bumper)
+1:57.225 moss_adapter.py MOSS base: 'drive' refused not_armed (firmware reason: drive) - re-arming once
+1:57.403 costmap2d.py costmap: camera obstacles: 1574 points, 4 cell hits over the last 2 revolutions
+1:57.445 moss_adapter.py MOSS base: twist vx=+0.313 vy=+0.000(ignored) wz=-0.200 -> tracks L=+75% R=+64% (limit 100%)
+1:57.447 blueprints.py base stall #1: publishing bump (contact) for the planner's escape
+1:57.503 recovering_planner BUMP received
+1:57.503 costmap2d.py costmap: contact at (-0.88, +0.73) heading +147 deg: 32 cells ahead marked as a wall
+1:57.504 sounds.py MOSS sounds: bump_surprise queued
+1:57.505 local_planner.py changed state
+1:57.505 local_planner.py Local planner loop exited due to stop event.
+1:57.509 moss_adapter.py MOSS base: twist vx=+0.000 vy=+0.000(ignored) wz=+0.000 -> tracks L=+0% R=+0% (limit 100%)
+1:57.570 moss_adapter.py MOSS base: twist vx=-0.100 vy=+0.000(ignored) wz=+0.000 -> tracks L=-50% R=-50% (limit 100%)
+1:57.581 costmap2d.py costmap: camera obstacles: 1594 points, 149 cell hits over the last 2 revolutions
+1:57.786 costmap2d.py costmap: camera obstacles: 1540 points, 35 cell hits over the last 2 revolutions
+1:57.985 costmap2d.py costmap: camera obstacles: 1596 points, 19 cell hits over the last 2 revolutions
+1:58.184 costmap2d.py costmap: camera obstacles: 1596 points, 5 cell hits over the last 2 revolutions
+1:58.375 costmap2d.py costmap: camera obstacles: 1594 points, 28 cell hits over the last 2 revolutions
+1:58.506 sounds.py MOSS sounds: recovery_success queued
+1:58.577 costmap2d.py costmap: camera obstacles: 1626 points, 52 cell hits over the last 2 revolutions
+1:58.752 moss_adapter.py MOSS telemetry: out=[-50, -50] target=[-50, -50] bus=10764 mV ticks=[399100, 474264] mode=2
+1:58.794 costmap2d.py costmap: camera obstacles: 1678 points, 189 cell hits over the last 2 revolutions
+1:58.975 costmap2d.py costmap: camera obstacles: 1642 points, 66 cell hits over the last 2 revolutions
+1:59.201 costmap2d.py costmap: camera obstacles: 1638 points, 191 cell hits over the last 2 revolutions
+1:59.376 costmap2d.py costmap: camera obstacles: 1648 points, 238 cell hits over the last 2 revolutions
+1:59.579 costmap2d.py costmap: camera obstacles: 1624 points, 131 cell hits over the last 2 revolutions
+1:59.605 recovering_planner Contact escape
+1:59.606 global_planner.py Cancelling goal.
+1:59.615 moss_adapter.py MOSS base: twist vx=+0.000 vy=+0.000(ignored) wz=+0.000 -> tracks L=+0% R=+0% (limit 100%)
+1:59.663 explorer2.py planner gave up on that goal: excluded until the map or the viewpoint changes
+1:59.777 costmap2d.py costmap: camera obstacles: 1594 points, 198 cell hits over the last 2 revolutions
+1:59.818 velocity_task.py JointVelocityTask vel_base timed out (no update for 0.209s)
+1:59.835 explorer2.py goal 2: (0.18, -1.47) 2.6 m away, 20 frontier cells, 19 clusters, chosen in 170 ms
+1:59.836 global_planner.py Got new goal
+1:59.837 global_planner.py Cancelling goal.
+1:59.840 global_planner.py Found safe goal.
+1:59.934 local_planner.py changed state
+1:59.949 moss_adapter.py MOSS base: twist vx=+0.000 vy=+0.000(ignored) wz=-0.385 -> tracks L=+100% R=-100% (limit 100%)
+1:59.993 costmap2d.py costmap: camera obstacles: 1650 points, 118 cell hits over the last 2 revolutions
+"""
+
+print("I. what it decides: log lines -> decision events -> the state word, /decisions.json")
+WORDS = {"IDLE", "READY", "HEADING", "OBSTACLE", "BACKING OFF", "STUCK", "WAITING", "DONE", "STOPPED"}
+EVENT_KEYS = {"ts", "kind", "text", "src", "seq"}
+
+
+def recs_of(text):
+ """'M:SS.mmm module event' lines (the run's lines as RUN_NAV prints them) -> log records, ts in s."""
+ out = []
+ for line in text.splitlines():
+ m = re.match(r"^(\d+):(\d+(?:\.\d+)?) (\S+) (.*)$", line)
+ if m:
+ out.append({"ts": int(m[1]) * 60 + float(m[2]), "level": "info", "event": m[4], "logger": m[3]})
+ return out
+
+
+def one(event, logger="x.py"):
+ return mc.decisions_from_records([{"ts": 1.0, "level": "info", "event": event, "logger": logger}])
+
+
+# every catalogue line, as the modules print it (from the run where the run has it), -> kind, src and
+# a text that keeps the line's numbers
+CATALOGUE = [
+ ("explorer2.py", "goal 1: (-1.27, 1.28) 2.0 m away, 35 frontier cells, 12 clusters, chosen in 114 ms",
+ "goal", "explorer", ["Goal 1: 2.0 m away (35 cells)"]),
+ ("explorer2.py", "goal 3: (-1.67, -0.12) 3.9 m away (beyond the cap: nothing safe within it), 110 frontier cells, "
+ "19 clusters, chosen in 156 ms", "goal", "explorer", ["Goal 3: 3.9 m away (110 cells, beyond the cap)"]),
+ ("explorer2.py", "planner gave up on that goal: excluded until the map or the viewpoint changes",
+ "goal_dropped", "explorer", ["Goal dropped: the planner gave up"]),
+ ("explorer2.py", "goal timeout after 30 s having closed 0.05 m of 2.40 m: excluded until the map or the "
+ "viewpoint changes", "goal_dropped", "explorer", ["Goal dropped", "0.05 of 2.40 m", "30 s"]),
+ ("explorer2.py", "10 of 20 clusters are on a recently failed goal: waiting for the map or the viewpoint to change, "
+ "not stopping", "waiting", "explorer", ["Waiting: 10 of 20 frontiers are failed goals"]),
+ ("explorer2.py", "starvation escape: the world stayed silent for 12 asks - reopening the oldest failed goal at "
+ "(-1.27, 1.28)", "retry", "explorer", ["Retrying an old goal at (-1.27, 1.28)"]),
+ ("local_planner.py", "Obstacle detected ahead, stopping local planner.", "obstacle", "planner",
+ ["Obstacle ahead: stop"]),
+ ("global_planner.py", "Replanning.", "replan", "planner", ["Replanning"]),
+ ("explorer2.py", "born cornered: 0.40 m2 of reachable floor and no frontier - one 0.20 m back-off via the bump "
+ "reflex", "obstacle", "explorer", ["Cornered (0.40 m2 of floor, no frontier): backing off 0.20 m"]),
+ ("global_planner.py", "No path found to the goal.", "goal_dropped", "planner", ["Goal dropped: no path"]),
+ ("global_planner.py", "No safe goal found near requested target.", "goal_dropped", "planner",
+ ["Goal dropped: no safe spot near it"]),
+ ("global_planner.py", "No safe goal found.", "goal_dropped", "planner", ["Goal dropped: no safe spot near it"]),
+ ("recovering_planner.py", "Goal abandoned, no recovery motion", "goal_dropped", "recovery",
+ ["Stuck: goal abandoned"]),
+ ("recovering_planner.py", "Contact during an escape: stopping dead", "contact", "recovery",
+ ["Contact during an escape: stop dead"]),
+ ("global_planner.py", "Got new goal", "new_goal", "planner", ["Planner: new goal"]),
+ ("global_planner.py", "Found safe goal.", "new_goal", "planner", ["Planner: safe goal found"]),
+ ("global_planner.py", "Travelling to goal 1.2850097467377768m away from requested goal.", "new_goal", "planner",
+ ["Planner: safe goal 1.29 m from the requested one"]),
+ ("local_planner.py", "Reached goal position, starting final rotation", "reached", "planner", ["Goal reached"]),
+ ("global_planner.py", "Close enough to goal. Accepting as arrived.", "reached", "planner",
+ ["Goal reached (close enough)"]),
+ ("moss_adapter.py", "MOSS base: STALL #1: tracks commanded L=+76% R=+64% but the encoders moved L=+1 R=+0 ticks "
+ "in 1.0 s - stopped (the encoders are the bumper)", "contact", "base",
+ ["Contact #1: both tracks blocked", "L=+76% R=+64%", "L=+1 R=+0", "1.0 s"]),
+ ("moss_adapter.py", "MOSS base: STALL #2: tracks commanded L=+100% R=-100% but the encoders moved L=+1259 R=+6 "
+ "ticks in 1.0 s - stopped (the encoders are the", "contact", "base",
+ ["Contact #2: right track blocked", "L=+100% R=-100%", "L=+1259 R=+6", "1.0 s"]),
+ ("moss_adapter.py", "MOSS base: STALL #8: tracks commanded L=+75% R=+75% but the encoders moved L=+18 R=+30 ticks "
+ "in 1.0 s - stopped (the encoders are the bumper)", "contact", "base",
+ ["Contact #8: left track blocked", "L=+18 R=+30"]),
+ ("recovering_planner.py", "BUMP received", "contact", "recovery", ["Bump: backing off"]),
+ ("recovering_planner.py", "Contact escape", "contact", "recovery", ["Escape done"]),
+ ("costmap2d.py", "costmap: contact at (-0.88, +0.73) heading +147 deg: 32 cells ahead marked as a wall",
+ "contact", "costmap", ["Wall remembered ahead (32 cells)"]),
+ ("moss_adapter.py", "MOSS base: tracks frozen at L=+56% R=+50% - pushing at 75 % for 1 s before calling it a "
+ "contact", "push", "base", ["Tracks frozen at L=+56% R=+50%: pushing at 75 %"]),
+ ("moss_adapter.py", "MOSS base: pivot L=+100% R=-100%: the right track did not turn (encoders L=+1259 R=+6 ticks "
+ "in 1.0 s) - not a contact: from now on this pivot turns on the left track only, the right track braked",
+ "pivot_fallback", "base",
+ ["Right track did not turn in reverse", "L=+100% R=-100%", "pivoting on the left track from now on"]),
+ ("moss_adapter.py", "MOSS base: pivot L=-100% R=+100%: the left track did not turn (encoders L=+3 R=+1180 ticks "
+ "in 1.0 s) - not a contact: from now on this pivot turns on the right track only, the left track braked",
+ "pivot_fallback", "base", ["Left track did not turn in reverse", "pivoting on the right track"]),
+ ("bump_imu.py", "IMU contact: forward -4.8 m/s^2 (threshold 4.0) while commanded L=+70% R=+60%", "imu", "imu",
+ ["IMU contact", "-4.8 m/s^2", "4.0", "L=+70% R=+60%"]),
+ ("explorer2.py", "exploration start: distance exp 2, cap 3 m, radius 4 m (0 = no limit; the radius is centred on "
+ "the first pose)", "mission", "mission", ["Exploration started (cap 3 m, radius 4 m)"]),
+ ("wavefront_frontier_goal_selector.py", "WavefrontFrontierExplorer: stop_movement received, stopping exploration",
+ "mission", "mission", ["Exploration stopped"]),
+ ("wavefront_frontier_goal_selector.py", "Received exploration stop command via LCM", "mission", "mission",
+ ["Exploration stopped"]),
+ ("wavefront_frontier_goal_selector.py", "Stopped autonomous frontier exploration", "mission", "mission",
+ ["Exploration stopped"]),
+ ("explorer2.py", "exploration complete: no reachable frontier left within 4 m of the start (12 targets, 150 ms)",
+ "mission", "mission", ["Exploration complete", "12 goals"]),
+ ("wavefront_frontier_goal_selector.py", "Exploration complete after 7 goals and 3 consecutive failures finding "
+ "new frontiers", "mission", "mission", ["Exploration complete (7 goals)"]),
+]
+for logger, line, kind, src, want in CATALOGUE:
+ ev = one(line, logger)
+ e = ev[0] if len(ev) == 1 else {}
+ check(f"{kind}: '{line[:58]}' -> '{e.get('text')}'",
+ e.get("kind") == kind and e.get("src") == src and all(w in e.get("text", "") for w in want)
+ and set(e) == EVENT_KEYS and e.get("seq") == 1 and e.get("ts") == 1.0 and len(e.get("text", "")) <= 120,
+ json.dumps(ev)[:200])
+
+IGNORED = [
+ "MOSS base: twist vx=+0.000 vy=+0.000(ignored) wz=+0.000 -> tracks L=+0% R=+0% (limit 100%)",
+ "MOSS telemetry: out=[0, 0] target=[0, 0] bus=10972 mV ticks=[387605, 451441] mode=0",
+ "costmap: camera obstacles: 1544 points, 9 cell hits over the last 2 revolutions",
+ "costmap: explore_cmd received - map writing OPEN for the mission",
+ "costmap: exploration complete - map writing continues",
+ "costmap: exploration stopped - map writing continues",
+ "MOSS sounds: bump_surprise queued",
+ "base stall #1: publishing bump (contact) for the planner's escape",
+ "MOSS base: 'drive' refused not_armed (firmware reason: drive) - re-arming once",
+ "twist_command: consumer behind, executing the newest command only (1 stale dropped so far)",
+ "Received exploration start command via LCM", "Started autonomous frontier exploration",
+ "Cancelling goal.", "changed state", "Local planner loop exited due to stop event.",
+ "Replan requested without a goal; ignoring.", "Failure in navigation.", "Error in local planning",
+ "goal timeout after 30 s, 0.80 m closer, re-deciding",
+ "JointVelocityTask vel_base timed out (no update for 0.209s)",
+]
+made = [line for line in IGNORED if one(line)]
+check(f"{len(IGNORED)} other lines of the run (twist, telemetry, camera counts, sounds, the bump relay, the "
+ "explore_cmd the costmap repeats...) -> no event", not made, json.dumps(made)[:200])
+
+
+def recs(*items):
+ """(ts, event[, fields]) -> log records."""
+ return [dict({"ts": it[0], "level": "info", "event": it[1], "logger": "x.py"}, **(it[2] if len(it) > 2 else {}))
+ for it in items]
+
+
+def kt(events):
+ return [(e["kind"], e["src"], e["text"]) for e in events]
+
+
+# the planner's replan reasons: the line before _replan_path is not the decision, what _replan_path does is
+REASONS = ["Robot is stuck. Replanning.", "Robot veered off track. Replanning.",
+ "Replanning path due to obstacle found.", "Failure in navigation."]
+check("a replan reason alone (the planner did nothing with it) -> no event",
+ not [line for line in REASONS if one(line, "global_planner.py")])
+check("each reason + dimOS's 'Replanning.' 2 ms later -> one replan event at the reason's time: Stuck: replanning, "
+ "Off track: replanning, Replanning, Navigation failed: replanning",
+ [kt(mc.decisions_from_records(recs((5.0, line), (5.002, "Replanning.")))) for line in REASONS]
+ == [[("replan", "planner", t)] for t in ("Stuck: replanning", "Off track: replanning", "Replanning",
+ "Navigation failed: replanning")]
+ and mc.decisions_from_records(recs((5.0, REASONS[0]), (5.002, "Replanning.")))[0]["ts"] == 5.0)
+ab = mc.decisions_from_records(recs((0.0, "goal 2: (1.00, 2.00) 2.0 m away, 30 frontier cells, 10 clusters, chosen "
+ "in 90 ms"),
+ (8.0, "Robot is stuck. Replanning."),
+ (8.001, "Goal abandoned, no recovery motion", {"trigger": "stuck_detector"}),
+ (8.002, "Cancelling goal."),
+ (8.008, "planner gave up on that goal: excluded until the map or the viewpoint "
+ "changes")))
+check("stuck -> MOSS's planner gives the goal up: 'Stuck: goal abandoned' (the explorer's give-up 7 ms later is "
+ "its echo); STUCK at 9.0, WAITING at 11.1 (the explorer runs)",
+ kt(ab)[1:] == [("goal_dropped", "recovery", "Stuck: goal abandoned")]
+ and [mc.now_state(ab, True, None, t)["word"] for t in (9.0, 11.1)] == ["STUCK", "WAITING"],
+ json.dumps(kt(ab)))
+check("the run's 'Failure in navigation.' + 'Replan requested without a goal; ignoring.' -> nothing",
+ mc.decisions_from_records(recs((215.218, "Failure in navigation."),
+ (215.219, "Replan requested without a goal; ignoring."))) == [])
+late = mc.decisions_from_records(recs((5.0, "Robot is stuck. Replanning."),
+ (5.6, "MOSS base: twist vx=+0.000 vy=+0.000(ignored) wz=-0.385 -> tracks "
+ "L=+100% R=-100% (limit 100%)")))
+check("a stuck verdict followed only by the pivot's twist 0.6 s later -> nothing (MOSS's planner ignores 'stuck' "
+ "while the follower turns in place)", late == [])
+carry = {}
+part1 = mc.decisions_from_records(recs((5.0, "Robot is stuck. Replanning.")), 3, None, carry)
+part2 = mc.decisions_from_records(recs((5.002, "Replanning.")), 3, None, carry)
+check("the reason at the end of one read, 'Replanning.' at the start of the next -> one event in the second read "
+ "(the reason waits in `carry`), seq 4",
+ part1 == [] and kt(part2) == [("replan", "planner", "Stuck: replanning")] and part2[0]["seq"] == 4
+ and carry.get("pending") is None, json.dumps([part1, part2, carry], default=str)[:300])
+
+RUN = recs_of(RUN_NAV)
+run_ev = mc.decisions_from_records(RUN)
+counts = {}
+for e in run_ev:
+ counts[e["kind"]] = counts.get(e["kind"], 0) + 1
+print(f" the whole run replayed: {len(RUN)} records -> {len(run_ev)} events: "
+ + ", ".join(f"{k} {counts[k]}" for k in sorted(counts)))
+check("whole run -> goal >= 10, contact >= 10, waiting >= 20",
+ counts.get("goal", 0) >= 10 and counts.get("contact", 0) >= 10 and counts.get("waiting", 0) >= 20,
+ json.dumps(counts))
+check("whole run, exact: 10 goals, 10 dropped, 44 waits, 3 retries, 30 contacts (10 stalls + 10 bumps + 10 "
+ "escapes), 6 pushes, 1 obstacle, 1 replan (the obstacle's: the 3 stuck verdicts were ignored by the planner, "
+ "the last 'Failure in navigation.' had no goal), 1 shifted safe goal, 2 mission lines (the start, the STOP), "
+ "no 'Goal reached' (the explorer's stay-here goal right after the STOP is an echo)",
+ counts == {"goal": 10, "goal_dropped": 10, "waiting": 44, "retry": 3, "contact": 30, "push": 6,
+ "obstacle": 1, "replan": 1, "new_goal": 1, "mission": 2}, json.dumps(counts))
+check("the run's 3 stuck verdicts (2:11.637, 3:04.072, 3:20.505), each 4-90 ms after 'Found safe goal.' and "
+ "followed by the in-place pivot, no 'Replanning.' -> no event; the word 0.3 s and 1.0 s after each = HEADING "
+ "toward that goal",
+ not any(e["text"].startswith("Stuck") for e in run_ev)
+ and all(mc.now_state([e for e in run_ev if e["ts"] <= t + d], True, None, t + d)["word"] == "HEADING"
+ for t in (131.637, 184.072, 200.505) for d in (0.3, 1.0)))
+chatty = [r for r in RUN if r["event"].startswith(("MOSS base: twist", "MOSS telemetry"))]
+check(f"no twist / telemetry line of the run makes an event ({len(chatty)} lines)",
+ len(chatty) == 181 and mc.decisions_from_records(chatty) == []
+ and not any("twist" in e["text"] or "telemetry" in e["text"] for e in run_ev))
+check("order kept: seq 1..N, times never go back",
+ [e["seq"] for e in run_ev] == list(range(1, len(run_ev) + 1))
+ and all(a["ts"] <= b["ts"] for a, b in zip(run_ev, run_ev[1:])))
+check("the run ends on the STOP: STOPPED, 'Exploration stopped (stop button or pad)', since 3:35.151",
+ mc.now_state(run_ev, True, None, 216.0) == {"word": "STOPPED", "detail": "Exploration stopped (stop button or "
+ "pad)", "since": 215.151},
+ json.dumps(mc.now_state(run_ev, True, None, 216.0)))
+run_log = mc.DecisionLog()
+run_log.add(RUN)
+rows = list(run_log.events)
+tail30 = rows[-30:]
+waits = [e["text"] for e in rows if e["kind"] == "waiting"]
+check(f"the run through the feed's store: {len(run_ev)} events -> {len(rows)} rows (identical events one after "
+ "the other fold into one, ' (xN)', last ts and seq); the 11 waits from 3:24.142 to 3:35.126 -> 2 rows "
+ "(7 of 20, then 7 of 19 x10); the page's last 30 rows keep 12 contacts, not 3",
+ len(rows) == 66 and run_log.seq == len(run_ev) and rows[-1]["seq"] == len(run_ev)
+ and [e["text"] for e in rows[-3:-1]] == ["Waiting: 7 of 20 frontiers are failed goals",
+ "Waiting: 7 of 19 frontiers are failed goals (x10)"]
+ and rows[-2]["ts"] == 215.126 and sum(e["kind"] == "contact" for e in tail30) == 12
+ and sum(e["kind"] == "waiting" for e in tail30) == 5
+ and sum(int(re.search(r"\(x(\d+)\)$", t)[1]) if re.search(r"\(x(\d+)\)$", t) else 1 for t in waits) == 44
+ and mc.now_state(rows, True, None, 216.0)["word"] == "STOPPED",
+ json.dumps([(e["seq"], e["text"]) for e in rows[-4:]]))
+
+# three seconds around the first contact, every module's line (the timeline of the same run)
+EX = recs_of(RUN_CONTACT)
+ex_ev = mc.decisions_from_records(EX)
+check(f"first contact, {len(EX)} lines of every module -> stall, bump, wall, escape, goal dropped, goal 2 "
+ "(the planner taking goal 2 is an echo)",
+ [(e["kind"], e["src"], e["text"]) for e in ex_ev]
+ == [("contact", "base", "Contact #1: both tracks blocked (commanded L=+76% R=+64%, encoders moved L=+1 R=+0 "
+ "ticks in 1.0 s)"),
+ ("contact", "recovery", "Bump: backing off"), ("contact", "costmap", "Wall remembered ahead (32 cells)"),
+ ("contact", "recovery", "Escape done"), ("goal_dropped", "explorer", "Goal dropped: the planner gave up"),
+ ("goal", "explorer", "Goal 2: 2.6 m away (20 cells)")],
+ json.dumps([(e["kind"], e["text"]) for e in ex_ev])[:300])
+
+
+def word_at(events, t, exploring=None):
+ return mc.now_state([e for e in events if e["ts"] <= t], True, exploring, t)
+
+
+check("first contact, the word at 1:57.3 / 1:59.0 / 1:59.7 / 1:59.9 -> BACKING OFF, BACKING OFF, "
+ "WAITING (goal dropped), HEADING (goal 2)",
+ [word_at(ex_ev, t)["word"] for t in (117.3, 119.0, 119.7, 119.9)]
+ == ["BACKING OFF", "BACKING OFF", "WAITING", "HEADING"]
+ and word_at(ex_ev, 119.9)["detail"] == "Goal 2: 2.6 m away (20 cells)",
+ json.dumps([word_at(ex_ev, t) for t in (117.3, 119.0, 119.7, 119.9)])[:300])
+
+# now_state: known sequences -> known words, holds to the millisecond
+G = "goal 4: (1.00, 2.00) 2.5 m away, 30 frontier cells, 9 clusters, chosen in 100 ms"
+GT = "Goal 4: 2.5 m away (30 cells)"
+OBST = "Obstacle detected ahead, stopping local planner."
+STALL = ("MOSS base: STALL #3: tracks commanded L=+60% R=+60% but the encoders moved L=+2 R=+1 ticks in 1.0 s - "
+ "stopped (the encoders are the bumper)")
+
+
+def evs(*pairs):
+ return mc.decisions_from_records([{"ts": t, "event": e} for t, e in pairs])
+
+
+def ns(events, now, exploring=None, running=True):
+ """The word at `now` from the events logged by then (a number ts after `now` is not written yet)."""
+ seen = [e for e in events if not isinstance(e["ts"], (int, float)) or e["ts"] <= now]
+ return mc.now_state(seen, running, exploring, now)
+
+
+check("the words are the nine of the page: IDLE READY HEADING OBSTACLE BACKING OFF STUCK WAITING DONE STOPPED",
+ set(mc.DECISION_WORDS) == WORDS and len(mc.DECISION_WORDS) == 9)
+check("no stack -> IDLE, whatever the events", ns(evs((100.0, G)), 101.0, running=False)["word"] == "IDLE")
+check("a stack and no decision -> READY, since null", ns([], 5.0) == {"word": "READY", "detail":
+ "Stack running, no decision yet", "since": None})
+check("goal at 100 s -> HEADING at 101 s, detail the goal line, since 100",
+ ns(evs((100.0, G)), 101.0) == {"word": "HEADING", "detail": GT, "since": 100.0})
+seq_o = evs((100.0, G), (105.0, OBST), (105.01, "Replanning path due to obstacle found."), (105.02, "Replanning."),
+ (105.05, "Found safe goal."))
+check("goal 100, obstacle 105 (+ replan, its echo, the safe goal) -> 2 + 1 events; OBSTACLE at 105.5 and 107.99 "
+ "(since the replan 105.01), HEADING the goal at 108.01 (3 s hold)",
+ [e["kind"] for e in seq_o] == ["goal", "obstacle", "replan"]
+ and [ns(seq_o, t)["word"] for t in (105.5, 107.99, 108.01)] == ["OBSTACLE", "OBSTACLE", "HEADING"]
+ and ns(seq_o, 105.5)["since"] == 105.01 and ns(seq_o, 108.01)["detail"] == GT,
+ json.dumps([ns(seq_o, t) for t in (105.5, 107.99, 108.01)])[:300])
+seq_c = evs((100.0, G), (110.0, STALL))
+check("goal 100, stall 110 and its escape's end line lost -> BACKING OFF at 110.1 and 115.99, HEADING at 116.0 "
+ "(the escape's 5 s timeout + 1 s)",
+ [ns(seq_c, t)["word"] for t in (110.1, 115.99, 116.0)] == ["BACKING OFF", "BACKING OFF", "HEADING"]
+ and ns(seq_c, 110.1)["detail"].startswith("Contact #3: both tracks blocked")
+ and mc.ESCAPE_HOLD_S == 6.0)
+slow = mc.decisions_from_records(recs((0.0, G), (10.0, STALL), (10.3, "BUMP received", {"acted": True}),
+ (10.3, "costmap: contact at (+0.10, +0.20) heading +10 deg: 30 cells ahead "
+ "marked as a wall"),
+ (15.3, "Contact escape", {"trigger": "bump", "travelled_m": 0.05,
+ "seconds": 5.0, "aborted": False})))
+check("a slow escape (a track slipping in reverse: 0.05 m in 5 s, its end at 15.3) -> BACKING OFF at 12, 14, 14.5, "
+ "15, 15.29; then WAITING 'Escape timed out: only 0.05 m in 5.0 s'",
+ [ns(slow, t)["word"] for t in (12.0, 14.0, 14.5, 15.0, 15.29)] == ["BACKING OFF"] * 5
+ and ns(slow, 15.4) == {"word": "WAITING", "detail": "Escape timed out: only 0.05 m in 5.0 s", "since": 15.3},
+ json.dumps([ns(slow, t) for t in (14.5, 15.4)]))
+seq_co = evs((100.0, G), (110.0, "MOSS base: tracks frozen at L=+56% R=+50% - pushing at 75 % for 1 s before calling "
+ "it a contact"), (111.5, OBST))
+check("frozen tracks at 110 (STUCK 4 s) outrank an obstacle at 111.5 (OBSTACLE 3 s): STUCK to 114, then OBSTACLE "
+ "to 114.5, then HEADING",
+ [ns(seq_co, t)["word"] for t in (112.0, 113.99, 114.2, 114.5)]
+ == ["STUCK", "STUCK", "OBSTACLE", "HEADING"])
+seq_oc = evs((100.0, G), (110.0, OBST), (111.0, STALL))
+check("an obstacle at 110 then a contact at 111 -> BACKING OFF at 111.5 (to 117 at most), HEADING at 117",
+ [ns(seq_oc, t)["word"] for t in (110.5, 111.5, 116.99, 117.0)] == ["OBSTACLE", "BACKING OFF", "BACKING OFF",
+ "HEADING"])
+seq_p = evs((100.0, G), (120.0, "MOSS base: tracks frozen at L=+56% R=+50% - pushing at 75 % for 1 s before "
+ "calling it a contact"))
+check("tracks frozen at 120 -> STUCK to 123.99, HEADING at 124 (4 s hold)",
+ [ns(seq_p, t)["word"] for t in (120.5, 123.99, 124.0)] == ["STUCK", "STUCK", "HEADING"])
+seq_s = evs((100.0, G), (130.0, "Robot is stuck. Replanning."), (130.001, "Replanning."))
+seq_s_ign = evs((100.0, G), (130.0, "Robot is stuck. Replanning."),
+ (130.025, "MOSS base: twist vx=+0.000 vy=+0.000(ignored) wz=-0.385 -> tracks L=+100% R=-100% (limit "
+ "100%)"), (130.6, "changed state"))
+check("the planner's stuck replan (the verdict + dimOS's 'Replanning.') at 130 -> STUCK to 132.99, HEADING at 133 "
+ "(3 s hold); the verdict the planner ignores (the pivot twist follows, no 'Replanning.') -> HEADING throughout",
+ [ns(seq_s, t)["word"] for t in (130.5, 132.99, 133.0)] == ["STUCK", "STUCK", "HEADING"]
+ and [ns(seq_s_ign, t)["word"] for t in (130.3, 131.0, 133.0)] == ["HEADING"] * 3 and len(seq_s_ign) == 1)
+seq_i = evs((100.0, G), (150.0, "IMU contact: forward -6.1 m/s^2 (threshold 4.0) while commanded L=+70% R=+60%"))
+check("an IMU contact at 150 (no escape line) -> BACKING OFF to 155.99, HEADING at 156",
+ [ns(seq_i, t)["word"] for t in (150.5, 155.99, 156.0)] == ["BACKING OFF", "BACKING OFF", "HEADING"])
+seq_e = evs((100.0, G), (160.0, STALL), (160.3, "BUMP received"), (162.4, "Contact escape"))
+check("stall 160, bump 160.3, escape done 162.4 -> BACKING OFF at 162.3, WAITING at 162.5 (the escape ends the "
+ "hold; the explorer runs)",
+ [ns(seq_e, t)["word"] for t in (162.3, 162.5)] == ["BACKING OFF", "WAITING"]
+ and ns(seq_e, 162.5) == {"word": "WAITING", "detail": "Escape done", "since": 162.4})
+seq_tap = evs((200.0, "Got new goal"), (205.0, STALL), (207.0, "Contact escape"), (220.0, "Got new goal"),
+ (230.0, "Reached goal position, starting final rotation"))
+check("a tapped goal (no explorer line): new goal -> HEADING; escape done -> READY; reached -> READY "
+ "(exploring true from the bus -> HEADING)",
+ [ns(seq_tap, t)["word"] for t in (201.0, 207.5, 221.0, 231.0)] == ["HEADING", "READY", "HEADING", "READY"]
+ and ns(seq_tap, 231.0, exploring=True)["word"] == "HEADING"
+ and ns(seq_tap, 221.0)["detail"] == "Planner: new goal")
+seq_w = evs((100.0, G), (105.0, "planner gave up on that goal: excluded until the map or the viewpoint changes"),
+ (106.0, "3 of 9 clusters are on a recently failed goal: waiting for the map or the viewpoint to change, "
+ "not stopping"))
+check("goal dropped, then waiting -> WAITING 'Waiting: 3 of 9 frontiers are failed goals', since 106",
+ ns(seq_w, 107.0) == {"word": "WAITING", "detail": "Waiting: 3 of 9 frontiers are failed goals",
+ "since": 106.0})
+seq_st = evs((100.0, G), (140.0, STALL), (141.0, "WavefrontFrontierExplorer: stop_movement received, stopping "
+ "exploration"))
+check("a stop during a contact hold -> STOPPED at once (141.5), the hold is over",
+ ns(seq_st, 141.5)["word"] == "STOPPED" and ns(seq_st, 145.0)["word"] == "STOPPED")
+seq_d = evs((100.0, G), (300.0, "exploration complete: no reachable frontier left (12 targets, 150 ms)"))
+check("exploration complete -> DONE", ns(seq_d, 301.0) == {"word": "DONE", "detail": "Exploration complete: no "
+ "reachable frontier left (12 goals)", "since": 300.0})
+seq_pv = evs((100.0, G), (101.0, "MOSS base: pivot L=+100% R=-100%: the right track did not turn (encoders "
+ "L=+900 R=+4 ticks in 1.0 s) - not a contact: from now on this pivot turns on "
+ "the left track only, the right track braked"))
+check("a pivot fallback while heading -> still HEADING, detail the goal line",
+ ns(seq_pv, 102.0) == {"word": "HEADING", "detail": GT, "since": 100.0})
+iso = "2026-01-01T07:49:15.000Z"
+t_iso = _dt.datetime(2026, 1, 1, 7, 49, 15, tzinfo=_dt.timezone.utc).timestamp()
+seq_iso = mc.decisions_from_records([{"ts": "2026-01-01T07:49:10.000Z", "event": G}, {"ts": iso, "event": OBST}])
+check("ISO timestamps as dimOS writes them (UTC, Z): ts kept as given; OBSTACLE 1 s after, HEADING 3.5 s after",
+ seq_iso[1]["ts"] == iso and ns(seq_iso, t_iso + 1.0)["word"] == "OBSTACLE"
+ and ns(seq_iso, t_iso + 3.5)["word"] == "HEADING")
+check("an obstacle whose time cannot be read holds nothing (HEADING)",
+ ns(evs((100.0, G), ("soon", OBST)), 101.0)["word"] == "HEADING")
+NO_PATH = mc.decisions_from_records(recs((0.0, "Got new goal"), (0.001, "Cancelling goal."), (0.1, "Found safe goal."),
+ (0.2, "No path found to the goal.", {"x": 1.03, "y": -2.72}),
+ (0.21, "Cancelling goal.")))
+NO_SAFE = mc.decisions_from_records(recs((0.0, "Got new goal"), (0.1, "No safe goal found near requested target."),
+ (0.11, "No safe goal found.", {"x": 1.0, "y": 2.0}),
+ (0.12, "Cancelling goal.")))
+ABANDON = mc.decisions_from_records(recs((0.0, "Got new goal"), (0.1, "Found safe goal."),
+ (5.0, "Robot is stuck. Replanning."),
+ (5.01, "Goal abandoned, no recovery motion", {"trigger": "stuck_detector"}),
+ (5.02, "Cancelling goal.")))
+check("a goal tapped on the map (no explorer) that the planner drops -> the reason in the feed, READY 10 s later: "
+ "'Goal dropped: no path to (1.03, -2.72)', 'Goal dropped: no safe spot near it' (its second line an echo), "
+ "'Stuck: goal abandoned' (STUCK until 8.01)",
+ kt(NO_PATH)[1:] == [("goal_dropped", "planner", "Goal dropped: no path to (1.03, -2.72)")]
+ and kt(NO_SAFE)[1:] == [("goal_dropped", "planner", "Goal dropped: no safe spot near it")]
+ and kt(ABANDON)[1:] == [("goal_dropped", "recovery", "Stuck: goal abandoned")]
+ and [mc.now_state(x, True, False, 10.0)["word"] for x in (NO_PATH, NO_SAFE, ABANDON)] == ["READY"] * 3
+ and mc.now_state(NO_PATH, True, False, 10.0)["detail"] == "Goal dropped: no path to (1.03, -2.72)"
+ and [mc.now_state(ABANDON, True, False, t)["word"] for t in (6.0, 8.0, 8.02)] == ["STUCK", "STUCK", "READY"],
+ json.dumps([kt(NO_PATH), kt(NO_SAFE), kt(ABANDON)])[:300])
+drop_x = mc.decisions_from_records(recs((0.0, G), (0.03, "Got new goal"), (0.2, "No path found to the goal."),
+ (0.21, "Cancelling goal."),
+ (0.215, "planner gave up on that goal: excluded until the map or the "
+ "viewpoint changes")))
+check("the same while exploring: the explorer's 'planner gave up' 15 ms after the planner's reason is its echo, "
+ "the reason stays; WAITING",
+ kt(drop_x)[1:] == [("goal_dropped", "planner", "Goal dropped: no path")]
+ and ns(drop_x, 1.0)["word"] == "WAITING", json.dumps(kt(drop_x)))
+dead = mc.decisions_from_records(recs((0.0, G), (0.5, "BUMP received", {"acted": True}),
+ (1.0, "Contact during an escape: stopping dead", {"trigger": "bump"}),
+ (1.0, "BUMP received", {"acted": False}),
+ (1.05, "Contact escape", {"travelled_m": 0.05, "seconds": 0.5,
+ "aborted": True})))
+check("a second contact during an escape: 'Contact during an escape: stop dead' (its BUMP acted=False backs nothing "
+ "off: no row), 'Escape aborted after 0.05 m'; WAITING once it ended",
+ [e["text"] for e in dead[1:]] == ["Bump: backing off", "Contact during an escape: stop dead",
+ "Escape aborted after 0.05 m"]
+ and ns(dead, 1.04)["word"] == "BACKING OFF" and ns(dead, 1.1)["word"] == "WAITING", json.dumps(kt(dead)))
+corner = mc.decisions_from_records(recs((0.0, "born cornered: 0.40 m2 of reachable floor and no frontier - one 0.20 m "
+ "back-off via the bump reflex"),
+ (0.05, "BUMP received", {"acted": True}),
+ (2.2, "Contact escape", {"travelled_m": 0.2, "seconds": 2.1,
+ "aborted": False})))
+check("born cornered: one row 'Cornered (0.40 m2 of floor, no frontier): backing off 0.20 m' (an obstacle, not a "
+ "contact; the BUMP it publishes is its echo), BACKING OFF until 'Escape done: 0.20 m in 2.1 s', then WAITING",
+ kt(corner) == [("obstacle", "explorer", "Cornered (0.40 m2 of floor, no frontier): backing off 0.20 m"),
+ ("contact", "recovery", "Escape done: 0.20 m in 2.1 s")]
+ and [ns(corner, t)["word"] for t in (1.0, 2.19, 2.3)] == ["BACKING OFF", "BACKING OFF", "WAITING"],
+ json.dumps(kt(corner)))
+quiet = mc.decisions_from_records(recs((0.0, G), (5.0, "Robot veered off track. Replanning.", {"deviation": 0.6}),
+ (5.001, "Replanning."), (9.0, "Failure in navigation."),
+ (9.001, "Replanning.")))
+check("off track + 'Replanning.', a navigation failure + 'Replanning.' -> replan rows with no hold: HEADING "
+ "'Off track: replanning' at 5.5, 'Navigation failed: replanning' at 9.5 (no OBSTACLE: nothing was in the way)",
+ ns(quiet, 5.5) == {"word": "HEADING", "detail": "Off track: replanning", "since": 5.0}
+ and ns(quiet, 9.5) == {"word": "HEADING", "detail": "Navigation failed: replanning", "since": 9.0},
+ json.dumps([ns(quiet, 5.5), ns(quiet, 9.5)]))
+tap = mc.decisions_from_records(recs((0.0, "WavefrontFrontierExplorer: stop_movement received, stopping exploration"),
+ (0.5, "Got new goal"), (0.6, "Found safe goal.")))
+check("STOP, then a goal tapped 0.5 s later (not exploring: the page sends it at once) -> its own row, HEADING; "
+ "only the explorer's stay-here goal within 0.2 s of the stop is an echo",
+ kt(tap)[1:] == [("new_goal", "planner", "Planner: new goal")] and ns(tap, 3.0, False)["word"] == "HEADING"
+ and mc.AFTER_STOP_ECHO_S == 0.2, json.dumps(kt(tap)))
+halt = mc.decisions_from_records(recs((0.0, G), (1.0, "BUMP received", {"acted": True}),
+ (1.5, "WavefrontFrontierExplorer: stop_movement received, stopping exploration"),
+ (1.55, "Contact escape", {"travelled_m": 0.04, "seconds": 0.5, "aborted": True})))
+check("a STOP during an escape: the escape's aborted end after it leaves STOPPED",
+ ns(halt, 2.0) == {"word": "STOPPED", "detail": "Exploration stopped (stop button or pad)", "since": 1.5},
+ json.dumps(ns(halt, 2.0)))
+
+# echoes, and the feed carried across reads
+check("goal, then the planner's 'Got new goal' 1 ms later and 'Found safe goal.' -> 1 event; 'Got new goal' 2 s "
+ "later -> kept; alone -> kept",
+ len(evs((100.0, G), (100.001, "Got new goal"), (100.1, "Found safe goal."))) == 1
+ and len(evs((100.0, G), (102.0, "Got new goal"))) == 2 and len(evs((200.0, "Got new goal"))) == 1)
+stop5 = evs((300.0, "WavefrontFrontierExplorer: stop_movement received, stopping exploration"),
+ (300.001, "Received exploration stop command via LCM"), (300.005, "Stopped autonomous frontier exploration"),
+ (300.01, "Got new goal"), (300.06, "Reached goal position, starting final rotation"))
+check("a STOP: its two stop lines, the explorer's line and its stay-here goal and arrival -> 1 event, STOPPED",
+ [e["text"] for e in stop5] == ["Exploration stopped (stop button or pad)"]
+ and ns(stop5, 301.0)["word"] == "STOPPED")
+prev = {"ts": 10.0, "kind": "replan", "text": "Replanning", "src": "planner", "seq": 7}
+check("across reads: seq continues from seq0, the echo rule sees the previous read's last event",
+ mc.decisions_from_records([{"ts": 10.2, "event": "Replanning."}], 7, prev) == []
+ and mc.decisions_from_records([{"ts": 12.0, "event": "Replanning."}], 7, prev)[0]["seq"] == 8)
+
+# bad records never raise
+bad = [None, 3, "x", [], {}, {"event": None}, {"event": 5}, {"level": "info"}, {"event": " "},
+ {"ts": object(), "event": OBST}, {"ts": float("nan"), "event": OBST}, {"event": OBST}]
+try:
+ got = mc.decisions_from_records(bad)
+ raised = None
+except Exception as exc: # noqa: BLE001
+ got, raised = None, exc
+check("records that are not dicts, without an event or with an odd one -> skipped, no exception; a ts that is "
+ "not a time (an object, NaN, none) -> null",
+ raised is None and [(e["kind"], e["ts"]) for e in got] == [("obstacle", None)] * 3,
+ repr(raised) if raised else json.dumps(got, default=str))
+try:
+ ok_nones = (mc.decisions_from_records(None) == [] and mc.decisions_from_records(5) == []
+ and mc.now_state([None, 1, {"kind": None}, {"kind": "goal", "ts": object()},
+ {"kind": "obstacle", "ts": "x"}], True, None, 1.0)["word"] in WORDS)
+except Exception as exc: # noqa: BLE001
+ ok_nones = False
+ print(" raised:", repr(exc))
+check("decisions_from_records(None / 5) -> [], now_state on junk events -> a word, no exception", ok_nones)
+
+# live: the running stack's main.jsonl through /decisions.json
+dec_dir = Path(TMP, "logs", "decisions")
+dec_dir.mkdir(parents=True)
+main = Path(dec_dir, "main.jsonl")
+DEC_RUN = "20260101-120000-moss-dimos-moss-explore"
+reg_i = Path(runs, "i.json")
+reg_i.write_text(json.dumps({"run_id": DEC_RUN, "pid": os.getpid(), "blueprint": "moss-dimos.moss-explore",
+ "started_at": (_dt.datetime.now(_dt.timezone.utc) + _dt.timedelta(seconds=5)).isoformat(),
+ "log_dir": str(dec_dir)}))
+
+
+def jline(event, t, logger="dimos/navigation/replanning_a_star/global_planner.py"):
+ """One main.jsonl record with the fields a real one carries."""
+ stamp = _dt.datetime.fromtimestamp(t, _dt.timezone.utc).strftime("%Y-%m-%dT%H:%M:%S.%fZ")
+ return json.dumps({"event": event, "level": "info", "logger": logger, "timestamp": stamp,
+ "func_name": "f", "lineno": 1, "module": "x", "worker": 3}) + "\n"
+
+
+t0 = time.time() - 60.0
+twist = "MOSS base: twist vx=+0.313 vy=+0.000(ignored) wz=-0.200 -> tracks L=+75% R=+64% (limit 100%)"
+old_goals = "".join(jline(f"goal {90 + i}: (0.00, 0.00) 1.0 m away, 5 frontier cells, 2 clusters, chosen in 9 ms",
+ t0, "/home/x/moss/moss_dimos/explorer2.py") for i in range(5))
+chatter = jline(twist, t0 + 1.0, "/home/x/moss/moss_dimos/moss_adapter.py")
+n_chatter = (mc.DECISIONS_TAIL_BYTES // len(chatter)) + 200 # more than the 1 MiB window by itself
+fresh = "".join(jline(f"goal {i}: (1.00, 2.00) 2.5 m away, 30 frontier cells, 9 clusters, chosen in 100 ms"
+ if i % 2 == 0 else "3 of 9 clusters are on a recently failed goal: waiting for the map or the "
+ "viewpoint to change, not stopping", t0 + 2.0 + i * 0.5, "/home/x/moss/moss_dimos/explorer2.py")
+ for i in range(45))
+main.write_text(old_goals + chatter * n_chatter + fresh)
+real_pid = mc._dimos_run_pid
+mc._dimos_run_pid = lambda: None
+srv_d, cd = serve(demo=False, page_dir=page_dir)
+t_read = time.perf_counter()
+code, dj = cd.json("/decisions.json?n=40")
+t_read = time.perf_counter() - t_read
+evs40 = dj.get("events", [])
+_, dall = cd.json("/decisions.json?n=500")
+check(f"/decisions.json?n=40 on a {main.stat().st_size / 1e6:.2f} MB main.jsonl (5 goals, then {n_chatter} twist "
+ f"lines, then 45 decisions) -> 40 events, the newest last, ts as written, run named; all of them = the 45 "
+ f"inside the 1 MiB window (first read {t_read * 1000:.0f} ms)",
+ code == 200 and set(dj) == {"now", "events", "run"} and len(evs40) == 40 and dj["run"] == DEC_RUN
+ and evs40[-1]["text"] == "Goal 44: 2.5 m away (30 cells)" and len(dall["events"]) == 45
+ and not any(e["text"].startswith("Goal 9") for e in dall["events"])
+ and evs40[-1]["ts"].endswith("Z") and all(set(e) == EVENT_KEYS for e in evs40),
+ json.dumps(dj)[:300])
+check("... now: the last line is goal 44 -> HEADING, since its ts",
+ dj.get("now", {}).get("word") == "HEADING" and dj["now"]["since"] == evs40[-1]["ts"], json.dumps(dj.get("now")))
+last_seq = evs40[-1]["seq"] if evs40 else 0
+stuck_t = time.time()
+torn = jline("Robot is stuck. Replanning.", stuck_t)
+with open(main, "a") as fh:
+ fh.write(jline(OBST, time.time()) + torn[:30])
+code, dj2 = cd.json("/decisions.json?n=40")
+check("a line appended within 0.5 s of the last read -> not read yet (2 Hz at most)",
+ dj2["events"] == evs40, json.dumps(dj2["events"][-1:]))
+time.sleep(0.6)
+code, dj3 = cd.json("/decisions.json?n=40")
+check("0.6 s later -> the obstacle, seq +1, OBSTACLE now; the torn last line waits",
+ dj3["events"][-1]["text"] == "Obstacle ahead: stop" and dj3["events"][-1]["seq"] == last_seq + 1
+ and dj3["now"]["word"] == "OBSTACLE", json.dumps(dj3["events"][-1:]) + json.dumps(dj3["now"]))
+with open(main, "a") as fh:
+ fh.write(torn[30:])
+time.sleep(0.6)
+code, dj4 = cd.json("/decisions.json?n=40")
+check("the torn line completed -> read whole: the stuck verdict, held until the planner's answer -> no event "
+ "yet, still OBSTACLE",
+ dj4["events"] == dj3["events"] and dj4["now"]["word"] == "OBSTACLE",
+ json.dumps(dj4["events"][-1:]) + json.dumps(dj4["now"]))
+with open(main, "a") as fh:
+ fh.write(jline("Replanning.", stuck_t + 0.002))
+time.sleep(0.6)
+code, dj4 = cd.json("/decisions.json?n=40")
+check("dimOS's 'Replanning.' in the next read -> 'Stuck: replanning' at the verdict's time, STUCK now",
+ dj4["events"][-1]["text"] == "Stuck: replanning" and dj4["events"][-1]["seq"] == last_seq + 2
+ and mc._epoch(dj4["events"][-1]["ts"]) == mc._epoch(json.loads(torn)["timestamp"])
+ and dj4["now"]["word"] == "STUCK", json.dumps(dj4["events"][-1:]) + json.dumps(dj4["now"]))
+code, dj5 = cd.json("/decisions.json?n=3")
+_, dj6 = cd.json("/decisions.json?n=abc")
+check("n=3 -> 3 events; n=abc -> 40", len(dj5["events"]) == 3 and len(dj6["events"]) == 40)
+main.write_text(jline(G, time.time()) + jline("BUMP received", time.time(),
+ "/home/x/moss/moss_dimos/recovering_planner.py"))
+time.sleep(0.6)
+code, dj7 = cd.json("/decisions.json?n=40")
+check("the file rewritten smaller (a new log) -> the feed starts over: 2 events, seq still growing, BACKING OFF",
+ [e["text"] for e in dj7["events"]] == [GT, "Bump: backing off"] and dj7["events"][0]["seq"] > last_seq + 2
+ and dj7["now"]["word"] == "BACKING OFF", json.dumps(dj7)[:300])
+reg_i.unlink()
+code, dj8 = cd.json("/decisions.json?n=40")
+check("the stack stops -> IDLE 'No stack running (last run: ...)', its events and run kept (what happened stays "
+ "readable, like the map)",
+ dj8 == {"now": {"word": "IDLE", "detail": f"No stack running (last run: {DEC_RUN})", "since": None},
+ "events": dj7["events"], "run": DEC_RUN}, json.dumps(dj8)[:300])
+mc._dimos_run_pid = lambda: 4242
+code, dj9 = cd.json("/decisions.json?n=40")
+check("a `dimos run` with no registry entry (foreground, swept registry) -> READY 'Stack running, its log is not "
+ "found', no events: never the last run's lines as if they were its own",
+ dj9 == {"now": {"word": "READY", "detail": "Stack running, its log is not found", "since": None},
+ "events": [], "run": None}, json.dumps(dj9))
+mc._dimos_run_pid = lambda: None
+dec_dir2 = Path(TMP, "logs", "decisions2")
+dec_dir2.mkdir(parents=True)
+Path(dec_dir2, "main.jsonl").write_text(jline("Obstacle detected ahead, stopping local planner.", time.time()))
+DEC_RUN2 = "20260101-130000-moss-dimos-moss-explore"
+reg_i.write_text(json.dumps({"run_id": DEC_RUN2, "pid": os.getpid(), "blueprint": "moss-dimos.moss-explore",
+ "started_at": (_dt.datetime.now(_dt.timezone.utc) + _dt.timedelta(seconds=6)).isoformat(),
+ "log_dir": str(dec_dir2)}))
+code, dj10 = cd.json("/decisions.json?n=40")
+check("the next run -> its own feed only (1 event, seq still growing), its run name",
+ [e["text"] for e in dj10["events"]] == ["Obstacle ahead: stop"] and dj10["run"] == DEC_RUN2
+ and dj10["events"][0]["seq"] > dj7["events"][-1]["seq"], json.dumps(dj10)[:300])
+reg_i.unlink()
+srv_d.shutdown()
+mc._dimos_run_pid = real_pid
+
+# demo: the scripted cycle through the same parser
+dsrv, dcl = serve(demo=True, page_dir=page_dir)
+code, dd = dcl.json("/decisions.json")
+check("demo /decisions.json -> now + events + run: exploration started, WAITING, run on moss-explore",
+ code == 200 and set(dd) == {"now", "events", "run"} and set(dd["now"]) == {"word", "detail", "since"}
+ and dd["now"]["word"] in WORDS and dd["events"] and dd["events"][0]["text"].startswith("Exploration started")
+ and all(set(e) == EVENT_KEYS for e in dd["events"]) and str(dd["run"]).endswith("moss-explore"),
+ json.dumps(dd)[:300])
+dsrv.cockpit._script_t0 -= mc.DEMO_DECISION_CYCLE_S + 0.5 # one whole cycle is due
+code, dd = dcl.json("/decisions.json")
+kinds = [e["kind"] for e in dd["events"]]
+cycle = ["mission", "goal", "obstacle", "replan", "contact", "contact", "contact", "contact", "goal_dropped", "goal",
+ "reached"]
+check("demo, one cycle later -> goal, obstacle, replan, contact (stall, bump, wall), escape (from its record's "
+ "fields: 'Escape done: 0.20 m in 2.1 s'), goal dropped, goal, reached; the stack log carries the same lines",
+ kinds[:len(cycle)] == cycle and dd["events"][7]["text"] == "Escape done: 0.20 m in 2.1 s"
+ and dd["now"]["word"] in WORDS, json.dumps(kinds))
+stamps = [e["ts"] for e in dd["events"]]
+check("demo times are dimOS's: ISO UTC with a Z, milliseconds, on the clock now (the page reads them as UTC)",
+ all(isinstance(t, str) and re.fullmatch(r"\d{4}-\d\d-\d\dT\d\d:\d\d:\d\d\.\d{3}Z", t) for t in stamps)
+ and abs(mc._epoch(stamps[0]) - time.time()) < 60, json.dumps(stamps[:2]))
+_, dlog = dcl.json("/log.json?n=200")
+check("demo: every decision has its raw line in /log.json",
+ any(ln["event"].startswith("MOSS base: STALL #1") for ln in dlog["lines"])
+ and any(ln["event"] == "Contact escape" for ln in dlog["lines"]))
+dcl.post("/cmd/stop", {})
+_, dd = dcl.json("/decisions.json")
+check("demo STOP -> STOPPED, 'Exploration stopped (stop button or pad)' (its second line is an echo)",
+ dd["now"]["word"] == "STOPPED" and dd["events"][-1]["text"] == "Exploration stopped (stop button or pad)",
+ json.dumps(dd["now"]))
+dcl.post("/cmd/explore", {"action": "start"})
+_, dd = dcl.json("/decisions.json")
+check("demo explore start -> WAITING, 'Exploration started (cap 3 m, radius 4 m)'",
+ dd["now"]["word"] == "WAITING" and dd["events"][-1]["text"] == "Exploration started (cap 3 m, radius 4 m)")
+dcl.post("/cmd/goal", {"x": 1.0, "y": 1.0})
+_, dd = dcl.json("/decisions.json")
+first = (dd["now"]["word"], dd["events"][-1]["text"])
+time.sleep(mc.GOAL_AFTER_STOP_S + 0.1) # the page sends the goal after the stop, as live
+_, dd = dcl.json("/decisions.json")
+check("demo goal while exploring -> 'Exploration stopped (stop command)' at once, the planner's new goal "
+ "3 s later -> HEADING",
+ first == ("STOPPED", "Exploration stopped (stop command)") and dd["now"]["word"] == "HEADING"
+ and dd["events"][-1]["text"] == "Planner: new goal", json.dumps([first, dd["now"]]))
+_, before = dcl.json("/decisions.json")
+dcl.post("/cmd/stack", {"action": "stop"})
+_, dd = dcl.json("/decisions.json")
+check("demo stack stop -> IDLE 'No stack running (last run: ...)', the run's events kept, as live",
+ dd["now"]["word"] == "IDLE" and dd["events"] == before["events"] and dd["events"]
+ and dd["run"] == before["run"] and dd["now"]["detail"] == f"No stack running (last run: {before['run']})",
+ json.dumps(dd)[:200])
+dcl.post("/cmd/stack", {"action": "map"})
+_, dd = dcl.json("/decisions.json")
+check("demo: a new stack -> a fresh feed", dd["events"] == [] and dd["now"]["word"] == "READY"
+ and dd["run"] != before["run"], json.dumps(dd)[:200])
+dsrv.shutdown()
+
+check("the server's docstring lists GET /decisions.json", "GET /decisions.json?n=40" in (mc.__doc__ or ""))
+cockpit_md = (ROOT / "docs" / "cockpit.md").read_text()
+check("docs/cockpit.md: a 'What it decides' section, /decisions.json, the link to imu.md",
+ "## What it decides" in cockpit_md and "/decisions.json" in cockpit_md and "(imu.md)" in cockpit_md)
+
# ---------------------------------------------------------------------------
print("F. hygiene")
src = (ROOT / "tools" / "moss_cockpit.py").read_text()
diff --git a/benches/bench_moss_cockpit_page_cold.py b/benches/bench_moss_cockpit_page_cold.py
new file mode 100644
index 0000000..af30963
--- /dev/null
+++ b/benches/bench_moss_cockpit_page_cold.py
@@ -0,0 +1,773 @@
+"""Cold bench for the cockpit page's decision panel and command arrow (tools/moss_cockpit/).
+
+Known input -> known output: metres on the map and pixels on the canvas for the arrow,
+rows, colours and words for the panel. Groups:
+
+ A. the page - index.html carries #mod-decisions ("What it decides.") above #mod-log in
+ the right column, #now-word, #now-detail and the #decisions list (an <ol>),
+ dimmed offline like the log; before the first answer it says '--', not IDLE;
+ on a phone it stacks after the controls and before the log; every id is
+ unique and every id app.js reads exists
+ B. the colours - the state word: warning for OBSTACLE / BACKING OFF / STUCK, accent for
+ HEADING, dim for the rest; the kinds: contact / push / imu warning,
+ obstacle / replan amber, goal / new_goal / reached accent, waiting /
+ goal_dropped / retry dim; amber is one token (--ui-caution), the depth
+ camera's colour on the map
+ C. the wiring - app.js polls /decisions.json?n=30 every 1000 ms through the same poller
+ as the others, draws the arrow from nav.last_cmd_vel at the robot through
+ pose(), and talks to no other host (relative paths only)
+ D. in node - node --check app.js; the page's own functions run on known input:
+ the arrow in metres (0.45 m/s = 0.6 m, reverse behind the robot, an arc
+ bends left for a left turn, a pivot sweeps the turn's direction, zero and
+ stale commands draw nothing) and on a recording canvas in pixels: it starts
+ at the marker's nose (tail in reverse) and runs at least 22 px past it, so
+ at the fitted zoom (37 px/m) a 0.2 m/s command shows; a pivot at any heading
+ starts at the nose; the feed (newest on top, 30 rows, mm:ss, kind pill and
+ colour, text escaped, only new rows flash); the state word (colour class,
+ detail, time since it was set); and the panel itself: pollDecisions and
+ renderDecisions on a stub document (word, colour, detail, 'for N s', the
+ newest row on top, an error answer keeping the feed, a failed fetch keeping
+ everything)
+ E. the server - when the cockpit serves /decisions.json (demo mode), its answer goes through
+ the page's functions: one row per event, every kind and word known
+
+Nothing here needs dimOS, a bus or hardware; D needs node (skipped with a line without it).
+
+Run: PYTHONDONTWRITEBYTECODE=1 PYTHONPATH=. .venv/bin/python benches/bench_moss_cockpit_page_cold.py
+"""
+
+import json
+import math
+import os
+import re
+import shutil
+import subprocess
+import sys
+import tempfile
+import threading
+import urllib.error
+import urllib.request
+from datetime import datetime, timezone
+from html.parser import HTMLParser
+from pathlib import Path
+
+ROOT = Path(__file__).resolve().parent.parent
+PAGE = ROOT / "tools" / "moss_cockpit"
+sys.path.insert(0, str(ROOT))
+sys.path.insert(0, str(ROOT / "tools"))
+
+OK = 0
+KO = 0
+_TMPDIR = tempfile.TemporaryDirectory(prefix="moss_cockpit_page_bench_")
+os.environ["MOSS_STATE_DIR"] = os.path.join(_TMPDIR.name, "state")
+os.environ["XDG_STATE_HOME"] = os.path.join(_TMPDIR.name, "xdg") # an empty dimOS run registry
+
+
+def check(label, ok, detail=""):
+ global OK, KO
+ print(f" {'OK ' if ok else 'KO '} {label}{' - ' + detail if detail else ''}")
+ if ok:
+ OK += 1
+ else:
+ KO += 1
+
+
+html = (PAGE / "index.html").read_text()
+js = (PAGE / "app.js").read_text()
+tokens = (PAGE / "tokens.css").read_text()
+
+# the decision catalogue (kinds) and the state words, as the server sends them
+KINDS = {"contact": "warn", "push": "warn", "imu": "warn", "obstacle": "caution", "replan": "caution",
+ "goal": "accent", "new_goal": "accent", "reached": "accent",
+ "waiting": "dim", "goal_dropped": "dim", "retry": "dim"}
+ALL_KINDS = list(KINDS) + ["pivot_fallback", "mission"]
+WORDS = {"OBSTACLE": "warn", "BACKING OFF": "warn", "STUCK": "warn", "HEADING": "accent",
+ "IDLE": "dim", "READY": "dim", "STOPPED": "dim", "DONE": "dim", "WAITING": "dim"}
+
+
+class Elements(HTMLParser):
+ """Every element with an id: tag, offset in the file, and the ids of its open ancestors."""
+
+ VOID = {"meta", "link", "img", "input", "br", "hr", "source", "path"}
+
+ def __init__(self):
+ super().__init__()
+ self.stack, self.found, self.ids = [], {}, []
+
+ def handle_starttag(self, tag, attrs):
+ a = dict(attrs)
+ if "id" in a:
+ self.ids.append(a["id"])
+ self.found.setdefault(a["id"], {"tag": tag, "pos": self.getpos(), "attrs": a,
+ "in": [s[1] for s in self.stack if s[1]]})
+ if tag not in self.VOID:
+ self.stack.append((tag, a.get("id")))
+
+ def handle_startendtag(self, tag, attrs):
+ a = dict(attrs)
+ if "id" in a:
+ self.ids.append(a["id"])
+ self.found.setdefault(a["id"], {"tag": tag, "pos": self.getpos(), "attrs": a,
+ "in": [s[1] for s in self.stack if s[1]]})
+
+ def handle_endtag(self, tag):
+ for i in range(len(self.stack) - 1, -1, -1):
+ if self.stack[i][0] == tag:
+ del self.stack[i:]
+ break
+
+
+# ---------------------------------------------------------------------------
+print("A. the page")
+el = Elements()
+el.feed(html)
+E = el.found
+need = ["mod-decisions", "dec-title", "now-word", "now-detail", "now-since", "decisions", "mod-log", "col-right"]
+check("index.html has #mod-decisions, #now-word, #now-detail, #now-since, #decisions, #mod-log",
+ all(i in E for i in need), ", ".join(i for i in need if i not in E))
+dups = sorted({i for i in el.ids if el.ids.count(i) > 1})
+check("every id in index.html is unique", not dups, ", ".join(dups))
+md, ml = E.get("mod-decisions", {}), E.get("mod-log", {})
+check("#mod-decisions sits ABOVE #mod-log, both in the right column (#col-right)",
+ md and ml and md["pos"] < ml["pos"] and "col-right" in md["in"] and "col-right" in ml["in"],
+ f"decisions {md.get('pos')} {md.get('in')}, log {ml.get('pos')} {ml.get('in')}")
+title = re.search(r'<h2 class="title" id="dec-title">([^<]*)</h2>', html)
+check('the module is titled "What it decides." and is labelled by that title',
+ bool(title) and title.group(1) == "What it decides."
+ and md.get("attrs", {}).get("aria-labelledby") == "dec-title", title.group(1) if title else "no #dec-title")
+check("#now-word, #now-detail and #decisions are inside #mod-decisions; #decisions is an <ol>",
+ all("mod-decisions" in E.get(i, {}).get("in", []) for i in ("now-word", "now-detail", "now-since", "decisions"))
+ and E.get("decisions", {}).get("tag") == "ol")
+check("#now-word announces itself (role status, aria-live polite)",
+ E.get("now-word", {}).get("attrs", {}).get("role") == "status"
+ and E.get("now-word", {}).get("attrs", {}).get("aria-live") == "polite")
+live = re.search(r'<div class="now" data-live>[\s\S]*?</div>', html)
+check("offline, the word and the feed keep their last values, dimmed like the log (data-live)",
+ bool(live) and 'id="now-word"' in live.group(0) and "data-live" in E.get("decisions", {}).get("attrs", {})
+ and "body.offline [data-live]" in html)
+nw0 = re.search(r'<p id="now-word" class="(\w*)"[^>]*>([^<]*)</p>', html)
+nd0 = re.search(r'<p id="now-detail">([^<]*)</p>', html)
+feed0 = re.search(r'<ol id="decisions"[^>]*>([\s\S]*?)</ol>', html)
+check("before the first answer the panel says it does not know: '--' dim, 'Waiting for the cockpit.', an empty "
+ "feed (not IDLE / 'No stack running.' while a run may be on)",
+ bool(nw0 and nd0 and feed0) and nw0.group(1) == "dim" and nw0.group(2) == "--"
+ and nd0.group(1) == "Waiting for the cockpit." and feed0.group(1).strip() == "",
+ json.dumps([nw0.groups() if nw0 else None, nd0.group(1) if nd0 else None, feed0.group(1) if feed0 else None]))
+mob = re.search(r"@media \(max-width: 1099px\) \{([\s\S]*?)\n \}", html)
+orders = dict(re.findall(r"#(mod-[a-z]+) \{ order: (\d+); \}", mob.group(1))) if mob else {}
+check("phone layout: the decisions stack after the controls and before the log (order 3 < 4 < log)",
+ {"mod-controls", "mod-decisions", "mod-log"} <= set(orders)
+ and int(orders["mod-controls"]) < int(orders["mod-decisions"]) < int(orders["mod-log"])
+ and len(set(orders.values())) == len(orders), json.dumps(orders))
+read_ids = set(re.findall(r'\$\("([\w-]+)"\)', js)) | set(re.findall(r'setText\("([\w-]+)"', js)) \
+ | set(re.findall(r'meter\("([\w-]+)"', js))
+missing = sorted(i for i in read_ids if i not in E)
+check(f"every id app.js reads ($, setText, meter: {len(read_ids)}) exists in index.html", not missing,
+ ", ".join(missing))
+check("the map caption's subtitle still carries the command (cap-sub, last_cmd_vel)",
+ 'setText("cap-sub", sub);' in js and 'sub += (sub ? " \u00b7 " : "") + "cmd "' in js and 'id="cap-sub"' in html)
+
+# ---------------------------------------------------------------------------
+print("B. the colours")
+
+
+def rule(sel):
+ m = re.search(re.escape(sel) + r" \{([^}]*)\}", html)
+ return m.group(1) if m else ""
+
+
+check("#now-word is big and uppercase", "text-transform: uppercase" in rule("#now-word")
+ and int(re.search(r"font-size: (\d+)px", rule("#now-word")).group(1)) >= 28, rule("#now-word"))
+check("#now-word.warn = the page's warning colour (--ui-attention), .accent = mint, .dim = --ui-dim",
+ "color: var(--ui-attention)" in rule("#now-word.warn") and "color: var(--moss-mint)" in rule("#now-word.accent")
+ and "color: var(--ui-dim)" in rule("#now-word.dim"))
+check("feed pills: warn filled with --ui-attention, caution in --ui-caution, accent in mint, dim in --ui-dim",
+ "background: var(--ui-attention)" in rule("#decisions .warn .k")
+ and "color: var(--ui-caution)" in rule("#decisions .caution .k")
+ and "color: var(--moss-mint)" in rule("#decisions .accent .k")
+ and "color: var(--ui-dim)" in rule("#decisions .dim .k, #decisions .dim .x"))
+caution = re.search(r"--ui-caution: (#[0-9A-Fa-f]{6});", tokens)
+depth = re.search(r'var DEPTH_COLOR = "rgba\((\d+),(\d+),(\d+),', js)
+rgb = tuple(int(caution.group(1)[i:i + 2], 16) for i in (1, 3, 5)) if caution else None
+check("amber is one token: --ui-caution in tokens.css = the depth camera's dots on the map (255, 193, 94); "
+ "the HUD's camera line uses it", rgb is not None and depth is not None
+ and rgb == tuple(int(g) for g in depth.groups()) and rgb == (255, 193, 94)
+ and "color: var(--ui-caution)" in rule("#hud-cam"), f"{rgb} {depth.groups() if depth else None}")
+
+# ---------------------------------------------------------------------------
+print("C. the wiring")
+poll = re.search(r"var POLL = \{[^}]*\bdecisions: (\d+)", js)
+rows = re.search(r"^var DECISION_ROWS = (\d+);", js, re.M)
+check("app.js polls /decisions.json?n=30 every 1000 ms (POLL.decisions, poller like the log)",
+ poll is not None and int(poll.group(1)) == 1000 and rows is not None and int(rows.group(1)) == 30
+ and 'fetchJSON("/decisions.json?n=" + DECISION_ROWS)' in js and "poller(pollDecisions, POLL.decisions);" in js,
+ (poll.group(0) if poll else "no POLL.decisions") + (rows.group(0) if rows else " no DECISION_ROWS"))
+check("a failed fetch keeps the last panel (as the log); a server without the endpoint says so",
+ re.search(r"function pollDecisions\(\) \{[\s\S]*?\}, function \(\) \{\}\);\n\}", js) is not None
+ and "d.__missing" in js and "no decision feed" in js)
+check("app.js names every kind of the catalogue and every state word",
+ all(re.search(r"\b" + k + r": \"", js) for k in ALL_KINDS) and all('"' + w + '"' in js for w in WORDS),
+ ", ".join([k for k in ALL_KINDS if not re.search(r"\b" + k + r": \"", js)] + [w for w in WORDS if '"' + w + '"' not in js]))
+check("the map draws the arrow at the robot: cmdArrow(nav.last_cmd_vel, pose(), age since the state), "
+ "then the robot marker over it",
+ re.search(r"var p = pose\(\);\n(?:\s*//.*\n)+\s*var cmd = cmdArrow\(nav && nav\.last_cmd_vel, p, sinceState\(\)\);\n"
+ r"\s*if \(cmd\) drawCmd\(cmd\);\n\s*if \(p && typeof p\.x === \"number\"\) \{", js) is not None)
+check("the server's command carries what the arrow reads: vx, wz, age_s (live and demo)",
+ all(k in (ROOT / "tools" / "moss_cockpit.py").read_text() for k in ('"last_cmd_vel"', '"vx"', '"wz"', '"age_s"')))
+urls = re.findall(r'fetchJSON\("([^"]*)"', js) + re.findall(r'src: "([^"]*)"', js) \
+ + re.findall(r'\.src = "([^"]*)"', js)
+check(f"app.js talks to no other host: no '://', no '//host' literal, every URL relative ({len(urls)})",
+ "://" not in js and not re.search(r"""["']//""", js) and urls and all(u.startswith("/") for u in urls)
+ and "new WebSocket" not in js and "EventSource" not in js, ", ".join(u for u in urls if not u.startswith("/")))
+check("index.html loads nothing from another host", "://" not in re.sub(r"<svg[\s\S]*?</svg>", "", html)
+ and all(u.startswith("/") for u in re.findall(r'(?:src|href)="([^"]+)"', html)))
+
+# ---------------------------------------------------------------------------
+print("D. in node")
+
+
+def js_def(name):
+ """app.js's top-level `function name(...) {...}` or `var name = ...;` (brace matched), or ""."""
+ m = re.search(r"^(function " + re.escape(name) + r"\(|var " + re.escape(name) + r" = )", js, re.M)
+ if not m:
+ return ""
+ i, depth, start = m.end(), 0, m.start()
+ line_end = js.find("\n", i)
+ if m.group(1).startswith("var") and not js[i:line_end].rstrip().endswith("{"):
+ return js[start:line_end] # a one-line var (a trailing // comment is harmless)
+ i = js.find("{", i)
+ while i < len(js):
+ if js[i] == "{":
+ depth += 1
+ elif js[i] == "}":
+ depth -= 1
+ if depth == 0:
+ tail = js.find("\n", i)
+ return js[start:tail if tail > 0 else len(js)]
+ i += 1
+ return ""
+
+
+NAMES = ["LIVE_S", "CMD_ARROW_S", "PIVOT_ARROW_S", "PIVOT_MIN_RAD", "CMD_VX_EPS", "CMD_WZ_EPS", "PIVOT_RADIUS_M",
+ "CMD_STEPS",
+ "CMD_W", "CMD_HEAD", "CMD_MIN_RUN", "PIVOT_R_PX", "MARK_L", "MINT", "HALO", "DECISION_ROWS",
+ "KIND_CLASS", "KIND_LABEL", "WORD_CLASS", "WORD_DETAIL", "pad2", "dur", "esc", "sx", "sy", "cmdArrow",
+ "trimEnd", "drawCmd", "tsSeconds", "minSec", "decisionRows", "freshRows", "decisionsHtml", "nowView"]
+PANEL = ["$", "setText", "stack", "sinceState", "serverNow", "pollDecisions", "renderDecisions"]
+defs = {n: js_def(n) for n in NAMES + PANEL}
+node = shutil.which("node")
+if node is None:
+ print(" -- skipped, no node here: node --check app.js and the page's functions on known input")
+else:
+ r = subprocess.run([node, "--check", str(PAGE / "app.js")], capture_output=True, text=True, timeout=30)
+ check("node --check app.js", r.returncode == 0, r.stderr.strip()[:200])
+ check("the page's functions and constants are found in app.js", all(defs.values()),
+ ", ".join(n for n, d in defs.items() if not d))
+
+ T0 = 1_000_000_000 # an epoch second the events count from (UTC)
+ harness = "\n".join(defs.values()) + "\n" + r"""
+var S = { view: { scale: 100, x: 0, y: 3 } }; // 100 px per metre; world (0, 3) at the top-left corner
+var ops = [];
+var ctx = { save: function () {}, restore: function () {}, closePath: function () { ops.push(["close"]); },
+ beginPath: function () { ops.push(["begin"]); },
+ moveTo: function (x, y) { ops.push(["move", x, y]); }, lineTo: function (x, y) { ops.push(["line", x, y]); },
+ arc: function (x, y, r, a0, a1, acw) { ops.push(["arc", x, y, r, a0, a1, !!acw]); },
+ stroke: function () { ops.push(["stroke", this.strokeStyle, this.lineWidth]); },
+ fill: function () { ops.push(["fill", this.fillStyle]); } };
+function drawn(a) { ops = []; drawCmd(a); return ops; }
+var P = { x: 1, y: 2, yaw: 0 }, O = { x: 0, y: 0, yaw: 0 };
+var C = function (vx, wz, age) { return { vx: vx, wz: wz, age_s: age === undefined ? 0.1 : age }; };
+// the same draw at another map scale (px per metre), the page's fitted zoom on the demo is 37.1
+function drawnAt(scale, a) { var s0 = S.view.scale; S.view.scale = scale; var o = drawn(a); S.view.scale = s0; return o; }
+var fit = {
+ slow: drawnAt(37.1, cmdArrow(C(0.2, 0), P, 0)),
+ fast: drawnAt(37.1, cmdArrow(C(0.45, 0), P, 0)),
+ metric: drawnAt(50, cmdArrow(C(0.45, 0), P, 0)),
+ arc: drawnAt(37.1, cmdArrow(C(0.3, 0.2), P, 0)),
+ back: drawnAt(37.1, cmdArrow(C(-0.15, 0), P, 0))
+};
+var T0 = """ + str(T0) + r""";
+var ev = [];
+for (var i = 1; i <= 40; i++) ev.push({ ts: T0 + 7 * i, kind: ["goal", "obstacle", "replan", "contact", "waiting"][i % 5],
+ text: "event " + i, src: "explorer", seq: i });
+var rowsA = decisionRows(ev.slice(0, 38), 30), rowsB = decisionRows(ev, 30);
+var rowsC = decisionRows([{ ts: T0, kind: "goal", text: "other run", seq: 1 }], 30);
+var dupA = decisionRows([{ ts: 62, kind: "replan", text: "Replanning", seq: 1 }], 30);
+var dupB = decisionRows([{ ts: 62, kind: "replan", text: "Replanning", seq: 1 },
+ { ts: 62, kind: "replan", text: "Replanning", seq: 2 }], 30);
+var out = {
+ straight: cmdArrow(C(0.45, 0), P, 0),
+ half: cmdArrow(C(0.225, 0), P, 0),
+ reverse: cmdArrow(C(-0.45, 0), P, 0),
+ north: cmdArrow(C(0.45, 0), { x: 1, y: 2, yaw: Math.PI / 2 }, 0),
+ arcLeft: cmdArrow(C(0.3, 0.3), O, 0),
+ arcRight: cmdArrow(C(0.3, -0.3), O, 0),
+ arcBack: cmdArrow(C(-0.3, 0.3), O, 0),
+ pivotLeft: cmdArrow(C(0, 0.385), P, 0),
+ pivotRight: cmdArrow(C(0.01, -0.5), P, 0),
+ pivotSlow: cmdArrow(C(0, 0.05), P, 0),
+ pivotFast: cmdArrow(C(0, 3), P, 0),
+ pivotTight: cmdArrow(C(0.03, 0.385), P, 0),
+ arcWide: cmdArrow(C(0.05, -0.3), P, 0),
+ zero: cmdArrow(C(0, 0), P, 0),
+ crawl: cmdArrow(C(0.01, 0.01), P, 0),
+ fresh: cmdArrow(C(0.45, 0, 1.9), P, 0.05) !== null,
+ staleAtTwo: cmdArrow(C(0.45, 0, 1.5), P, 0.5),
+ noAge: cmdArrow({ vx: 0.45, wz: 0 }, P, 0),
+ noPose: cmdArrow(C(0.45, 0), null, 0),
+ nan: cmdArrow(C(NaN, 0), P, 0),
+ drawStraight: drawn(cmdArrow(C(0.45, 0), P, 0)),
+ drawReverse: drawn(cmdArrow(C(-0.45, 0), P, 0)),
+ drawNorth: drawn(cmdArrow(C(0.45, 0), { x: 1, y: 2, yaw: Math.PI / 2 }, 0)),
+ drawPivotLeft: drawn(cmdArrow(C(0, 0.385), P, 0)),
+ drawPivotRight: drawn(cmdArrow(C(0, -0.385), P, 0)),
+ drawTiny: drawn(cmdArrow(C(0.03, 0), P, 0)),
+ drawPivotNorth: drawn(cmdArrow(C(0, 0.385), { x: 1, y: 2, yaw: Math.PI / 2 }, 0)),
+ fitSlow: fit.slow, fitFast: fit.fast, fitMetric: fit.metric, fitArc: fit.arc, fitBack: fit.back,
+ rowsB: rowsB,
+ nRowsB: rowsB.length,
+ kinds: ["contact", "push", "imu", "obstacle", "replan", "pivot_fallback", "goal", "new_goal", "reached",
+ "waiting", "goal_dropped", "retry", "mission", "foo_bar"].map(function (k) {
+ var r = decisionRows([{ ts: T0, kind: k, text: "t", seq: 1 }], 30)[0]; return [k, r.cls, r.label]; }),
+ junk: decisionRows([null, 5, "x", { kind: "goal", text: "Goal 1: 2.0 m away (35 cells)", ts: "1970-01-01T00:42:07Z" }], 30),
+ notList: [decisionRows(null, 30).length, decisionRows({}, 30).length],
+ noSeq: decisionRows([{ kind: "goal", text: "first" }, { kind: "obstacle", text: "second" }], 30).map(function (r) { return r.text; }),
+ times: [minSec("1970-01-01T00:42:07Z"), minSec("1970-01-01T00:42:07"), minSec(2527), minSec("12:34:56"), minSec(undefined)],
+ escaped: decisionsHtml(decisionRows([{ ts: T0, kind: "contact", text: "<img src=x onerror=alert(1)>", seq: 1 }], 30), null),
+ freshFirst: freshRows(rowsA, null).fresh,
+ freshEmptyBefore: freshRows(rowsA, []).fresh.filter(Boolean).length,
+ freshTwoNew: freshRows(rowsB, freshRows(rowsA, null).keys).fresh,
+ freshNewRun: freshRows(rowsC, freshRows(rowsA, null).keys).fresh,
+ freshDup: freshRows(dupB, freshRows(dupA, null).keys).fresh,
+ htmlFresh: decisionsHtml(rowsB.slice(0, 3), [true, false, false]),
+ words: ["OBSTACLE", "BACKING OFF", "STUCK", "HEADING", "IDLE", "READY", "WAITING", "DONE", "STOPPED", "heading", "FOO"]
+ .map(function (w) { return [w, nowView({ word: w, detail: "d", since: null }, 100).cls]; }),
+ nowObstacle: nowView({ word: "OBSTACLE", detail: "Obstacle ahead: stop", since: T0 - 2 }, T0),
+ nowIso: nowView({ word: "WAITING", detail: "Waiting: 3 of 5 frontiers are failed goals", since: "1970-01-01T00:00:10Z" }, 70),
+ nowLong: nowView({ word: "HEADING", detail: "Goal 4: 2.1 m away (12 cells)", since: 5 }, 100),
+ nowIdle: nowView({ word: "IDLE", detail: "", since: null }, 100),
+ nowNone: [nowView(null, 1), nowView({}, 1), nowView("x", 1)]
+};
+console.log(JSON.stringify(out));
+"""
+ r = subprocess.run([node, "-e", harness], capture_output=True, text=True, timeout=30, env={**os.environ, "TZ": "UTC"})
+ try:
+ J = json.loads(r.stdout)
+ except ValueError:
+ J = {}
+ print(" node said:", r.stderr.strip()[:600])
+
+ def near(a, b, tol=1e-6):
+ return isinstance(a, (int, float)) and abs(a - b) <= tol
+
+ def tip_at(key, x, y, ux, uy):
+ a = J.get(key) or {}
+ t = a.get("tip") or {}
+ return (a.get("kind") == "path" and near(t.get("x"), x) and near(t.get("y"), y)
+ and near(t.get("ux"), ux) and near(t.get("uy"), uy))
+
+ def length(pts):
+ return sum(math.dist(pts[i - 1], pts[i]) for i in range(1, len(pts)))
+
+ st = J.get("straight") or {}
+ check("arrow: 0.45 m/s straight from (1, 2) facing +x -> a 0.6 m path, (1, 2) -> (1.6, 2), pointing +x",
+ tip_at("straight", 1.6, 2.0, 1, 0) and st.get("pts", [[0, 0]])[0] == [1, 2]
+ and near(length(st.get("pts", [])), 0.6, 1e-9), json.dumps(st.get("tip")))
+ check("arrow: 0.225 m/s -> 0.3 m (length proportional to |vx|): tip (1.3, 2)",
+ tip_at("half", 1.3, 2.0, 1, 0), json.dumps((J.get("half") or {}).get("tip")))
+ check("arrow: reverse -0.45 m/s -> drawn behind the robot, tip (0.4, 2), pointing -x",
+ tip_at("reverse", 0.4, 2.0, -1, 0), json.dumps((J.get("reverse") or {}).get("tip")))
+ check("arrow: facing +y (yaw 90 deg), 0.45 m/s -> tip (1, 2.6), pointing +y",
+ tip_at("north", 1.0, 2.6, 0, 1), json.dumps((J.get("north") or {}).get("tip")))
+ th = 0.3 * 0.6 / 0.45 # 0.4 rad of turn over the arrow's 4/3 s
+ al = J.get("arcLeft") or {}
+ check("arrow: arc vx 0.3 m/s, wz +0.3 rad/s -> bends LEFT on a 1 m radius: tip (sin 0.4, 1 - cos 0.4) "
+ "= (0.3894, 0.0789), heading 0.4 rad, 0.4 m of path",
+ tip_at("arcLeft", math.sin(th), 1 - math.cos(th), math.cos(th), math.sin(th))
+ and near(length(al.get("pts", [])), 0.4, 1e-4), json.dumps(al.get("tip")))
+ check("arrow: wz -0.3 rad/s -> bends RIGHT: tip (0.3894, -0.0789)",
+ tip_at("arcRight", math.sin(th), -(1 - math.cos(th)), math.cos(th), -math.sin(th)),
+ json.dumps((J.get("arcRight") or {}).get("tip")))
+ check("arrow: reverse arc vx -0.3, wz +0.3 -> behind and to the right: tip (-0.3894, -0.0789), pointing back",
+ tip_at("arcBack", -math.sin(th), -(1 - math.cos(th)), -math.cos(th), -math.sin(th)),
+ json.dumps((J.get("arcBack") or {}).get("tip")))
+ pl, pr = J.get("pivotLeft") or {}, J.get("pivotRight") or {}
+ check("arrow: pivot wz +0.385 rad/s (vx 0) -> a left (counterclockwise) arc of 3 s of turn: 1.155 rad",
+ pl.get("kind") == "pivot" and pl.get("dir") == 1 and near(pl.get("sweep"), 1.155), json.dumps(pl))
+ check("arrow: vx 0.01 m/s (~0), wz -0.5 rad/s -> a right pivot of 1.5 rad",
+ pr.get("kind") == "pivot" and pr.get("dir") == -1 and near(pr.get("sweep"), 1.5), json.dumps(pr))
+ pt, aw = J.get("pivotTight") or {}, J.get("arcWide") or {}
+ check("arrow: a turn about a centre under the robot is a pivot: vx 0.03, wz +0.385 (radius 0.078 m) -> left "
+ "pivot; vx 0.05, wz -0.3 (radius 0.167 m) -> a path bending right",
+ pt.get("kind") == "pivot" and pt.get("dir") == 1 and aw.get("kind") == "path"
+ and (aw.get("tip") or {}).get("y", 2) < 2, json.dumps([pt, aw.get("tip")]))
+ check("arrow: a pivot sweeps at least 45 deg (wz 0.05) and at most 270 deg (wz 3)",
+ near((J.get("pivotSlow") or {}).get("sweep"), math.pi / 4)
+ and near((J.get("pivotFast") or {}).get("sweep"), 1.5 * math.pi))
+ check("arrow: nothing for a zero command (0, 0) or a crawl under the thresholds (0.01 m/s, 0.01 rad/s)",
+ "zero" in J and J["zero"] is None and "crawl" in J and J["crawl"] is None)
+ check("arrow: only fresher than LIVE_S = 2 s: 1.9 + 0.05 s drawn, 1.5 + 0.5 s not, no age not; "
+ "no pose / NaN -> nothing", J.get("fresh") is True and all(k in J and J[k] is None
+ for k in ("staleAtTwo", "noAge", "noPose", "nan")))
+
+ def ops_of(key, op):
+ return [o for o in J.get(key, []) if o[0] == op]
+
+ def head_tip(key):
+ """The arrowhead's tip: the moveTo that starts a closed path (the last one drawn, the mint fill)."""
+ ops, tips = J.get(key, []), []
+ for i, o in enumerate(ops):
+ if o[0] == "move" and i + 3 < len(ops) and ops[i + 3][0] == "close":
+ tips.append(o[1:3])
+ return tips[-1] if tips else None
+
+ def head_dir(key):
+ """The arrowhead's tip and its unit direction (tip minus the middle of its base, over its length)."""
+ ops = J.get(key, [])
+ for i in range(len(ops) - 4, -1, -1):
+ o = ops[i]
+ if o[0] == "move" and ops[i + 3][0] == "close":
+ b = [(ops[i + 1][1] + ops[i + 2][1]) / 2, (ops[i + 1][2] + ops[i + 2][2]) / 2]
+ d = math.dist(o[1:3], b) or 1
+ return o[1:3], [(o[1] - b[0]) / d, (o[2] - b[1]) / d]
+ return None, None
+
+ def shaft_of(key):
+ """The mint line's points (the last stroked move/line run before the mint stroke)."""
+ ops, run, last = J.get(key, []), [], []
+ for o in ops:
+ if o[0] == "begin":
+ run = []
+ elif o[0] in ("move", "line"):
+ run.append(o[1:3])
+ elif o[0] == "stroke" and o[1] == "#6CF8C7" and o[2] == 4:
+ last = run
+ return last
+
+ def at(pt, x, y, tol=1e-6):
+ return pt is not None and near(pt[0], x, tol) and near(pt[1], y, tol)
+
+ NOSE, TAIL = 15 * .62, 15 * .38 # px: the marker's nose ahead of its centre, its tail behind
+ ds = J.get("drawStraight", [])
+ mint = [o for o in ops_of("drawStraight", "stroke") if o[1] == "#6CF8C7"]
+ fills = ops_of("drawStraight", "fill")
+ sh = shaft_of("drawStraight")
+ check("canvas: 0.45 m/s at (1, 2), 100 px/m -> the line starts at the marker's nose (109.3, 100) px, "
+ "head tip 60 px further at (169.3, 100)",
+ sh and at(sh[0], 100 + NOSE, 100) and at(head_tip("drawStraight"), 160 + NOSE, 100),
+ json.dumps([sh[:1], head_tip("drawStraight")]))
+ strokes = ops_of("drawStraight", "stroke")
+ check("canvas: mint like the path but thicker (4 px > 2.25 px), inside an opaque navy outline (8 px, "
+ "--moss-navy) that sets it apart from the path; the head is filled mint, outlined navy",
+ mint and mint[-1][2] == 4 and fills and fills[-1][1] == "#6CF8C7"
+ and strokes[0][1] == "#080F1A" and strokes[0][2] == 8 and strokes[1][1] == "#080F1A", json.dumps(strokes))
+ shr = shaft_of("drawReverse")
+ check("canvas: reverse -> the line starts at the marker's tail (94.3, 100), head tip at (34.3, 100), behind",
+ shr and at(shr[0], 100 - TAIL, 100) and at(head_tip("drawReverse"), 40 - TAIL, 100),
+ json.dumps([shr[:1], head_tip("drawReverse")]))
+ check("canvas: facing +y -> head tip at (100, 30.7) px: up the screen, past the nose",
+ at(head_tip("drawNorth"), 100, 40 - NOSE), json.dumps(head_tip("drawNorth")))
+ arcs_l, arcs_r = ops_of("drawPivotLeft", "arc"), ops_of("drawPivotRight", "arc")
+ tl, tr = head_tip("drawPivotLeft"), head_tip("drawPivotRight")
+ ex = (100 + 17 * math.cos(-1.155), 100 + 17 * math.sin(-1.155))
+ check("canvas: left pivot -> an arc of 17 px around the robot (100, 100), from its nose (angle 0) "
+ "counterclockwise; head tip at the arc's end, up the screen (" + f"{ex[0]:.1f}, {ex[1]:.1f}" + ")",
+ arcs_l and arcs_l[-1][1:4] == [100, 100, 17] and near(arcs_l[-1][4], 0) and arcs_l[-1][6] is True
+ and arcs_l[-1][5] < 0 and tl is not None and near(tl[0], ex[0], 1e-6) and near(tl[1], ex[1], 1e-6),
+ json.dumps({"arc": arcs_l[-1] if arcs_l else None, "tip": tl}))
+ check("canvas: right pivot -> clockwise, head tip below the robot (y > 100)",
+ arcs_r and arcs_r[-1][6] is False and arcs_r[-1][5] > 0 and tr is not None and tr[1] > 100,
+ json.dumps({"arc": arcs_r[-1] if arcs_r else None, "tip": tr}))
+ an, tn = ops_of("drawPivotNorth", "arc"), head_tip("drawPivotNorth")
+ exn = (100 + 17 * math.cos(-math.pi / 2 - 1.155), 100 + 17 * math.sin(-math.pi / 2 - 1.155))
+ check("canvas: left pivot facing +y (yaw 90 deg) -> the arc starts at the nose, screen angle -90 deg (up), "
+ "and turns counterclockwise to the left: head tip (" + f"{exn[0]:.1f}, {exn[1]:.1f}" + ")",
+ an and near(an[-1][4], -math.pi / 2) and an[-1][6] is True and at(tn, exn[0], exn[1]),
+ json.dumps({"arc": an[-1] if an else None, "tip": tn}))
+ sht = shaft_of("drawTiny")
+ check("canvas: a 0.03 m/s command (4 px of map) -> never under 22 px past the nose: a line and the head, "
+ "tip (131.3, 100)",
+ at(head_tip("drawTiny"), 100 + NOSE + 22, 100) and sht and at(sht[0], 100 + NOSE, 100),
+ json.dumps([head_tip("drawTiny"), sht[:1]]))
+ # the fitted zoom of the demo (37.1 px/m): the robot at (1, 2) is at (37.1, 37.1) px
+ F0, nose_f = 37.1, 37.1 + NOSE
+ ss, tip_s = shaft_of("fitSlow"), head_tip("fitSlow")
+ check("canvas at the fitted zoom (37.1 px/m), 0.2 m/s (10 px of map, the run's slowest quarter): the line "
+ "leaves the nose, the head tip lies 22 px past it, >= 12 px of line before the head",
+ ss and at(ss[0], nose_f, F0) and at(tip_s, nose_f + 22, F0)
+ and (ss[-1][0] - ss[0][0]) - 3 >= 12 - 1e-6, json.dumps([ss[:1], ss[-1:], tip_s]))
+ check("canvas at 37.1 px/m: 0.45 m/s is 22.3 px of map, past the 22 px floor -> its metres again "
+ "(tip 22.26 px past the nose); at 50 px/m 0.45 m/s -> 30 px past the nose",
+ at(head_tip("fitFast"), nose_f + 0.6 * 37.1, F0) and at(head_tip("fitMetric"), 50 + NOSE + 30, 50),
+ json.dumps([head_tip("fitFast"), head_tip("fitMetric")]))
+ th2 = 0.2 * 0.6 / 0.45 # 0.267 rad of turn over the arrow
+ tip_a, dir_a = head_dir("fitArc")
+ exa = (nose_f + 22 * math.sin(th2) / th2, F0 - 22 * (1 - math.cos(th2)) / th2)
+ check("canvas at 37.1 px/m, arc vx 0.3 wz +0.2: a 22 px arc from the nose that turns 0.267 rad left: tip "
+ f"({exa[0]:.2f}, {exa[1]:.2f}), above the axis, the head pointing 0.267 rad up the screen",
+ at(tip_a, exa[0], exa[1], 1e-6) and dir_a is not None and near(dir_a[0], math.cos(th2), 1e-6)
+ and near(dir_a[1], -math.sin(th2), 1e-6), json.dumps([tip_a, dir_a]))
+ sb = shaft_of("fitBack")
+ check("canvas at 37.1 px/m, reverse 0.15 m/s: from the tail, head tip 22 px behind it",
+ sb and at(sb[0], F0 - TAIL, F0) and at(head_tip("fitBack"), F0 - TAIL - 22, F0),
+ json.dumps([sb[:1], head_tip("fitBack")]))
+
+ rb = J.get("rowsB") or []
+ t40 = datetime.fromtimestamp(T0 + 7 * 40, timezone.utc).strftime("%M:%S")
+ check(f"feed: 40 events -> 30 rows, newest on top (seq 40 ... 11), time mm:ss ({t40} for seq 40)",
+ J.get("nRowsB") == 30 and [r.get("seq") for r in rb] == list(range(40, 10, -1))
+ and rb[0].get("time") == t40 and rb[0].get("text") == "event 40",
+ json.dumps(rb[:2]))
+ got = {k: (c, lab) for k, c, lab in J.get("kinds", [])}
+ want = {k: (KINDS.get(k, ""), lab) for k, lab in
+ [("contact", "CONTACT"), ("push", "PUSH"), ("imu", "IMU"), ("obstacle", "OBSTACLE"),
+ ("replan", "REPLAN"), ("goal", "GOAL"), ("new_goal", "NEW GOAL"), ("reached", "REACHED"),
+ ("waiting", "WAITING"), ("goal_dropped", "DROPPED"), ("retry", "RETRY"), ("mission", "MISSION"),
+ ("foo_bar", "FOO BAR")]}
+ want["pivot_fallback"] = ("caution", "PIVOT")
+ check("feed pills: contact/push/imu warn, obstacle/replan amber (caution), goal/new_goal/reached accent, "
+ "waiting/goal_dropped/retry dim, mission plain; an unknown kind shows its name",
+ got == want, json.dumps({k: v for k, v in got.items() if want.get(k) != v}))
+ junk = J.get("junk") or []
+ check("feed: null, a number and a string are dropped; an ISO 'Z' time -> 42:07",
+ len(junk) == 1 and junk[0].get("time") == "42:07" and junk[0].get("cls") == "accent"
+ and J.get("notList") == [0, 0], json.dumps(junk))
+ check("feed: events without seq keep the server's order, newest (last) on top",
+ J.get("noSeq") == ["second", "first"], json.dumps(J.get("noSeq")))
+ check("feed times: ISO with and without zone (UTC here), epoch 2527 s, a bare hh:mm:ss, none -> "
+ "42:07, 42:07, 42:07, 34:56, --:--",
+ J.get("times") == ["42:07", "42:07", "42:07", "34:56", "--:--"], json.dumps(J.get("times")))
+ esc_html = J.get("escaped", "")
+ check("feed: an event's text is escaped (no markup from the log reaches the page)",
+ "&lt;img src=x onerror=alert(1)&gt;" in esc_html and "<img" not in esc_html
+ and esc_html.startswith('<li class="warn"><span class="t">'), esc_html[:120])
+ check("feed flash: the first answer flashes nothing; after an empty one, every row flashes",
+ J.get("freshFirst") == [False] * 30 and J.get("freshEmptyBefore") == 30,
+ json.dumps([J.get("freshFirst", [])[:3], J.get("freshEmptyBefore")]))
+ check("feed flash: two new events on top -> exactly those two rows flash",
+ J.get("freshTwoNew") == [True, True] + [False] * 28, json.dumps(J.get("freshTwoNew", [])[:4]))
+ check("feed flash: an answer with nothing in common (a new run) flashes nothing; "
+ "a second identical line in the same second flashes once",
+ J.get("freshNewRun") == [False] and J.get("freshDup") == [True, False],
+ json.dumps([J.get("freshNewRun"), J.get("freshDup")]))
+ hf = J.get("htmlFresh", "").split("</li>")
+ check("feed html: a fresh row carries its kind class and 'fresh' (goal: accent), the next one only its own (waiting: dim)",
+ len(hf) >= 3 and hf[0].startswith('<li class="accent fresh">') and hf[1].startswith('<li class="dim">'),
+ " | ".join(hf[:2])[:160])
+ words = {w: c for w, c in J.get("words", [])}
+ check("state word colour: OBSTACLE / BACKING OFF / STUCK warn, HEADING accent, IDLE / READY / WAITING / "
+ "DONE / STOPPED dim; 'heading' -> HEADING; an unknown word plain",
+ all(words.get(w) == c for w, c in WORDS.items()) and words.get("heading") == "accent" and words.get("FOO") == "",
+ json.dumps(words))
+ no = J.get("nowObstacle") or {}
+ check("state word: OBSTACLE set 2 s ago on the server's clock -> 'OBSTACLE', warn, its detail, 'for 2 s'",
+ no == {"word": "OBSTACLE", "cls": "warn", "detail": "Obstacle ahead: stop", "since": "for 2 s"}, json.dumps(no))
+ check("state word: since as ISO (10 s) at 70 s -> 'for 60 s'; since 5 s at 100 s -> 'for 2 min'",
+ (J.get("nowIso") or {}).get("since") == "for 60 s" and (J.get("nowLong") or {}).get("since") == "for 2 min",
+ json.dumps([J.get("nowIso"), J.get("nowLong")]))
+ check("state word: IDLE without detail -> 'No stack running.', no since; nothing -> '--' dim",
+ J.get("nowIdle") == {"word": "IDLE", "cls": "dim", "detail": "No stack running.", "since": ""}
+ and J.get("nowNone") == [{"word": "--", "cls": "dim", "detail": "", "since": ""}] * 3,
+ json.dumps([J.get("nowIdle"), J.get("nowNone")]))
+
+ # The panel itself: pollDecisions and renderDecisions, the page's own code, on a stub document.
+ # The server's clock (S.state.ts = T0) is 25 years behind this machine's: a word timed on the
+ # browser's clock instead would read in hours, not "for 2 s".
+ panel_js = "\n".join(defs[n] for n in ("pad2", "dur", "esc", "DECISION_ROWS", "KIND_CLASS", "KIND_LABEL",
+ "WORD_CLASS", "WORD_DETAIL", "tsSeconds", "minSec", "decisionRows",
+ "freshRows", "decisionsHtml", "nowView") + tuple(PANEL)) + r"""
+var T0 = """ + str(T0) + r""";
+var els = {};
+["now-word", "now-detail", "now-since", "decisions"].forEach(function (id) {
+ els[id] = { textContent: "", className: "", innerHTML: "" }; });
+var document = { getElementById: function (id) { return els[id] || null; } };
+var S = { state: { ts: T0, stack: { running: true } }, stateAt: 0, decisions: null, decHtml: null, decKeys: null };
+var answers = [], urls = [];
+function fetchJSON(url) {
+ urls.push(url); var a = answers.shift();
+ return a instanceof Error ? Promise.reject(a) : Promise.resolve(a);
+}
+function snap() {
+ return { word: els["now-word"].textContent, cls: els["now-word"].className, detail: els["now-detail"].textContent,
+ since: els["now-since"].textContent, feed: els["decisions"].innerHTML, keys: (S.decKeys || []).length };
+}
+function evs(n) {
+ var out = [];
+ for (var i = 1; i <= n; i++) out.push({ ts: T0 - 10 + i, kind: ["goal", "obstacle", "replan", "contact"][i % 4],
+ text: "decision " + i, src: "explorer", seq: i });
+ return out;
+}
+var docA = { now: { word: "OBSTACLE", detail: "Obstacle ahead: stop", since: T0 - 2 }, events: evs(3), run: "r1" };
+var docB = { now: { word: "HEADING", detail: "Goal 2: 1.5 m away (20 cells)", since: T0 - 65 }, events: evs(4), run: "r1" };
+var out = {};
+answers.push(docA);
+pollDecisions().then(function () {
+ out.polled = snap(); out.urls = urls.slice();
+ S.decisions = docB; renderDecisions(); out.next = snap();
+ S.decisions = { error: "decisions: main.jsonl unreadable" }; renderDecisions(); out.error = snap();
+ S.decisions = docB; renderDecisions(); out.after = snap();
+ answers.push(new Error("offline"));
+ return pollDecisions();
+}).then(function () {
+ out.offline = snap();
+ S.decisions = { __missing: true }; renderDecisions(); out.missing = snap();
+ S.state.stack.running = false; S.decKeys = null;
+ S.decisions = { now: { word: "IDLE", detail: "", since: null }, events: [] }; renderDecisions(); out.idle = snap();
+ console.log(JSON.stringify(out));
+}).catch(function (e) { console.log(JSON.stringify({ crash: String(e && e.stack || e) })); });
+"""
+ r = subprocess.run([node, "-e", panel_js], capture_output=True, text=True, timeout=30, env={**os.environ, "TZ": "UTC"})
+ try:
+ PJ = json.loads(r.stdout)
+ except ValueError:
+ PJ = {}
+ print(" node said:", r.stderr.strip()[:600])
+ check("the panel's code is found in app.js ($, setText, stack, sinceState, serverNow, pollDecisions, "
+ "renderDecisions)", all(defs[n] for n in PANEL), ", ".join(n for n in PANEL if not defs[n]))
+
+ def mmss(t):
+ return datetime.fromtimestamp(t, timezone.utc).strftime("%M:%S")
+
+ def first_li(feed):
+ return feed.split("</li>")[0] if feed else ""
+
+ pa, nx, er, af = (PJ.get(k) or {} for k in ("polled", "next", "error", "after"))
+ check("panel: pollDecisions asks /decisions.json?n=30 and draws the answer: OBSTACLE, class warn, its detail, "
+ "'for 2 s' on the server's clock",
+ PJ.get("urls") == ["/decisions.json?n=30"] and pa.get("word") == "OBSTACLE" and pa.get("cls") == "warn"
+ and pa.get("detail") == "Obstacle ahead: stop" and pa.get("since") == "for 2 s",
+ json.dumps({"urls": PJ.get("urls"), "polled": {k: pa.get(k) for k in ("word", "cls", "detail", "since")},
+ "crash": PJ.get("crash")})[:400])
+ check("panel: the first answer's feed has its 3 rows, newest on top (decision 3, contact: warn), nothing flashes",
+ pa.get("feed", "").count("<li") == 3 and first_li(pa.get("feed")).startswith(
+ '<li class="warn"><span class="t">' + mmss(1_000_000_000 - 7) + "</span>")
+ and "decision 3" in first_li(pa.get("feed")) and "fresh" not in pa.get("feed", ""), first_li(pa.get("feed"))[:160])
+ check("panel: the next answer (one more event) -> HEADING, accent, 'for 65 s'; the new row on top, and only it, "
+ "flashes",
+ nx.get("word") == "HEADING" and nx.get("cls") == "accent" and nx.get("since") == "for 65 s"
+ and first_li(nx.get("feed")).startswith('<li class="accent fresh">') and "decision 4" in first_li(nx.get("feed"))
+ and nx.get("feed", "").count("fresh") == 1, json.dumps({k: nx.get(k) for k in ("word", "cls", "since")})
+ + " " + first_li(nx.get("feed"))[:100])
+ check("panel: an error answer ({error}, no events) -> '--', 'Decision feed error: ...', and the feed and its "
+ "keys stay (the decisions still exist)",
+ er.get("word") == "--" and er.get("cls") == "dim"
+ and er.get("detail") == "Decision feed error: decisions: main.jsonl unreadable"
+ and er.get("feed") == nx.get("feed") and er.get("keys") == 4,
+ json.dumps({k: er.get(k) for k in ("word", "detail", "keys")}))
+ check("panel: the good answer after the error flashes nothing (the keys were kept), and a failed fetch keeps "
+ "the word, the detail and the feed",
+ af.get("feed") and "fresh" not in af.get("feed") and af.get("word") == "HEADING"
+ and PJ.get("offline") == af, json.dumps({"after": first_li(af.get("feed"))[:80], "same": PJ.get("offline") == af}))
+ mi, idl = PJ.get("missing") or {}, PJ.get("idle") or {}
+ check("panel: no endpoint (404) -> '--', 'no decision feed'; no stack and no event -> IDLE, 'No stack running.', "
+ "'No stack, no decisions.'",
+ mi.get("word") == "--" and "no decision feed" in mi.get("detail", "") and "No /decisions.json" in mi.get("feed", "")
+ and idl.get("word") == "IDLE" and idl.get("cls") == "dim" and idl.get("detail") == "No stack running."
+ and "No stack, no decisions." in idl.get("feed", ""), json.dumps([mi, idl])[:300])
+
+# ---------------------------------------------------------------------------
+print("E. the server's answer through the page")
+try:
+ import moss_cockpit as mc # noqa: E402 - standard library + numpy, no dimOS
+ server = mc.make_server("127.0.0.1", 0, demo=True, listen=False)
+ threading.Thread(target=server.serve_forever, daemon=True).start()
+ base = f"http://127.0.0.1:{server.server_address[1]}"
+ try:
+ with urllib.request.urlopen(base + "/decisions.json?n=30", timeout=10) as resp:
+ code, body = resp.status, resp.read()
+ except urllib.error.HTTPError as e:
+ code, body = e.code, e.read()
+except Exception as e: # noqa: BLE001 - the server module itself is broken: a failure, not a skip
+ code, body, server = None, repr(e).encode(), None
+ check("the cockpit server starts in demo mode", False, body.decode()[:200])
+if code == 404:
+ print(" -- skipped, this cockpit server has no /decisions.json yet (the page says so: 'no decision feed')")
+elif code is not None:
+ try:
+ d = json.loads(body)
+ except ValueError:
+ d = None
+ ok_shape = (isinstance(d, dict) and isinstance(d.get("now"), dict) and isinstance(d.get("events"), list)
+ and "run" in d and isinstance(d["now"].get("word"), str))
+ check("demo /decisions.json?n=30 -> 200 {now: {word, detail, since}, events: [...], run}",
+ code == 200 and ok_shape, body[:200].decode(errors="replace"))
+ if ok_shape and node is not None:
+ feed = "\n".join(defs[n] for n in ("pad2", "dur", "esc", "DECISION_ROWS", "KIND_CLASS", "KIND_LABEL",
+ "WORD_CLASS", "WORD_DETAIL", "tsSeconds", "minSec", "decisionRows",
+ "decisionsHtml", "nowView")) + "\nvar d = " + json.dumps(d) + r""";
+var rows = decisionRows(d.events, DECISION_ROWS);
+console.log(JSON.stringify({ n: rows.length, labels: rows.map(function (r) { return r.label; }),
+ unknown: d.events.filter(function (e) { return !(e.kind in KIND_LABEL); }).map(function (e) { return e.kind; }),
+ times: rows.map(function (r) { return r.time; }), word: nowView(d.now, Date.now() / 1000).word,
+ wordKnown: String(d.now.word).toUpperCase() in WORD_CLASS }));
+"""
+ r = subprocess.run([node, "-e", feed], capture_output=True, text=True, timeout=30)
+ try:
+ F = json.loads(r.stdout)
+ except ValueError:
+ F = {}
+ check(f"the page renders the demo answer: one row per event (<= 30), every kind and the word known "
+ f"({F.get('n')} rows, word {F.get('word')})",
+ F.get("n") == min(30, len(d["events"])) and not F.get("unknown") and F.get("wordKnown") is True
+ and "--:--" not in F.get("times", ["--:--"]) if d["events"] else F.get("wordKnown") is True,
+ json.dumps(F)[:300] + r.stderr[:200])
+ elif ok_shape:
+ print(" -- skipped, no node here: the demo answer through the page's functions")
+if server is not None:
+ server.shutdown()
+
+# The stack's own decision lines (one per kind, as a real exploration logs them) through the
+# server's parser and the page's functions: every kind the server makes has a pill, the word a colour.
+REAL = [("explorer2.py", "goal 1: (-1.27, 1.28) 2.0 m away, 35 frontier cells, 12 clusters, chosen in 114 ms"),
+ ("global_planner.py", "Got new goal"),
+ ("global_planner.py", "Found safe goal."),
+ ("local_planner.py", "Obstacle detected ahead, stopping local planner."),
+ ("global_planner.py", "Replanning path due to obstacle found."),
+ ("global_planner.py", "Robot is stuck. Replanning."),
+ ("global_planner.py", "Travelling to goal 1.2850097467377768m away from requested goal."),
+ ("moss_adapter.py", "MOSS base: tracks frozen at L=+56% R=+50% - pushing at 75 % for 1 s before calling it a contact"),
+ ("moss_adapter.py", "MOSS base: STALL #1: tracks commanded L=+76% R=+64% but the encoders moved L=+1 R=+0 ticks "
+ "in 1.0 s - stopped (the encoders are the bumper)"),
+ ("recovering_planner.py", "BUMP received"),
+ ("recovering_planner.py", "Contact escape"),
+ ("explorer2.py", "planner gave up on that goal: excluded until the map or the viewpoint changes"),
+ ("explorer2.py", "10 of 20 clusters are on a recently failed goal: waiting for the map or the viewpoint to change, "
+ "not stopping"),
+ ("explorer2.py", "starvation escape: the world stayed silent for 12 asks - reopening the oldest failed goal at "
+ "(-1.27, 1.28)"),
+ ("local_planner.py", "Reached goal position, starting final rotation"),
+ ("wavefront_frontier.py", "Stopped autonomous frontier exploration")]
+if not hasattr(mc if "mc" in globals() else None, "decisions_from_records"):
+ print(" -- skipped, this cockpit server has no decisions_from_records yet: the real lines through the page")
+elif node is None:
+ print(" -- skipped, no node here: the real lines through the page's functions")
+else:
+ recs = [{"ts": T0 + 5.0 * i, "level": "info", "event": e, "logger": lg} for i, (lg, e) in enumerate(REAL)]
+ evs = mc.decisions_from_records(recs)
+ doc = mc.decisions_doc(evs, 30, True, True, T0 + 5.0 * len(REAL), "bench-run") \
+ if hasattr(mc, "decisions_doc") else {"now": mc.now_state(evs, True, True, T0 + 5.0 * len(REAL)), "events": evs}
+ feed = "\n".join(defs[n] for n in ("pad2", "dur", "esc", "DECISION_ROWS", "KIND_CLASS", "KIND_LABEL",
+ "WORD_CLASS", "WORD_DETAIL", "tsSeconds", "minSec", "decisionRows",
+ "nowView")) + "\nvar d = " + json.dumps(doc) + r""";
+var rows = decisionRows(d.events, DECISION_ROWS);
+console.log(JSON.stringify({ n: rows.length, kinds: rows.map(function (r) { return r.label; }),
+ unknown: d.events.filter(function (e) { return !(e.kind in KIND_LABEL); }).map(function (e) { return e.kind; }),
+ newest: rows.length ? rows[0].text : null, word: nowView(d.now, 0).word,
+ wordKnown: String(d.now.word).toUpperCase() in WORD_CLASS }));
+"""
+ r = subprocess.run([node, "-e", feed], capture_output=True, text=True, timeout=30)
+ try:
+ F = json.loads(r.stdout)
+ except ValueError:
+ F = {}
+ kinds = {e.get("kind") for e in evs}
+ check(f"{len(REAL)} real decision lines -> the server's {len(evs)} events -> the page: every kind has its pill, "
+ f"newest on top, the word known ({F.get('word')})",
+ len(evs) >= 10 and {"goal", "obstacle", "contact", "reached"} <= kinds and F.get("n") == len(evs)
+ and not F.get("unknown") and F.get("newest") == evs[-1].get("text") and F.get("wordKnown") is True,
+ json.dumps({"kinds": sorted(k for k in kinds if k), "page": F})[:400] + r.stderr[:200])
+
+_TMPDIR.cleanup()
+print(f"{OK} OK, {KO} KO")
+print("TEST PASSED" if KO == 0 else "TEST FAILED")
+sys.exit(1 if KO else 0)
diff --git a/docs/cockpit.md b/docs/cockpit.md
index fcfc1da..1086d5b 100644
--- a/docs/cockpit.md
+++ b/docs/cockpit.md
@@ -242,7 +242,8 @@ temperatures, disk), the live map (occupancy grid + lidar scan + pose + path +
goal; drag to pan, wheel or pinch to zoom, tap to send a goal), the two cameras
under the map (nose = RealSense colour, wrist = the arm's camera; MJPEG, 5 fps
each, one "Pause video" for both), controls (launch map / explore, stop stack,
-start / stop exploration, record episode, STOP) and the log tail.
+start / stop exploration, record episode, STOP), what it decides (below) and
+the log tail.
STOP on the page is a soft stop: it publishes `stop_explore_cmd`,
`stop_movement` and a zero twist. The hard stops are the firmware deadman
@@ -260,8 +261,9 @@ always allowed. Commands that publish on the bus need the stack on LCM
`X-Requested-With: moss-cockpit` (the page does, a cross-site form cannot).
Endpoints, for anything else that wants the data: `/state.json`, `/map.png`,
-`/map_meta`, `/scan.json`, `/path.json`, `/log.json`, `/preflight.json`,
-`/launch.json`, `/camera.mjpg` (`?cam=nose`, the default, or `?cam=wrist`), and
+`/map_meta`, `/scan.json`, `/path.json`, `/log.json`, `/decisions.json` (below),
+`/preflight.json`, `/launch.json`, `/camera.mjpg` (`?cam=nose`, the default, or
+`?cam=wrist`), and `POST /cmd/{explore,goal,stop,episode,stack}`.
`/scan.json` splits the latest `lidar` cloud by height: `points` (and `n`) are the lidar returns, at
the profile's lidar height; `camera` (and `camera_n`) are the depth camera's obstacle points, at
@@ -275,10 +277,116 @@ the next revolution. The base panel shows a STALLED badge for at least 4 s whene
base adapter's stall guard fires (a commanded track whose encoder did not move for
1 s: the tracks stop and the planner backs off), and a `stalls` row counts them
over the run.
-`POST /cmd/{explore,goal,stop,episode,stack}`.
`tools/moss_cockpit.py --demo` serves synthetic data for working on the page
without a robot.
+## What it decides
+
+The map shows what the rover sees; the "What it decides." panel says what it
+made of it and what it did as a consequence. Nothing new runs on the robot for
+this: the explorer, the planners, the base adapter, the costmap and the contact
+sources already write each of their decisions into the run's log (`main.jsonl`
+in the run's log directory). The cockpit reads that file and turns every
+decision line into one short line, in the order the stack wrote them, and
+keeps one word on top for the state now.
+
+| Kind | Who says it | The line in the log | On the page |
+|---|---|---|---|
+| `goal` | explorer | `goal 3: (-1.67, -0.12) 3.9 m away (beyond the cap: ...), 110 frontier cells, ...` | Goal 3: 3.9 m away (110 cells, beyond the cap) |
+| `goal_dropped` | explorer | `planner gave up on that goal: ...`, `goal timeout after 30 s having closed 0.05 m of 2.40 m: ...` | Goal dropped: the planner gave up / 0.05 of 2.40 m closed in 30 s |
+| `goal_dropped` | global planner | `No path found to the goal.`, `No safe goal found near requested target.` | Goal dropped: no path to (1.03, -2.72) / no safe spot near it |
+| `goal_dropped` | recovering planner | `Goal abandoned, no recovery motion` (stuck) | Stuck: goal abandoned |
+| `waiting` | explorer | `10 of 20 clusters are on a recently failed goal: waiting ...` | Waiting: 10 of 20 frontiers are failed goals |
+| `retry` | explorer | `starvation escape: ... reopening the oldest failed goal at (-1.27, 1.28)` | Retrying an old goal at (-1.27, 1.28) |
+| `obstacle` | local planner | `Obstacle detected ahead, stopping local planner.` | Obstacle ahead: stop |
+| `obstacle` | explorer | `born cornered: 0.40 m2 of reachable floor and no frontier - one 0.20 m back-off ...` | Cornered (0.40 m2 of floor, no frontier): backing off 0.20 m |
+| `replan` | global planner | a reason, then `Replanning.` (see below) | Replanning / Stuck: replanning / Off track: replanning / Navigation failed: replanning |
+| `new_goal` | global planner | `Got new goal`, `Found safe goal.`, `Travelling to goal 1.29m away from requested goal.` | Planner: new goal / safe goal found / safe goal 1.29 m from the requested one |
+| `reached` | local planner | `Reached goal position, starting final rotation` | Goal reached |
+| `contact` | base adapter | `STALL #1: tracks commanded L=+76% R=+64% but the encoders moved L=+1 R=+0 ticks in 1.0 s` | Contact #1: both tracks blocked (commanded ..., encoders moved ...) |
+| `contact` | recovering planner | `BUMP received`, `Contact during an escape: stopping dead`, `Contact escape` | Bump: backing off / Contact during an escape: stop dead / Escape done: 0.20 m in 2.1 s, Escape timed out: only 0.05 m in 5.0 s, Escape aborted after 0.05 m |
+| `contact` | costmap | `costmap: contact at (-0.88, +0.73) heading +147 deg: 32 cells ahead marked as a wall` | Wall remembered ahead (32 cells) |
+| `push` | base adapter | `tracks frozen at L=+56% R=+50% - pushing at 75 % for 1 s ...` | Tracks frozen at L=+56% R=+50%: pushing at 75 % |
+| `pivot_fallback` | base adapter | `pivot L=+100% R=-100%: the right track did not turn ...` | Right track did not turn in reverse (...): pivoting on the left track from now on |
+| `imu` | IMU contact detector | `IMU contact: forward -4.8 m/s^2 (threshold 4.0) while commanded L=+70% R=+60%` | the line itself |
+| `mission` | explorer | `exploration start: ...`, the stop lines, `exploration complete: ...` | Exploration started (cap 3 m, radius 4 m) / stopped (stop button or pad) / complete |
+
+A contact names the track that did not move: a track counts as blocked when it
+was commanded and its encoder moved fewer than 20 ticks over the stall window
+(the base adapter's own rule). Some decisions read the record's fields as well
+as its text: how far an escape went and how long it took (`travelled_m`,
+`seconds`), whether it was cut short (`aborted`), whether a bump moved anything
+(`acted`), what made the planner give a goal up (`trigger`), the spot it could
+not reach (`x`, `y`). Every other line (twists, telemetry, the camera's obstacle
+counts, sounds, transport) is not a decision and does not appear.
+
+A replan is shown only when the planner really replans. The global planner
+first logs why it might (`Robot is stuck. Replanning.`, `Replanning path due to
+obstacle found.`, `Robot veered off track. Replanning.`, `Failure in
+navigation.`), then decides. dimOS logs `Replanning.` when it replans, and that
+line is what shows the reason. MOSS's planner logs `Goal abandoned, no recovery
+motion` when it gives the goal up, and that line is shown instead. Two cases
+drop the reason: `Replan requested without a goal; ignoring.`, and no answer
+within 0.5 s. The second happens when MOSS's planner ignores a stuck verdict
+while the rover turns in place, waits, or backs off from a contact. A turn in
+place is not being stuck. A stuck verdict the planner ignored is therefore not
+in the feed: the rover is still heading for its goal.
+
+A line that only repeats the decision just before it is left out (within one
+second, unless said otherwise). That covers:
+- the planner taking the explorer's goal;
+- the planner's second `No safe goal found.`;
+- the explorer's "planner gave up" right after the planner gave its own reason, so the reason stays;
+- the bump the explorer publishes when it backs out of a corner;
+- the second of the two stop lines a STOP produces;
+- the explorer's "stay here" goal and its arrival, within 0.2 s of its stop.
+
+A goal tapped after that is a new decision. A `BUMP received` during an escape
+moves nothing: the line before it says the rover stopped dead, and the bump
+gets no row. Identical lines one after the other, such as the explorer saying
+every second that it waits, make one row ending in a count such as `(x10)`, so
+a long wait never pushes the contacts out of the page's 30 rows.
+
+| Word | When |
+|---|---|
+| IDLE | no stack is running (the last run's lines stay below it until the next run) |
+| READY | a stack and no decision yet; a tapped goal reached, dropped, or an escape over while nothing explores |
+| HEADING | a goal chosen (by the explorer or tapped on the map), a goal reached while exploring, a replan that keeps the goal (off track, a navigation failure) |
+| OBSTACLE | the local planner stopped on an obstacle, or the planner replans around it: 3 s |
+| STUCK | the planner's stuck replan or its giving a goal up when stuck (3 s), the base pushing harder on frozen tracks (4 s) |
+| BACKING OFF | a contact (an encoder stall, the bump, the wall remembered, an IMU contact) or the explorer backing out of a corner: until the escape's end line, at most 6 s (the escape's 5 s timeout plus one second for a lost line) |
+| WAITING | the explorer waits (every frontier left is a failed goal), dropped a goal, has just started, or an escape is over |
+| STOPPED | exploration stopped (STOP, the pad, a stop command); an escape ending after the STOP leaves it |
+| DONE | exploration complete |
+
+OBSTACLE, STUCK and BACKING OFF are held for their time even when the next line
+comes a millisecond later, so they can be seen; while several run the strongest
+shows (BACKING OFF, then STUCK, then OBSTACLE). The end of an escape and the
+end of the mission end every hold. Outside a hold the word is what the last
+other line said. Under the word, the detail is the goal being driven to or the
+text of the line that set the word.
+
+`GET /decisions.json?n=40` serves it: `{"now": {"word", "detail", "since"},
+"events": [...], "run"}`, where each event is `{"ts", "kind", "text", "src",
+"seq"}`: `ts` the log's own timestamp (ISO, UTC, with a Z), `src` one of
+explorer, planner, base, costmap, recovery, imu and mission, `seq` one more per
+decision, never going back while the cockpit runs. The cockpit reads
+`main.jsonl` incrementally (its last MiB at first, then only what was
+appended), at most twice a second whoever asks, and keeps the last 500 rows.
+When the stack stops, its rows and its run name stay, under IDLE "No stack
+running (last run: ...)", as its map does; the next run's log starts a new
+feed. A `dimos run` with no registry entry (run in the foreground, or a
+registry swept by `dimos status`) has no log the cockpit can find: the word is
+READY, "Stack running, its log is not found", and the feed is empty. `--demo`
+plays a scripted cycle through the same parser: goal, obstacle, replan,
+contact, escape, goal dropped, next goal, arrival.
+
+The feed says what the stack decided, not everything the sensors saw: an
+obstacle the camera saw and the planner never acted on is on the map, in amber,
+not in the feed.
+
+Contacts from the RealSense's IMU, which do not depend on the tracks gripping: [imu.md](imu.md).
+
## The cockpit as a service
By hand, `tools/moss-fly.sh cockpit` starts the server and prints its address;
diff --git a/docs/imu.md b/docs/imu.md
new file mode 100644
index 0000000..4ad6100
--- /dev/null
+++ b/docs/imu.md
@@ -0,0 +1,213 @@
+# Contacts from the IMU
+
+MOSS knows it touched something in two ways. The base adapter watches the
+encoders: tracks commanded but not turning for a second is a stall, and the
+coordinator publishes it on `bump`. That misses the contacts that matter
+most: a track that slips keeps turning against the wall, an oblique contact
+leaves one track free, and the verdict comes a second or more after the
+impact. The chassis says it at once: hitting something while driving is a
+short, hard deceleration along the direction of travel. The RealSense on the
+nose carries an IMU, and `MOSS_IMU=1` turns its decelerations into contacts on
+the same `bump` stream.
+
+It is opt-in (default off) until a stationary launch on the robot shows
+colour, depth and the IMU alive together (below).
+
+## The IMU
+
+The D455F has a motion module inside the camera: a 3-axis accelerometer
+(`accel`) and a 3-axis gyroscope (`gyro`). Measured on the robot: accel at
+100, 200 or 400 Hz, gyro at 200 or 400 Hz, streaming alone and in ONE
+pipeline next to depth and colour without an error.
+
+Two facts decide how it must be opened:
+
+* A second process cannot touch the camera while the stack holds it
+ ("failed to set power state").
+* dimOS's `RealSenseCamera` with `enable_imu=True` opens a SECOND pipeline for
+ the motion module (`_start_imu`). On the rover that pipeline fails ("No
+ device connected") and takes colour and depth down with it. That is why
+ `MOSS_GYRO=1` alone stays an experiment.
+
+So `moss_dimos/realsense_imu.py` defines `MossRealSense`, the same module with
+the same outputs, which changes one method: `_start_imu` opens the motion
+module through the sensor API of the device the colour/depth pipeline already
+holds (`device.query_sensors()`, the sensor named "Motion Module"), accel at
+its highest rate and gyro at `imu_hz` (200 with `MOSS_IMU`), and hands the
+samples to dimOS's own handler, which publishes `imu`. No second pipeline, no
+second process. If the camera has no motion module, or the gyro does not offer
+the rate, or opening fails, the camera logs a warning and goes on with colour
+and depth only. `stop()` stops and closes the motion module before the
+pipeline.
+
+## The axes
+
+dimOS publishes the motion data as librealsense delivers it, without
+rotating it: one `Imu` per gyro sample, the accel interpolated linearly
+between the two accel samples around it, `frame_id`
+`camera_accel_optical_frame`, timestamps on the host clock (dimOS turns on the
+camera's global time). librealsense already aligns the motion module with the
+depth imager's optical frame:
+
+| Axis | Points | At rest, camera level |
+|---|---|---|
+| x | right | 0 |
+| y | down | -9.81 m/s^2 (measured on the robot) |
+| z | forward, out of the lens | 0 |
+
+An accelerometer reads the push that holds it up, so gravity shows as -9.81 on
+the axis that points down. The gyro uses the same axes: the rover's yaw rate
+is `-gyro.y` (lidar_odometry's `gyro_axis="-y"`).
+
+Forward is `+z`. The y reading is measured; the direction of z comes from
+librealsense's convention and is the one thing the probe checks on the robot
+before the default is trusted: run the probe (below) with the nose tilted down by hand,
+and the mean accel on z goes negative (about -1.7 m/s^2 at 10 degrees). If it
+goes positive, the camera is mounted the other way round: set
+`MOSS_BUMP_AXIS=-z`.
+
+## The rule
+
+`moss_dimos/bump_imu.py`, class `ImuContactDetector` (pure, benched with known
+traces), inside the `BumpImu` module:
+
+1. **Forward component.** The accel sample on the forward axis
+ (`MOSS_BUMP_AXIS`, default `+z`).
+2. **High-pass.** That value minus the mean of the samples of the last 0.5 s
+ before it. Gravity leaking onto z (a pitched camera, a slope) and a slow
+ tilt cancel; a hit does not. Nothing is judged before the window holds
+ 0.25 s of history.
+3. **Hard enough.** |high-pass| >= `MOSS_BUMP_ACCEL` (default 4.0 m/s^2).
+4. **While driving.** The tracks are commanded: the firmware's `target_pct`
+ from `base_telemetry`, not (0, 0), younger than 1 s (time since it arrived
+ plus the firmware's own age of that line). A rover kicked or lifted while
+ idle is not a contact.
+5. **Against the command.** Driving forward (no track backward): only a
+ deceleration, high-pass <= -threshold. Reversing (no track forward): only a
+ forward kick, high-pass >= +threshold. Pivoting (tracks of opposite signs):
+ either sign. Taking off is never a contact.
+6. **Once.** After a contact, nothing for `MOSS_BUMP_DEBOUNCE_S` (default
+ 0.7 s). The debounce also covers the planner's escape taking off in
+ reverse while the last telemetry line still says forward.
+
+On a contact `BumpImu` publishes `Bool(True)` on `bump` and logs one line:
+
+```
+IMU contact: forward -4.8 m/s^2 (threshold 4.0) while commanded L=+70% R=+60%
+```
+
+`bump` is the stream the coordinator's stall guard and the explorer's back-off
+already publish on, so the same three listeners react: the planner's escape
+(stop, 0.20 m in reverse, the goal abandoned; a second contact during an
+escape stops dead), the costmap's contact memory (the band ahead becomes a
+wall) and the sounds (`bump_surprise`, at the impact rather than a second
+later). The stall guard stays on: a contact with the tracks frozen can be
+reported twice, by the IMU at the impact and by the encoders a second later.
+
+The cockpit shows each contact in its decision feed
+([cockpit.md](cockpit.md#what-it-decides)). The module's other lines start
+with `MOSS IMU:` and so appear in `tools/moss-fly.sh log`: the knobs at
+start, the first sample (where gravity sits), then once a minute the sample
+rate, the mean accel and the contacts so far. The contact lines themselves
+are in `.venv/bin/dimos log` and in the cockpit.
+
+Why the threshold should hold against the rover's own stops: the launcher sets
+the firmware ramp to 5 ms per % (`MOSS_RAMP_MS="5 5"`), so a commanded stop
+from 70 % takes about 0.35 s, roughly 1 m/s^2 of deceleration at the rover's
+speeds. That figure comes from the ramp setting, not from a recording. With `MOSS_PULSE_MS` the ramp
+is 1 ms per % and every 100 ms slice starts and stops the tracks: record a
+signature before using the two together.
+
+Where it could fire wrongly: pivots, which accept either sign. The camera sits
+0.156 m ahead of the centre, so a pivot pulls it toward the back with
+yaw rate squared times 0.156 m: about 3 m/s^2 at the 254 deg/s measured for a
+full-power pivot. Held steady, the high-pass removes it; its onset and its end
+are what the signature must look at, with the gyro columns of the CSV next to
+`hp` (a figure from the geometry, not from a recording).
+
+## Knobs
+
+| Variable | Default | What |
+|---|---|---|
+| `MOSS_IMU` | off | `1`: MossRealSense with the IMU at 200 Hz and the BumpImu module. Needs the camera (`MOSS_CAMERA=0` refuses it with a warning). |
+| `MOSS_BUMP_ACCEL` | 4.0 | threshold in m/s^2 on the high-passed forward accel, above 0 and up to 100 |
+| `MOSS_BUMP_AXIS` | `+z` | the forward axis of the camera's frame, x, y or z with a sign |
+| `MOSS_BUMP_DEBOUNCE_S` | 0.7 | seconds of silence after a contact, 0 to 10 |
+| `MOSS_IMU_LOG` | unset | a CSV path: every sample is appended (below) |
+| `MOSS_GYRO` | off | the gyro prior of the lidar odometry; with `MOSS_IMU=1` it reads the in-process IMU |
+
+They are read when the blueprint is built (the build log says
+`MOSS blueprint: IMU contacts on bump, forward axis +z, threshold 4 m/s^2, ...`);
+a value that does not parse is refused with a warning and the default stays.
+
+## Bringing it up
+
+1. **The probe, stack down.** `tools/moss_imu_probe.py` opens ONE pipeline with
+ depth and colour at 640x480 30 fps, then the motion module of that same
+ device through the sensor API, exactly as `MossRealSense` does, and counts
+ frames for 4 s:
+
+ ```
+ $ tools/moss-fly.sh stop
+ $ export GLIBC_TUNABLES=glibc.rtld.optional_static_tls=4194304 # on the Jetson, as the launchers do
+ $ .venv/bin/python tools/moss_imu_probe.py
+ camera: Intel RealSense D455F, depth + colour 640x480 @ 30 fps on one pipeline
+ motion module open on the same device: accel 400 Hz, gyro 200 Hz
+ depth 120 frames ( 30.0 per s)
+ color 120 frames ( 30.0 per s)
+ accel 1600 frames ( 400.0 per s)
+ gyro 800 frames ( 200.0 per s)
+ last accel (+0.031, -9.794, +0.112) m/s^2
+ mean accel (+0.028, -9.801, +0.104) m/s^2 - gravity on -y (level camera, librealsense axes: -y)
+ video and motion together: YES
+ ```
+
+ (illustrative numbers). Exit 0 when depth, colour, accel and gyro all
+ counted frames, 1 otherwise with the error of the step that failed. It
+ refuses to start while a stack is running. `--seconds`, `--imu-hz` and
+ `--serial` (default `MOSS_RS_SERIAL`) change the run. Tilt the nose down
+ by hand during a run to check the sign of z (above).
+
+2. **A stationary launch.** `MOSS_IMU=1 tools/moss-fly.sh map`, the rover
+ still. `tools/moss-fly.sh log` must show `MOSS IMU: motion module open on
+ the camera's own device` and `MOSS IMU: first sample`, and the cockpit must
+ show colour and the depth obstacles as before. Until that launch has been
+ seen, `MOSS_IMU` stays off by default.
+
+3. **A signature before tightening the threshold.** Record what real contacts,
+ real stops and the real floor look like:
+
+ ```
+ $ MOSS_IMU=1 MOSS_IMU_LOG=~/imu_signature.csv tools/moss-fly.sh map
+ ```
+
+ The CSV gets one row per sample, flushed every 2 s:
+ `ts,ax,ay,az,gx,gy,gz,hp,cmd_l,cmd_r` (`hp` empty while the window warms
+ up, the command empty when there is none or it is stale). Drive into a
+ soft obstacle a few times, straight and oblique, stop hard a few times,
+ drive over the floor's joints. Then list the hardest moments:
+
+ ```
+ $ python3 - ~/imu_signature.csv <<'EOF'
+ import csv, sys
+ rows = [r for r in csv.DictReader(open(sys.argv[1])) if r["hp"]]
+ for r in sorted(rows, key=lambda r: abs(float(r["hp"])), reverse=True)[:30]:
+ print(r["ts"], r["hp"], r["cmd_l"], r["cmd_r"])
+ EOF
+ ```
+
+ The threshold belongs between the largest |hp| of the stops and the floor
+ and the smallest |hp| of the contacts. A write error is logged once and
+ never stops the stack.
+
+## Benches
+
+`benches/bench_bump_imu_cold.py`: the rule on synthetic 200 Hz traces with
+gravity on y (a 6 m/s^2 pulse of 60 ms while commanded is exactly one contact
+of about -6.0 m/s^2; no command, a wobble, the wrong sign, a tilt, a pitched
+camera are none; the debounce; the knobs), the module (telemetry then samples
+-> one `bump` and the log line), the CSV, and `MossRealSense` on a fake
+pyrealsense2 (the motion sensor found by name on the pipeline's own device,
+the profiles it opens, stop, every failure leaving the camera running).
+`benches/bench_moss_blueprints_cold.py` section J: the blueprint with and
+without `MOSS_IMU`, and where `bump`, `imu` and `base_telemetry` are wired.
diff --git a/moss_dimos/blueprints.py b/moss_dimos/blueprints.py
index c0ffd29..8b8d1ca 100644
--- a/moss_dimos/blueprints.py
+++ b/moss_dimos/blueprints.py
@@ -34,8 +34,18 @@ Switches (env, read when moss-nav / moss-explore is built; robot_profile.env_on
parses them: 0, false, no or off = off, any other value = on, unset = the default):
MOSS_CAMERA=0 no RealSense (default on: colour 640x480 @ 5 fps, JPEG on the bus, plus depth aligned
to colour at the same rate, turned into map obstacles by lidar_odometry)
- MOSS_GYRO=1 gyro prior for the lidar odometry from the RealSense's IMU (default OFF: the IMU
- pipeline fails to open on the rover and takes the camera down with it; an experiment)
+ MOSS_GYRO=1 gyro prior for the lidar odometry from the RealSense's IMU (default OFF: alone, it uses
+ dimOS's second IMU pipeline, which fails to open on the rover and takes the camera down
+ with it; an experiment. With MOSS_IMU=1 the gyro comes from the in-process motion module)
+ MOSS_IMU=1 contacts from the RealSense's IMU (default OFF: opt-in until a stationary launch on the
+ robot shows colour, depth and IMU alive together). The camera becomes MossRealSense
+ (moss_dimos/realsense_imu.py: the motion module opened in the camera's process on the
+ device its pipeline holds, accel + gyro at 200 Hz on `imu`) and BumpImu
+ (moss_dimos/bump_imu.py) publishes `bump` - the stream of the stall guard: the planner's
+ escape, the costmap's contact memory, the sounds - on a hard deceleration against the
+ track command. MOSS_BUMP_ACCEL (m/s^2, default 4.0), MOSS_BUMP_AXIS (default +z),
+ MOSS_BUMP_DEBOUNCE_S (default 0.7), MOSS_IMU_LOG=<csv> (every sample); docs/imu.md.
+ Needs the camera: with MOSS_CAMERA=0 it is refused with a warning
MOSS_DESKEW=1 kiss-icp straightens each lidar revolution by the motion prior (default OFF: on the
rover its first thread fails and the odometry stops; an experiment)
MOSS_SOUNDS=0 no sound module (default on when pw-play or paplay is on the PATH: the sound pack
@@ -258,6 +268,7 @@ def _coordinator_blueprint(base: HardwareComponent):
LIDAR_BAUDRATE = 460800
CAMERA_WIDTH, CAMERA_HEIGHT, CAMERA_FPS = 640, 480, 5
+IMU_HZ = 200 # MOSS_IMU: gyro rate, and so the rate of `imu` (the accel runs at its highest)
EXPLORER_MIN_FRONTIER_PERIMETER_M = 0.3 # doorway-sized frontiers stay visible
EXPLORER_GOAL_TIMEOUT_S = 45.0
# Where the explorer sends the rover, set for the room (docs/cockpit.md):
@@ -448,6 +459,12 @@ def _common_parts(ports: dict, profile: RobotProfile) -> list:
LidarOdometry.blueprint(use_gyro_prior=gyro, deskew=deskew),
MossCostMap.blueprint(),
]
+ # MOSS_IMU=1: the IMU is opened inside the camera's own process (MossRealSense) and BumpImu turns
+ # its decelerations into contacts on `bump`; the IMU lives on the RealSense, so no camera, no IMU
+ imu = robot_profile.env_on("MOSS_IMU", False)
+ if imu and not robot_profile.env_on("MOSS_CAMERA", True):
+ _logger.warning("MOSS blueprint: MOSS_IMU needs the RealSense and MOSS_CAMERA is off: no IMU contacts")
+ imu = False
if robot_profile.env_on("MOSS_CAMERA", True):
from dimos.hardware.sensors.camera.realsense.camera import RealSenseCamera
from dimos.msgs.geometry_msgs.Quaternion import Quaternion
@@ -464,7 +481,19 @@ def _common_parts(ports: dict, profile: RobotProfile) -> list:
serial = os.environ.get("MOSS_RS_SERIAL", "").strip()
if serial:
cam_kwargs["serial_number"] = serial
- parts.append(RealSenseCamera.blueprint(**cam_kwargs))
+ if imu:
+ from moss_dimos.realsense_imu import MossRealSense
+ cam_kwargs.update(enable_imu=True, imu_hz=IMU_HZ)
+ parts.append(MossRealSense.blueprint(**cam_kwargs))
+ else:
+ parts.append(RealSenseCamera.blueprint(**cam_kwargs))
+ if imu:
+ from moss_dimos.bump_imu import BumpImu, bump_settings
+ knobs = bump_settings()
+ _logger.info(f"MOSS blueprint: IMU contacts on bump, forward axis {knobs['axis']}, "
+ f"threshold {knobs['threshold']:g} m/s^2, debounce {knobs['debounce_s']:g} s, "
+ f"csv {knobs['log_path'] or 'off'}")
+ parts.append(BumpImu.blueprint(**knobs))
if robot_profile.env_on("MOSS_SOUNDS", True):
from moss_dimos.sounds import MossSounds, playback_backend
if playback_backend() is None:
diff --git a/moss_dimos/bump_imu.py b/moss_dimos/bump_imu.py
new file mode 100644
index 0000000..d464269
--- /dev/null
+++ b/moss_dimos/bump_imu.py
@@ -0,0 +1,419 @@
+"""BumpImu: contacts felt by the RealSense's accelerometer, published on `bump`.
+
+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 contact. 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 `bump`, the stream the coordinator's stall guard and
+the explorer's back-off already publish on: the planner's escape, the
+costmap's contact memory and the sounds react to it the same way.
+
+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; reversing: >= +threshold; pivoting, the
+ tracks of opposite signs: either sign);
+ * once per contact: nothing for MOSS_BUMP_DEBOUNCE_S (default 0.7 s) after one.
+
+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
+
+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%
+(the cockpit's decision feed matches its start, "IMU contact"). The other
+lines start with "MOSS IMU:": the knobs at start, the first sample, then one
+line a minute with the sample rate and where gravity sits.
+
+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 coordinator's and the explorer's bump
+
+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"
+HEARTBEAT_S = 60.0 # one "IMU alive" line per minute of samples
+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 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; last_hp is the
+ high-pass of the last sample (None while the window warms up)."""
+
+ 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) -> 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._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.contacts = 0
+
+ @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:
+ 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
+ if self.last_contact_ts is not None and 0.0 <= ts - self.last_contact_ts < self.debounce_s:
+ return False
+ self.last_contact_ts = ts
+ self.contacts += 1
+ self.last_contact = {"ts": ts, "hp": hp, "commanded": (int(commanded[0]), int(commanded[1]))}
+ return True
+
+
+def contact_line(hp: float, threshold: float, commanded: tuple[int, int]) -> 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}%")
+
+
+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`; see the module docstring."""
+
+ imu: In[Imu]
+ base_telemetry: In[String]
+ bump: 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._beat_ts: float | None = None
+ self._beat_samples = 0
+ self._beat_sum = [0.0, 0.0, 0.0]
+ self._error: str | None = None
+
+ @property
+ def detector(self) -> ImuContactDetector:
+ return self._detector
+
+ @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, forward axis {d.axis}, threshold {d.threshold:.1f} m/s^2, "
+ f"debounce {d.debounce_s:.1f} s, command younger than {d.command_max_age_s:.1f} s, "
+ f"csv {self._csv.path if self._csv else 'off'}")
+
+ @rpc
+ def stop(self) -> None:
+ 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()
+
+ 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))
+ commanded, age = self.command_now()
+ 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)
+ self._heartbeat(ts, accel, msg)
+ if contact:
+ self._publish_contact()
+ 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:
+ c = self._detector.last_contact or {}
+ logger.warning(contact_line(c.get("hp", 0.0), self._detector.threshold, c.get("commanded", (0, 0))))
+ if getattr(self.bump, "transport", None) is None:
+ return
+ try:
+ self.bump.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 bump ({type(exc).__name__}: {exc})")
+
+ def _heartbeat(self, ts: float, accel: tuple, msg: Imu) -> None:
+ """The first sample, then one line per HEARTBEAT_S of samples: the IMU
+ is alive, at what rate, and 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")
+ self._beat_ts = ts
+ return
+ self._beat_samples += 1
+ for i in range(3):
+ self._beat_sum[i] += accel[i]
+ if self._beat_ts is not None and ts - self._beat_ts >= HEARTBEAT_S:
+ span, n = ts - self._beat_ts, self._beat_samples
+ mean = [s / n for s in self._beat_sum]
+ logger.info(f"MOSS IMU: {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, "
+ f"{self._detector.contacts} contact(s) so far")
+ self._beat_ts, self._beat_samples, self._beat_sum = ts, 0, [0.0, 0.0, 0.0]
diff --git a/moss_dimos/realsense_imu.py b/moss_dimos/realsense_imu.py
new file mode 100644
index 0000000..3b083d3
--- /dev/null
+++ b/moss_dimos/realsense_imu.py
@@ -0,0 +1,150 @@
+"""MossRealSense: dimOS's RealSense module with its IMU opened in the camera's own process.
+
+dimOS's RealSenseCamera streams colour and depth on one pipeline, and with
+enable_imu=True it starts a SECOND pipeline for the motion module
+(_start_imu). On MOSS's D455F that second pipeline fails ("No device
+connected") and takes colour and depth down with it, while the same motion
+module streams fine next to colour and depth when it is opened from the
+process that already holds the device. A second process cannot touch the
+device at all while the stack holds it ("failed to set power state").
+
+So this subclass changes ONE thing: _start_imu opens the motion module
+through the sensor API of the device the running pipeline already holds
+(device.query_sensors(), the sensor named "Motion Module"), never through
+another pipeline. Accel at the highest rate the sensor offers, gyro at
+config.imu_hz, both handed to the parent's own frame handler
+(_on_motion_frame), which pairs them and publishes `imu` exactly as before.
+
+What the parent publishes on `imu` (docs/imu.md): one Imu per gyro sample,
+the accel interpolated to that instant, in librealsense's motion axes, which
+librealsense already aligns with the depth imager's optical frame: x right,
+y down, z forward out of the lens. Nothing is rotated on the way; frame_id is
+"<camera_name>_accel_optical_frame". At rest, level, the accel reads the
+reaction to gravity: about (0, -9.81, 0) m/s^2.
+
+A failure of the motion module never stops the camera: no motion module, a
+gyro rate the sensor does not offer, or an error while opening it is logged
+as a warning and colour and depth go on without `imu`.
+
+Same module, same Outs, same config: the blueprint swaps the class only
+when MOSS_IMU is on (moss_dimos/blueprints.py).
+"""
+from __future__ import annotations
+
+from typing import Any, Callable
+
+from dimos.core.core import rpc
+from dimos.hardware.sensors.camera.realsense.camera import RealSenseCamera
+from dimos.utils.logging_config import setup_logger
+
+logger = setup_logger()
+
+MOTION_SENSOR_MARK = "Motion" # the D4xx motion sensor is named "Motion Module"
+
+
+def find_motion_sensor(device: Any, rs: Any) -> Any | None:
+ """The device's motion sensor (its name contains "Motion"), or None."""
+ for sensor in device.query_sensors():
+ try:
+ name = str(sensor.get_info(rs.camera_info.name))
+ except Exception: # noqa: BLE001 - a sensor without a name is not the motion module
+ continue
+ if MOTION_SENSOR_MARK in name:
+ return sensor
+ return None
+
+
+def motion_profiles(sensor: Any, rs: Any, imu_hz: int) -> tuple[Any, Any]:
+ """(accel profile at the highest offered rate, gyro profile at imu_hz),
+ float xyz format, as motion stream profiles. Raises ValueError naming the
+ offered rates when the accel is missing or the gyro does not offer
+ imu_hz."""
+ accel: list[Any] = []
+ gyro: list[Any] = []
+ for profile in sensor.get_stream_profiles():
+ kind = profile.stream_type()
+ if kind not in (rs.stream.accel, rs.stream.gyro):
+ continue
+ if profile.format() != rs.format.motion_xyz32f:
+ continue
+ if hasattr(profile, "is_motion_stream_profile") and not profile.is_motion_stream_profile():
+ continue
+ motion = profile.as_motion_stream_profile() if hasattr(profile, "as_motion_stream_profile") else profile
+ (accel if kind == rs.stream.accel else gyro).append(motion)
+ if not accel:
+ raise ValueError("the motion module offers no accel stream (motion_xyz32f)")
+ gyro_rates = sorted({int(p.fps()) for p in gyro})
+ at_rate = [p for p in gyro if int(p.fps()) == int(imu_hz)]
+ if not at_rate:
+ raise ValueError(f"imu_hz={imu_hz} is not offered by the gyro; it offers {gyro_rates}")
+ return max(accel, key=lambda p: int(p.fps())), at_rate[0]
+
+
+def open_motion_sensor(device: Any, rs: Any, imu_hz: int,
+ callback: Callable[[Any], None]) -> tuple[Any, Any, Any] | None:
+ """Open and start the motion sensor of a device the caller already
+ streams from: accel + gyro profiles (motion_profiles), sensor.open, then
+ sensor.start(callback). Returns (sensor, accel profile, gyro profile), or
+ None when the device has no motion sensor. Raises on any other failure,
+ after closing a sensor it had opened."""
+ sensor = find_motion_sensor(device, rs)
+ if sensor is None:
+ return None
+ accel, gyro = motion_profiles(sensor, rs, imu_hz)
+ sensor.open([accel, gyro])
+ try:
+ sensor.start(callback)
+ except Exception:
+ try:
+ sensor.close()
+ except Exception: # noqa: BLE001 - the start error is the one to report
+ pass
+ raise
+ return sensor, accel, gyro
+
+
+class MossRealSense(RealSenseCamera):
+ """RealSenseCamera whose IMU rides the camera's own device handle (see the
+ module docstring). Everything but _start_imu and stop is the parent's."""
+
+ def __init__(self, *args: Any, **kwargs: Any) -> None:
+ super().__init__(*args, **kwargs)
+ self._imu_sensor: Any | None = None
+ self._imu_warned = False
+
+ def _start_imu(self) -> None:
+ """Open the motion module on the device the colour/depth pipeline holds.
+ Never raises: the camera keeps streaming without `imu` on a failure."""
+ import pyrealsense2 as rs
+
+ if self._profile is None:
+ return
+ try:
+ opened = open_motion_sensor(self._profile.get_device(), rs, int(self.config.imu_hz),
+ self._on_motion_frame)
+ except Exception as exc: # noqa: BLE001 - the IMU never takes colour and depth down
+ self._imu_sensor = None
+ logger.warning(f"MOSS IMU: the motion module did not start ({type(exc).__name__}: {exc}); "
+ "colour and depth go on, no imu")
+ return
+ if opened is None:
+ if not self._imu_warned:
+ self._imu_warned = True
+ logger.warning("MOSS IMU: this camera has no motion module; colour and depth go on, no imu")
+ return
+ sensor, accel, gyro = opened
+ self._imu_sensor = sensor
+ logger.info(f"MOSS IMU: motion module open on the camera's own device (no second pipeline): "
+ f"accel {int(accel.fps())} Hz, gyro {int(gyro.fps())} Hz -> imu in {self._imu_optical_frame} "
+ "(x right, y down, z forward)")
+
+ @rpc
+ def stop(self) -> None:
+ sensor, self._imu_sensor = getattr(self, "_imu_sensor", None), None
+ if sensor is not None:
+ for step in (sensor.stop, sensor.close):
+ try:
+ step()
+ except Exception: # noqa: BLE001 - already stopped or the device is gone
+ pass
+ super().stop()
diff --git a/tools/moss_arm_calibrate.py b/tools/moss_arm_calibrate.py
new file mode 100644
index 0000000..52dbb13
--- /dev/null
+++ b/tools/moss_arm_calibrate.py
@@ -0,0 +1,163 @@
+#!/usr/bin/env python3
+"""Calibrate the arm's Feetech servos (STS family, protocol 0, 1 Mbps), the SO-101 way.
+
+Two steps, each on the operator's word (the arm is moved by hand, torque off):
+ 1. --middle the arm is held in the middle of every joint's travel: the servo's
+ Homing_Offset is written so that this pose reads 2048 (the centre of
+ the 0..4095 turn), which puts the encoder's zero crossing OUT of the
+ useful range.
+ 2. --range every joint has just been moved through its whole travel: the lowest
+ and highest positions seen during --watch are written as the servo's
+ Min/Max_Position_Limit and saved with the offsets.
+ --watch N read positions for N seconds and print the min/max per joint (used by
+ --range; run it while the operator sweeps the joints).
+ --show read what the servos hold now (offset, limits, position).
+
+Registers (STS/SMS control table): Homing_Offset 31 (2 bytes, sign-magnitude, bit 11 =
+sign), Min_Position_Limit 9, Max_Position_Limit 11, Lock 55 (0 unlocks the EEPROM),
+Present_Position 56, Torque_Enable 40. The calibration is saved as JSON in
+config/arm_calibration.json: {joint: {id, homing_offset, range_min, range_max}}.
+
+ tools/moss_arm_calibrate.py --show
+ tools/moss_arm_calibrate.py --middle # arm held in the middle pose
+ tools/moss_arm_calibrate.py --range --watch 30 # then sweep every joint for 30 s
+"""
+import argparse, json, os, sys, time
+
+PORT = os.environ.get("MOSS_ARM_PORT", "/dev/serial/by-id/usb-1a86_USB_Single_Serial_5B3D041749-if00")
+BAUD = 1000000
+JOINTS = {"shoulder_pan": 1, "shoulder_lift": 2, "elbow_flex": 3, "wrist_flex": 4, "wrist_roll": 5, "gripper": 6}
+CENTRE = 2048
+OUT = os.path.join(os.path.dirname(os.path.dirname(os.path.abspath(__file__))), "config", "arm_calibration.json")
+
+
+def encode_sign_magnitude(value: int, sign_bit: int = 11) -> int:
+ """Feetech stores the homing offset as sign-magnitude: bit `sign_bit` set = negative."""
+ mag = abs(int(value))
+ if mag >= (1 << sign_bit):
+ raise ValueError(f"offset {value} does not fit in {sign_bit} bits")
+ return mag | (1 << sign_bit) if value < 0 else mag
+
+
+def decode_sign_magnitude(raw: int, sign_bit: int = 11) -> int:
+ mag = raw & ((1 << sign_bit) - 1)
+ return -mag if raw & (1 << sign_bit) else mag
+
+
+def open_bus():
+ import scservo_sdk as scs
+ ph = scs.PortHandler(PORT)
+ if not ph.openPort():
+ sys.exit(f"cannot open {PORT}")
+ ph.setBaudRate(BAUD)
+ return scs, ph, scs.PacketHandler(0)
+
+
+def present(scs, ph, pk):
+ out = {}
+ for name, sid in JOINTS.items():
+ if pk.ping(ph, sid)[1] == scs.COMM_SUCCESS:
+ out[name] = pk.read2ByteTxRx(ph, sid, 56)[0]
+ return out
+
+
+def write_eeprom(scs, ph, pk, sid, addr, value, width):
+ pk.write1ByteTxRx(ph, sid, 55, 0)
+ time.sleep(0.02)
+ if width == 1:
+ comm, err = pk.write1ByteTxRx(ph, sid, addr, value)
+ else:
+ comm, err = pk.write2ByteTxRx(ph, sid, addr, value)
+ time.sleep(0.02)
+ pk.write1ByteTxRx(ph, sid, 55, 1)
+ return comm == scs.COMM_SUCCESS and err == 0
+
+
+def load():
+ try:
+ with open(OUT) as fh:
+ return json.load(fh)
+ except (OSError, ValueError):
+ return {}
+
+
+def save(doc):
+ os.makedirs(os.path.dirname(OUT), exist_ok=True)
+ with open(OUT, "w") as fh:
+ json.dump(doc, fh, indent=2, sort_keys=True)
+ print(f"saved {OUT}")
+
+
+def main(argv=None) -> int:
+ ap = argparse.ArgumentParser(description=__doc__.splitlines()[0])
+ ap.add_argument("--show", action="store_true")
+ ap.add_argument("--middle", action="store_true", help="write homing offsets so the current pose reads 2048")
+ ap.add_argument("--range", action="store_true", help="write the min/max seen during --watch as position limits")
+ ap.add_argument("--watch", type=float, default=0.0, help="seconds to read positions and track min/max")
+ a = ap.parse_args(argv)
+ scs, ph, pk = open_bus()
+ pos = present(scs, ph, pk)
+ if not pos:
+ print("no servo answers on", PORT); return 1
+ print("answering:", {n: JOINTS[n] for n in pos})
+ if a.show or not (a.middle or a.range or a.watch):
+ for name in pos:
+ sid = JOINTS[name]
+ off = decode_sign_magnitude(pk.read2ByteTxRx(ph, sid, 31)[0])
+ lo = pk.read2ByteTxRx(ph, sid, 9)[0]; hi = pk.read2ByteTxRx(ph, sid, 11)[0]
+ print(f" {name:14s} id {sid} position {pos[name]:4d} homing_offset {off:+5d} limits {lo}..{hi}")
+ return 0
+ doc = load()
+ if a.middle:
+ for name, p in pos.items():
+ sid = JOINTS[name]
+ if name == "gripper":
+ continue
+ raw_now = pk.read2ByteTxRx(ph, sid, 31)[0]
+ old = decode_sign_magnitude(raw_now)
+ # the position the servo reports already includes its current offset: undo it
+ raw_pos = (p + old) % 4096
+ # offset such that raw_pos - offset == CENTRE, kept in (-2048, 2048)
+ off = raw_pos - CENTRE
+ if off > 2047: off -= 4096
+ if off < -2047: off += 4096
+ okw = write_eeprom(scs, ph, pk, sid, 31, encode_sign_magnitude(off), 2)
+ time.sleep(0.05)
+ after = pk.read2ByteTxRx(ph, sid, 56)[0]
+ doc.setdefault(name, {})
+ doc[name].update({"id": sid, "homing_offset": off})
+ print(f" {name:14s} id {sid}: was {p} (offset {old:+d}) -> offset {off:+d} written {'OK' if okw else 'FAILED'}, now reads {after}")
+ save(doc)
+ if a.watch > 0:
+ lo = dict(present(scs, ph, pk)); hi = dict(lo)
+ t0 = time.monotonic()
+ # the operator sweeps the joints by hand: no fixed window is imposed on them, the watch
+ # ends on their word (SIGINT / SIGTERM) or after --watch seconds, whichever comes first
+ import signal
+ stop = {"now": False}
+ for sig in (signal.SIGINT, signal.SIGTERM):
+ signal.signal(sig, lambda *_: stop.__setitem__("now", True))
+ last_print = 0.0
+ while not stop["now"] and time.monotonic() - t0 < a.watch:
+ for name, p in present(scs, ph, pk).items():
+ lo[name] = min(lo[name], p); hi[name] = max(hi[name], p)
+ if time.monotonic() - last_print > 2.0:
+ last_print = time.monotonic()
+ print("range so far:", {n: (lo[n], hi[n]) for n in lo}, flush=True)
+ time.sleep(0.05)
+ print("range seen:", {n: (lo[n], hi[n]) for n in lo}, flush=True)
+ if a.range:
+ for name in lo:
+ sid = JOINTS[name]
+ ok1 = write_eeprom(scs, ph, pk, sid, 9, lo[name], 2)
+ ok2 = write_eeprom(scs, ph, pk, sid, 11, hi[name], 2)
+ doc.setdefault(name, {})
+ doc[name].update({"id": sid, "range_min": lo[name], "range_max": hi[name]})
+ print(f" {name:14s} id {sid}: limits {lo[name]}..{hi[name]} written {'OK' if ok1 and ok2 else 'FAILED'}")
+ save(doc)
+ ph.closePort()
+ return 0
+
+
+if __name__ == "__main__":
+ sys.exit(main())
diff --git a/tools/moss_cockpit.py b/tools/moss_cockpit.py
index 4dc5e57..4915a03 100644
--- a/tools/moss_cockpit.py
+++ b/tools/moss_cockpit.py
@@ -32,6 +32,11 @@ Endpoints (all Cache-Control: no-store; a failing request never kills the server
polls the scan at 3 Hz, so its robot marker moves at 3 Hz)
GET /path.json planner path
GET /log.json?n=40 tail of the running stack's main.jsonl
+ GET /decisions.json?n=40 what the stack decided, read from the same main.jsonl: the last n
+ decision events (goals, obstacles, replans, contacts, waits, the
+ mission) as short English lines, and the state word now
+ (HEADING, OBSTACLE, BACKING OFF...), at most 2 Hz of file reads;
+ after a stop, the last run's events until the next run
GET /preflight.json the last saved flight check
GET /launch.json tail of the last launcher output
GET /camera.mjpg nose camera (RealSense colour), multipart JPEG, at most 5 fps
@@ -708,6 +713,544 @@ def log_tail(log_dir: str | None, n: int) -> list[dict]:
return out[::-1]
+# --- what it decides: the stack's own log lines, read as decisions --------------------
+#
+# The explorer, the planners, the base adapter, the costmap and the contact sources
+# all log through dimOS's logger into the run's main.jsonl. Each line below is one
+# of their decisions, matched on its real text; every other line (twists,
+# telemetry, camera counts, transport, workers) is ignored. The page shows the
+# result as a feed and one state word.
+
+DECISIONS_MIN_INTERVAL_S = 0.5 # main.jsonl read for decisions at most 2 Hz, whoever asks
+DECISIONS_TAIL_BYTES = 1 << 20 # a first read takes the last MiB: thousands of records, past the twists
+DECISIONS_KEEP = 500 # decision rows kept per run (the page shows 30)
+DECISION_ECHO_S = 1.0 # a generic line this soon after the specific one it repeats is not a new decision
+AFTER_STOP_ECHO_S = 0.2 # the explorer's stay-here goal and its arrival land within ~60 ms of its stop
+REASON_CONFIRM_S = 0.5 # a replan reason with no consequence logged this soon was not acted on
+OBSTACLE_HOLD_S = 3.0 # OBSTACLE (a stop on an obstacle, a replan) and the planner's STUCK stay up this long
+CONTACT_HOLD_S = 4.0 # STUCK for frozen tracks (the base pushing harder) stays up this long
+ESCAPE_TIMEOUT_S = 5.0 # recovering_planner's: an escape that never covers its distance ends after this
+ESCAPE_DISTANCE_M = 0.20 # recovering_planner's: an escape that ended short of this timed out
+ESCAPE_HOLD_S = ESCAPE_TIMEOUT_S + 1.0 # BACKING OFF lasts until the escape's end line, at most this (a lost line)
+STALL_MIN_TICKS = 20 # moss_adapter's: fewer ticks over the stall window = that track did not move
+DECISION_WORDS = ("IDLE", "READY", "HEADING", "OBSTACLE", "BACKING OFF", "STUCK", "WAITING", "DONE", "STOPPED")
+HOLD_RANK = {"OBSTACLE": 1, "STUCK": 2, "BACKING OFF": 3} # several holds running: the highest rank shows
+RECORD_FIELDS = ("acted", "aborted", "travelled_m", "seconds", "trigger", "x", "y") # what a decision reads
+ # beyond the event text
+
+_N = r"([-+]?(?:\d+(?:\.\d+)?|nan|inf))" # a number as the line prints it, kept as printed
+_P = r"([-+]?\d+)" # a signed track percent or tick count
+
+TXT_ESCAPE = "Escape " # every end of an escape: done, timed out, aborted
+TXT_ESCAPE_DONE = "Escape done"
+TXT_STARTED = "Exploration started"
+TXT_STOPPED = "Exploration stopped"
+TXT_COMPLETE = "Exploration complete"
+TXT_REPLAN = "Replanning"
+TXT_STUCK = "Stuck: replanning"
+TXT_OFF_TRACK = "Off track: replanning"
+TXT_NAV_FAILED = "Navigation failed: replanning"
+QUIET_REPLANS = (TXT_OFF_TRACK, TXT_NAV_FAILED) # the rover keeps heading for its goal: no hold
+TXT_ABANDONED = "Stuck: goal abandoned"
+TXT_NO_PATH = "Goal dropped: no path"
+TXT_NO_SAFE = "Goal dropped: no safe spot near it"
+TXT_PLANNER_DROPS = "Goal dropped: no " # the planner's own reasons for dropping a goal
+TXT_CORNERED = "Cornered"
+TXT_PLANNER_ECHOES = ("Planner: new goal", "Planner: safe goal found") # the detail keeps the explorer's goal line
+TXT_NO_LOG = "Stack running, its log is not found"
+_FOLD = re.compile(r" \(x\d+\)$") # the count a folded run of identical rows carries
+
+
+def _num(rec, key) -> float | None:
+ """A record field as a finite number, else None."""
+ value = rec.get(key) if isinstance(rec, dict) else None
+ if isinstance(value, bool) or not isinstance(value, (int, float)) or not math.isfinite(value):
+ return None
+ return float(value)
+
+
+def _stall_text(m, rec=None) -> str:
+ n, cmd_l, cmd_r, enc_l, enc_r, secs = m.groups()
+ blocked = [side for side, cmd, enc in (("left", cmd_l, enc_l), ("right", cmd_r, enc_r))
+ if int(cmd) != 0 and abs(int(enc)) < STALL_MIN_TICKS]
+ which = "both tracks" if len(blocked) == 2 else f"{blocked[0]} track" if blocked else "tracks"
+ return (f"Contact #{n}: {which} blocked (commanded L={cmd_l}% R={cmd_r}%, "
+ f"encoders moved L={enc_l} R={enc_r} ticks in {secs} s)")
+
+
+def _pivot_text(m, rec=None) -> str:
+ cmd_l, cmd_r, side = m.groups()
+ cmd = cmd_r if side == "right" else cmd_l
+ other = "left" if side == "right" else "right"
+ way = "reverse" if int(cmd) < 0 else "forward"
+ return (f"{side.capitalize()} track did not turn in {way} (L={cmd_l}% R={cmd_r}%): "
+ f"pivoting on the {other} track from now on")
+
+
+def _bump_text(m, rec) -> str | None:
+ """'BUMP received' with acted=False is a contact during an escape: the line before it
+ ('Contact during an escape: stopping dead') already says so, and nothing backs off."""
+ return None if rec.get("acted") is False else "Bump: backing off"
+
+
+def _escape_text(m, rec) -> str:
+ """The end of an escape, from the record's fields (aborted, travelled_m, seconds)."""
+ travelled, seconds = _num(rec, "travelled_m"), _num(rec, "seconds")
+ took = f" in {seconds:.1f} s" if seconds is not None else ""
+ if rec.get("aborted") is True:
+ return "Escape aborted" + (f" after {travelled:.2f} m" if travelled is not None else "")
+ if travelled is None:
+ return TXT_ESCAPE_DONE
+ if travelled < ESCAPE_DISTANCE_M - 0.005:
+ return f"Escape timed out: only {travelled:.2f} m{took}"
+ return f"{TXT_ESCAPE_DONE}: {travelled:.2f} m{took}"
+
+
+def _abandon_text(m, rec) -> str:
+ trigger = rec.get("trigger")
+ if trigger is None or trigger == "stuck_detector":
+ return TXT_ABANDONED
+ return f"Goal abandoned ({str(trigger)[:40]})"
+
+
+def _no_path_text(m, rec) -> str:
+ x, y = _num(rec, "x"), _num(rec, "y")
+ return TXT_NO_PATH + (f" to ({x:.2f}, {y:.2f})" if x is not None and y is not None else "")
+
+
+def _metres(text: str) -> str:
+ try:
+ return f"{float(text):.2f}"
+ except ValueError:
+ return text
+
+
+# (kind, src, pattern on the event text, the short English text or a function of the
+# match and the record (None: no event), echo, role).
+#
+# `echo` lists (kind, text prefix or None[, window s]) entries: when the previous
+# decision is one of them and at most the window (DECISION_ECHO_S by default) older,
+# this line only repeats it and is not a new event (the planner taking the
+# explorer's goal, the second of the two stop lines a STOP produces, the explorer's
+# give-up after the planner's own reason, the explorer's "stay here" goal right
+# after it stops).
+#
+# `role` ties the global planner's replan lines together. Its monitor and its stop
+# handler log a REASON ("Robot is stuck. Replanning.", "Replanning path due to
+# obstacle found.", "Robot veered off track. Replanning.", "Failure in
+# navigation.") and then call _replan_path, which decides: "Replanning." when it
+# really replans (logged by dimOS's _replan_path), "Goal abandoned, no recovery
+# motion" when MOSS's planner gives the goal up, "Replan requested without a goal;
+# ignoring.", or nothing at all (MOSS's planner ignores a stuck verdict while the
+# follower turns in place, is idle or a contact escape owns the wheels). So a
+# reason is held, not shown; the "Replanning." that follows within
+# REASON_CONFIRM_S ("confirm") shows it, an abandon ("resolve") replaces it, and
+# anything else ("clear", or nothing in time) drops it: the planner did not act on it.
+_AFTER_STOP = ("mission", TXT_STOPPED)
+_AFTER_STOP_GOAL = ("mission", TXT_STOPPED, AFTER_STOP_ECHO_S)
+DECISION_CATALOGUE = (
+ ("goal", "explorer",
+ r"goal (\d+): \(" + _N + r", " + _N + r"\) " + _N + r" m away( \(beyond the cap[^)]*\))?, (\d+) frontier cells",
+ lambda m, r: f"Goal {m[1]}: {m[4]} m away ({m[6]} cells{', beyond the cap' if m[5] else ''})", None, None),
+ ("goal_dropped", "explorer", r"planner gave up on that goal\b", "Goal dropped: the planner gave up",
+ (("goal_dropped", TXT_PLANNER_DROPS), ("goal_dropped", TXT_ABANDONED)), None),
+ ("goal_dropped", "explorer", r"goal timeout after " + _N + r" s having closed " + _N + r" m of " + _N + r" m",
+ lambda m, r: f"Goal dropped: {m[2]} of {m[3]} m closed in {m[1]} s", None, None),
+ ("waiting", "explorer", r"(\d+) of (\d+) clusters are on a recently failed goal",
+ lambda m, r: f"Waiting: {m[1]} of {m[2]} frontiers are failed goals", None, None),
+ ("retry", "explorer", r"starvation escape:.*reopening the oldest failed goal at \(" + _N + r", " + _N + r"\)",
+ lambda m, r: f"Retrying an old goal at ({m[1]}, {m[2]})", None, None),
+ ("obstacle", "explorer",
+ r"born cornered: " + _N + r" m2 of reachable floor and no frontier - one " + _N + r" m back-off",
+ lambda m, r: f"{TXT_CORNERED} ({m[1]} m2 of floor, no frontier): backing off {m[2]} m", None, None),
+ ("obstacle", "planner", r"Obstacle detected ahead, stopping local planner", "Obstacle ahead: stop", None, None),
+ ("replan", "planner", r"Replanning path due to obstacle found", TXT_REPLAN, None, "reason"),
+ ("replan", "planner", r"Robot is stuck\. Replanning", TXT_STUCK, None, "reason"),
+ ("replan", "planner", r"Robot veered off track\. Replanning", TXT_OFF_TRACK, None, "reason"),
+ ("replan", "planner", r"Failure in navigation\b", TXT_NAV_FAILED, None, "reason"),
+ ("replan", "planner", r"Replanning\.?$", TXT_REPLAN, (("obstacle", None), ("replan", None)), "confirm"),
+ (None, None, r"Replan requested without a goal", None, None, "clear"),
+ ("goal_dropped", "recovery", r"Goal abandoned, no recovery motion", _abandon_text, None, "resolve"),
+ ("goal_dropped", "planner", r"No path found to the goal\b", _no_path_text, None, None),
+ ("goal_dropped", "planner", r"No safe goal found\b", TXT_NO_SAFE, (("goal_dropped", TXT_NO_SAFE),), None),
+ ("new_goal", "planner", r"Got new goal\b", TXT_PLANNER_ECHOES[0],
+ (("goal", None), ("retry", None), _AFTER_STOP_GOAL), None),
+ ("new_goal", "planner", r"Found safe goal\b", TXT_PLANNER_ECHOES[1],
+ (("goal", None), ("new_goal", None), ("obstacle", None), ("replan", None), _AFTER_STOP_GOAL), None),
+ ("new_goal", "planner", r"Travelling to goal " + _N + r" ?m away from requested goal",
+ lambda m, r: f"Planner: safe goal {_metres(m[1])} m from the requested one", None, None),
+ ("reached", "planner", r"Reached goal position\b", "Goal reached", (_AFTER_STOP_GOAL,), None),
+ ("reached", "planner", r"Close enough to goal\. Accepting as arrived", "Goal reached (close enough)",
+ (_AFTER_STOP_GOAL,), None),
+ ("contact", "base",
+ r"(?:MOSS base: )?STALL #(\d+): tracks commanded L=" + _P + r"% R=" + _P + r"% but the encoders moved L="
+ + _P + r" R=" + _P + r" ticks in " + _N + r" s", _stall_text, None, None),
+ ("contact", "recovery", r"BUMP received\b", _bump_text, (("obstacle", TXT_CORNERED),), None),
+ ("contact", "recovery", r"Contact during an escape: stopping dead", "Contact during an escape: stop dead",
+ None, None),
+ ("contact", "recovery", r"Contact escape$", _escape_text, None, None),
+ ("contact", "costmap",
+ r"costmap: contact at \(" + _N + r", " + _N + r"\) heading " + _N + r" deg: (\d+) cells ahead marked as a wall",
+ lambda m, r: f"Wall remembered ahead ({m[4]} cells)", None, None),
+ ("push", "base", r"(?:MOSS base: )?tracks frozen at L=" + _P + r"% R=" + _P + r"% - pushing at (\d+) %",
+ lambda m, r: f"Tracks frozen at L={m[1]}% R={m[2]}%: pushing at {m[3]} %", None, None),
+ ("pivot_fallback", "base",
+ r"(?:MOSS base: )?pivot L=" + _P + r"% R=" + _P + r"%: the (right|left) track did not turn", _pivot_text,
+ None, None),
+ ("imu", "imu", r"IMU contact\b.*", lambda m, r: m[0][:120], None, None),
+ ("mission", "mission", r"exploration start: .*?cap " + _N + r" m, radius " + _N + r" m",
+ lambda m, r: f"{TXT_STARTED} (cap {m[1]} m, radius {m[2]} m)", None, None),
+ ("mission", "mission", r"exploration start\b", TXT_STARTED, None, None),
+ ("mission", "mission", r"(?:\w+: )?stop_movement received, stopping exploration",
+ f"{TXT_STOPPED} (stop button or pad)", (_AFTER_STOP,), None),
+ ("mission", "mission", r"Received exploration stop command\b", f"{TXT_STOPPED} (stop command)",
+ (_AFTER_STOP,), None),
+ ("mission", "mission", r"Stopped autonomous frontier exploration", TXT_STOPPED, (_AFTER_STOP,), None),
+ ("mission", "mission", r"exploration complete: no reachable frontier left.*?\((\d+) targets",
+ lambda m, r: f"{TXT_COMPLETE}: no reachable frontier left ({m[1]} goals)", None, None),
+ ("mission", "mission", r"[Ee]xploration complete after (\d+) goals",
+ lambda m, r: f"{TXT_COMPLETE} ({m[1]} goals)", None, None),
+ ("mission", "mission", r"exploration complete\b", TXT_COMPLETE, None, None),
+)
+_CATALOGUE = tuple((kind, src, re.compile(pattern), text, echo, role)
+ for kind, src, pattern, text, echo, role in DECISION_CATALOGUE)
+
+
+def _epoch(ts) -> float | None:
+ """A record's time as epoch seconds: a number as is, an ISO string (dimOS writes
+ UTC with a Z) through fromisoformat; anything else None."""
+ if isinstance(ts, bool):
+ return None
+ if isinstance(ts, (int, float)):
+ return float(ts) if math.isfinite(ts) else None
+ if isinstance(ts, str):
+ t = _parse_iso(ts)
+ if t is None:
+ try:
+ t = float(ts)
+ except ValueError:
+ return None
+ return t if math.isfinite(t) else None
+ return None
+
+
+def _record_ts(rec: dict):
+ """The record's own time as given (ISO string or epoch), None when it is not a time."""
+ ts = rec.get("ts", rec.get("timestamp"))
+ if (isinstance(ts, bool) or not isinstance(ts, (int, float, str))
+ or (isinstance(ts, float) and not math.isfinite(ts))):
+ return None
+ return ts
+
+
+def decide(record) -> tuple | None:
+ """(kind, src, text, echo, role) for one log record whose event is a decision line,
+ else None. A "clear" line has no kind, src or text."""
+ if not isinstance(record, dict):
+ return None
+ event = record.get("event")
+ if not isinstance(event, str):
+ return None
+ event = event.strip()
+ for kind, src, rx, text, echo, role in _CATALOGUE:
+ m = rx.match(event)
+ if m:
+ said = text(m, record) if callable(text) else text
+ if said is None and role != "clear":
+ return None
+ return kind, src, said, echo, role
+ return None
+
+
+def _is_echo(echo, last: dict | None, ts) -> bool:
+ if not echo or last is None:
+ return False
+ a, b = _epoch(last.get("ts")), _epoch(ts)
+ if a is None or b is None:
+ return False
+ text = str(last.get("text", ""))
+ for item in echo:
+ kind, prefix = item[0], item[1]
+ window = item[2] if len(item) > 2 else DECISION_ECHO_S
+ if last.get("kind") == kind and (not prefix or text.startswith(prefix)) and abs(b - a) <= window:
+ return True
+ return False
+
+
+def decisions_from_records(records, seq0: int = 0, prev: dict | None = None, carry: dict | None = None) -> list[dict]:
+ """Log records ({ts, level, event, logger} plus any RECORD_FIELDS, oldest first, as
+ log_tail returns them) -> decision events {ts, kind, text, src, seq}, in the same
+ order. `ts` is the record's own (an ISO string or epoch seconds); `seq` counts from
+ seq0 + 1; `prev` is the last event already emitted (for the echo rule across reads);
+ `carry` is a dict the caller keeps between reads: a replan reason still waiting for
+ its consequence when the records ran out stays in it (read and updated in place).
+ Never raises: a record that is not a dict, has no event or an unknown one is skipped."""
+ out: list[dict] = []
+ last = prev if isinstance(prev, dict) else None
+ pending = carry.get("pending") if isinstance(carry, dict) else None
+ seq = seq0
+ try:
+ items = list(records or ())
+ except Exception: # noqa: BLE001 - not a list of records at all
+ items = []
+ for rec in items:
+ try:
+ if pending is not None and isinstance(rec, dict):
+ t = _epoch(rec.get("ts", rec.get("timestamp")))
+ if t is not None and pending["t"] is not None and t - pending["t"] > REASON_CONFIRM_S:
+ pending = None # the planner did not act on that reason
+ hit = decide(rec)
+ if hit is None:
+ continue
+ kind, src, text, echo, role = hit
+ ts = _record_ts(rec)
+ if role == "clear":
+ pending = None
+ continue
+ if role == "reason":
+ pending = {"ts": ts, "t": _epoch(ts), "kind": kind, "src": src, "text": text}
+ continue
+ if role == "confirm" and pending is not None:
+ kind, src, text, ts = pending["kind"], pending["src"], pending["text"], pending["ts"]
+ pending = None
+ else:
+ if role == "resolve":
+ pending = None
+ if _is_echo(echo, last, ts):
+ continue
+ except Exception: # noqa: BLE001 - one odd record never costs the feed
+ continue
+ seq += 1
+ last = {"ts": ts, "kind": kind, "text": text, "src": src, "seq": seq}
+ out.append(last)
+ if isinstance(carry, dict):
+ carry["pending"] = pending
+ return out
+
+
+def now_state(events, stack_running: bool, exploring: bool | None, now_ts: float) -> dict:
+ """The state word for the page, from the decision events (oldest first).
+
+ The last decision wins, with holds. OBSTACLE_HOLD_S: an obstacle stop or a replan
+ around an obstacle -> OBSTACLE; the planner's stuck replan or its giving the goal up
+ when stuck -> STUCK. Frozen tracks (the base pushing harder) -> STUCK for
+ CONTACT_HOLD_S. A contact (a stall, the bump, the wall it leaves, an IMU contact) or
+ the explorer backing out of a corner -> BACKING OFF until the escape's end line,
+ at most ESCAPE_HOLD_S. While several holds run, the highest rank shows (BACKING OFF
+ over STUCK over OBSTACLE, the latest among equals); the end of an escape or of the
+ mission ends them all. Outside a hold the word is what the other events said: goal
+ / retry / new goal / a replan that keeps the goal (off track, a navigation failure)
+ -> HEADING; waiting / the explorer dropping a goal -> WAITING; exploration started
+ -> WAITING (the explorer picks a goal), stopped -> STOPPED, complete -> DONE. A goal
+ reached, the planner dropping a goal and the end of an escape -> HEADING / WAITING /
+ WAITING while the explorer runs (its own lines since the last start say so, or
+ `exploring` from the bus), READY otherwise: a tapped goal is over. An escape that
+ ends after a STOP leaves STOPPED. No stack -> IDLE; a stack and no decision yet ->
+ READY. Returns {word, detail, since}: since = the ts of the event that set the
+ word, detail = the goal line or that event's text."""
+ if not stack_running:
+ return {"word": "IDLE", "detail": "No stack running", "since": None}
+ base = None # (word, detail, ts) said by the last event that is not a hold
+ holds: list = [] # (rank, start epoch, end epoch, word, detail, ts)
+ last_hold = None # the last hold event, for its text once every hold ran out
+ goal_text = None # the explorer's line for the goal being driven to
+ explorer_on = False # the explorer's own lines say it runs
+ seen = False
+ for ev in events or ():
+ if not isinstance(ev, dict):
+ continue
+ seen = True
+ kind, ts, src = ev.get("kind"), ev.get("ts"), ev.get("src")
+ shown = str(ev.get("text") or "")
+ text = _FOLD.sub("", shown)
+ on = explorer_on or exploring is True
+ new_hold = None
+ if kind == "obstacle":
+ if text.startswith(TXT_CORNERED):
+ explorer_on = True
+ new_hold = ("BACKING OFF", ESCAPE_HOLD_S)
+ else:
+ new_hold = ("OBSTACLE", OBSTACLE_HOLD_S)
+ elif kind == "replan":
+ if text in QUIET_REPLANS:
+ base = ("HEADING", shown, ts)
+ else:
+ new_hold = ("STUCK" if text.startswith(TXT_STUCK) else "OBSTACLE", OBSTACLE_HOLD_S)
+ elif kind == "contact" and text.startswith(TXT_ESCAPE):
+ holds, goal_text = [], None
+ if base is None or base[0] not in ("STOPPED", "DONE"):
+ base = ("WAITING" if on else "READY", shown, ts)
+ elif kind in ("contact", "imu"):
+ new_hold = ("BACKING OFF", ESCAPE_HOLD_S)
+ elif kind == "push":
+ new_hold = ("STUCK", CONTACT_HOLD_S)
+ elif kind in ("goal", "retry"):
+ explorer_on = True
+ goal_text = shown if kind == "goal" else goal_text
+ base = ("HEADING", shown, ts)
+ elif kind == "new_goal":
+ base = ("HEADING", goal_text if goal_text and text in TXT_PLANNER_ECHOES else shown, ts)
+ elif kind == "reached":
+ goal_text = None
+ base = ("HEADING" if on else "READY", shown, ts)
+ elif kind == "waiting":
+ explorer_on, goal_text = True, None
+ base = ("WAITING", shown, ts)
+ elif kind == "goal_dropped":
+ goal_text = None
+ if src in ("planner", "recovery"):
+ base = ("WAITING" if on else "READY", shown, ts)
+ if text.startswith(TXT_ABANDONED):
+ new_hold = ("STUCK", OBSTACLE_HOLD_S)
+ else:
+ explorer_on = True
+ base = ("WAITING", shown, ts)
+ elif kind == "mission":
+ holds, goal_text = [], None
+ explorer_on = text.startswith(TXT_STARTED)
+ base = ("STOPPED" if text.startswith(TXT_STOPPED) else "DONE" if text.startswith(TXT_COMPLETE)
+ else "WAITING", shown, ts)
+ if new_hold is not None:
+ word, dur = new_hold
+ last_hold = (word, shown, ts)
+ t = _epoch(ts)
+ if t is not None: # a hold needs a time to end
+ holds.append((HOLD_RANK[word], t, t + dur, word, shown, ts))
+ if not seen:
+ return {"word": "READY", "detail": "Stack running, no decision yet", "since": None}
+ active = [h for h in holds if h[1] <= now_ts < h[2]]
+ if active:
+ _, _, _, word, detail, since = max(active, key=lambda h: (h[0], h[1]))
+ return {"word": word, "detail": detail, "since": since}
+ if base is not None:
+ return {"word": base[0], "detail": base[1], "since": base[2]}
+ return {"word": "WAITING" if explorer_on or exploring is True else "READY",
+ "detail": last_hold[1] if last_hold else "Stack running, no decision yet",
+ "since": last_hold[2] if last_hold else None}
+
+
+def decisions_doc(events, n: int, stack_running: bool, exploring: bool | None, now_ts: float, run) -> dict:
+ """/decisions.json: the state word now, the last n events, the run they belong to.
+ With no stack running, `run` is the last run whose events are kept."""
+ events = list(events or ())
+ now = now_state(events, stack_running, exploring, now_ts)
+ if not stack_running and run:
+ now["detail"] = f"No stack running (last run: {run})"
+ return {"now": now, "events": events[-n:] if n > 0 else [], "run": run}
+
+
+class DecisionLog:
+ """Decision rows in arrival order; `seq` never goes back, even across a restart.
+ Identical events one after the other (same kind and text: the explorer saying it
+ waits every second) make one row whose text ends in ' (xN)', with the last one's
+ ts and seq, so a long wait never pushes the contacts out of the page's 30 rows."""
+
+ def __init__(self, keep: int = DECISIONS_KEEP):
+ self.events: deque = deque(maxlen=keep)
+ self.seq = 0
+ self._carry: dict = {} # a replan reason waiting for its consequence across reads
+ self._fold = None # (kind, text, count) of the last row
+
+ def add(self, records) -> None:
+ new = decisions_from_records(records, self.seq, self.events[-1] if self.events else None, self._carry)
+ for ev in new:
+ self.seq = ev["seq"]
+ fold = self._fold
+ if fold is not None and self.events and fold[:2] == (ev["kind"], ev["text"]):
+ count = fold[2] + 1
+ self.events[-1] = dict(ev, text=f"{ev['text']} (x{count})") # a new dict: readers hold the old one
+ self._fold = (ev["kind"], ev["text"], count)
+ else:
+ self.events.append(ev)
+ self._fold = (ev["kind"], ev["text"], 1)
+
+ def clear(self) -> None:
+ self.events.clear()
+ self._carry, self._fold = {}, None
+
+
+class DecisionFeed(DecisionLog):
+ """The decision events of one run's main.jsonl, read incrementally and at most
+ every DECISIONS_MIN_INTERVAL_S. The first read takes the last DECISIONS_TAIL_BYTES;
+ later reads only the bytes appended since (a half-written last line waits for the
+ next read). Another file (a new run, a rotation, a truncation) or more than the
+ window appended since the last read starts over from the tail: the feed is always
+ one unbroken stretch of the log. A read without a log (the stack stopped) keeps the
+ last run's events and `run`; the next run's log starts a new feed."""
+
+ def __init__(self, tail_bytes: int = DECISIONS_TAIL_BYTES, keep: int = DECISIONS_KEEP,
+ min_interval_s: float = DECISIONS_MIN_INTERVAL_S):
+ super().__init__(keep)
+ self._tail_bytes = tail_bytes
+ self._min_interval_s = min_interval_s
+ self._lock = threading.Lock()
+ self._path = None
+ self._ident = None
+ self._offset = None
+ self._partial = b""
+ self._read_at = None
+ self.run = None # the run the events belong to
+
+ def _restart(self, path, ident=None) -> None:
+ self.clear()
+ self._path, self._ident, self._offset, self._partial = path, ident, None, b""
+
+ def read(self, log_dir: str | None, run=None) -> list[dict]:
+ """The events of <log_dir>/main.jsonl (oldest first); without a log_dir, the
+ events already read, as they are."""
+ path = os.path.join(log_dir, "main.jsonl") if log_dir else None
+ with self._lock:
+ if path is None:
+ return list(self.events)
+ if path != self._path:
+ self._restart(path)
+ self._read_at = None
+ self.run = run
+ now = time.monotonic()
+ if self._read_at is not None and now - self._read_at < self._min_interval_s:
+ return list(self.events)
+ self._read_at = now
+ try:
+ self._read_new(path)
+ except OSError:
+ pass # gone or unreadable for now: keep what was read
+ return list(self.events)
+
+ def _read_new(self, path: str) -> None:
+ with open(path, "rb") as fh:
+ st = os.fstat(fh.fileno())
+ ident = (st.st_dev, st.st_ino)
+ size = st.st_size
+ if (ident != self._ident or self._offset is None or size < self._offset
+ or size - self._offset > self._tail_bytes):
+ self._restart(path, ident)
+ start = max(0, size - self._tail_bytes)
+ drop_first = start > 0 # the window starts inside a line
+ else:
+ start, drop_first = self._offset, False
+ fh.seek(start)
+ chunk = fh.read(size - start)
+ self._offset = start + len(chunk)
+ lines = (self._partial + chunk).split(b"\n")
+ self._partial = lines.pop() # b"" after a final newline, else a line still being written
+ if len(self._partial) > self._tail_bytes:
+ self._partial = b""
+ if drop_first and lines:
+ lines = lines[1:]
+ records = []
+ for line in lines:
+ try:
+ rec = json.loads(line)
+ except ValueError:
+ continue # a stray print, a torn line
+ if isinstance(rec, dict):
+ out = {"ts": rec.get("timestamp"), "level": rec.get("level"),
+ "event": rec.get("event"), "logger": rec.get("logger")}
+ out.update((k, rec[k]) for k in RECORD_FIELDS if k in rec)
+ records.append(out)
+ self.add(records)
+
+
def preflight_doc() -> dict:
"""The last flight check moss-fly.sh saved (moss_preflight.py --json)."""
path = os.path.join(state_dir(), "preflight.json")
@@ -1028,6 +1571,7 @@ class LiveCockpit(Cockpit):
self._run_id = None # the run the bus cache belongs to
self._goal_lock = threading.Lock()
self._goal_gen = 0 # a stop, an explore or a newer goal cancels a queued goal
+ self._feed = DecisionFeed() # the decisions in the running stack's main.jsonl
# the listener ------------------------------------------------------------
def start_listener(self) -> None:
@@ -1229,6 +1773,23 @@ class LiveCockpit(Cockpit):
def log(self, n: int) -> list[dict]:
return log_tail(stack_status().get("log_dir"), n)
+ def decisions(self, n: int) -> dict:
+ """The running stack's decisions (its main.jsonl, read incrementally) and the word now.
+ `exploring` comes from the bus when it is listened to, else it is unknown (None).
+ Once the stack stops, the last run's events stay (IDLE, "last run: ..."), as its map
+ does, until a new run's log replaces them. A `dimos run` with no registry entry (a
+ foreground run, a swept registry) has no known log: READY, and the detail says so."""
+ stack = stack_status()
+ self._sync_run(stack)
+ now = time.time()
+ if stack.get("running") and stack.get("log_dir"):
+ events = self._feed.read(stack["log_dir"], stack.get("run_id"))
+ exploring = self._nav(time.monotonic(), stack)["exploring"] if lcm is not None and HAVE_MSGS else None
+ return decisions_doc(events, n, True, exploring, now, stack.get("run_id"))
+ if stack.get("running") or _dimos_run_pid() is not None:
+ return {"now": {"word": "READY", "detail": TXT_NO_LOG, "since": None}, "events": [], "run": None}
+ return decisions_doc(self._feed.read(None), n, False, None, now, self._feed.run)
+
def preflight(self) -> dict:
return preflight_doc()
@@ -1576,6 +2137,50 @@ class DemoWorld:
return np.column_stack((px[idx, first[idx]], py[idx, first[idx]])).astype(np.float32)
+# The demo's decisions: one exploration cycle as the stack's modules log it, in the
+# order a real run logs them (goal, obstacle and replan, contact, bump and wall,
+# escape, goal dropped, next goal, arrival). The lines go into the demo's log and
+# through decisions_from_records, like a real main.jsonl.
+DEMO_DECISION_CYCLE_S = 16.0
+DEMO_DECISION_SCRIPT = ( # (seconds into the cycle, level, logger, line)
+ (0.0, "info", "explorer2.py", "goal {goal}: ({gx:.2f}, {gy:.2f}) {dist:.1f} m away, {cells} frontier cells, "
+ "14 clusters, chosen in 120 ms"),
+ (0.1, "info", "global_planner.py", "Got new goal"),
+ (0.2, "info", "global_planner.py", "Found safe goal."),
+ (4.0, "info", "local_planner.py", "Obstacle detected ahead, stopping local planner."),
+ (4.1, "info", "global_planner.py", "Replanning path due to obstacle found."),
+ (4.2, "info", "global_planner.py", "Replanning."),
+ (4.3, "info", "global_planner.py", "Found safe goal."),
+ (8.0, "warning", "moss_adapter.py", "MOSS base: STALL #{stall}: tracks commanded L=+60% R=+60% but the encoders "
+ "moved L=+2 R=+1 ticks in 1.0 s - stopped (the encoders are the bumper)"),
+ (8.3, "warning", "recovering_planner.py", "BUMP received"),
+ (8.3, "info", "costmap2d.py", "costmap: contact at ({x:+.2f}, {y:+.2f}) heading {deg:+.0f} deg: "
+ "32 cells ahead marked as a wall"),
+ (10.4, "warning", "recovering_planner.py", "Contact escape"),
+ (10.5, "info", "explorer2.py", "planner gave up on that goal: excluded until the map or the viewpoint changes"),
+ (10.7, "info", "explorer2.py", "goal {goal2}: ({gx:.2f}, {gy:.2f}) {dist:.1f} m away, {cells} frontier cells, "
+ "14 clusters, chosen in 130 ms"),
+ (14.0, "info", "local_planner.py", "Reached goal position, starting final rotation"),
+)
+DEMO_LINE_FIELDS = { # the fields a real record carries with its line, where a decision reads them
+ "Contact escape": {"trigger": "bump", "direction": "reverse", "travelled_m": 0.201, "seconds": 2.1,
+ "aborted": False},
+ "BUMP received": {"reflex": "escape reverse 0.20 m", "acted": True},
+}
+DEMO_EXPLORE_START = ("exploration start: distance exp 2, cap 3 m, radius 4 m "
+ "(0 = no limit; the radius is centred on the first pose)")
+DEMO_STOP_LINES = { # how the explorer logs each way of being stopped
+ "cmd": ("Received exploration stop command via LCM", "Stopped autonomous frontier exploration"),
+ "stop": ("WavefrontFrontierExplorer: stop_movement received, stopping exploration",
+ "Stopped autonomous frontier exploration"),
+}
+
+
+def _iso_ms(t: float) -> str:
+ """Epoch seconds as dimOS stamps its records: ISO, UTC, milliseconds, a Z."""
+ return _dt.datetime.fromtimestamp(t, _dt.timezone.utc).isoformat(timespec="milliseconds").replace("+00:00", "Z")
+
+
class DemoCockpit(Cockpit):
"""Same endpoints, synthetic sources, never touches LCM or the launcher; the
machine metrics are real."""
@@ -1597,13 +2202,67 @@ class DemoCockpit(Cockpit):
self._launch_lines: deque = deque(maxlen=LAUNCH_TAIL)
self._launch_action = None
self._preflight_ts = now
+ self._decisions = DecisionLog()
+ self._script_t0 = None # the scripted exploration's start; None = nothing scripted runs
+ self._script_pos = (0, 0) # (cycle, step) of the next scripted line
+ self._goal_said = True # the arrival at a tapped goal is logged once
+ self._pending: list = [] # (at, level, logger, line) logged once their time comes
self._say("info", "demo cockpit: synthetic MOSS, nothing on the bus")
self._say("info", "RPLIDAR C1 up (demo)")
self._say("info", "lidar odometry up (demo)")
+ self._start_script(now)
def _say(self, level: str, event: str) -> None:
self._log.append({"ts": time.strftime(_ISO), "level": level, "event": event, "logger": "demo"})
+ # the decisions, scripted (called with self._lock held) ---------------------
+ def _line(self, at: float, level: str, logger: str, event: str) -> None:
+ """One line as a stack module logs it: into the demo log and the decision feed."""
+ rec = {"ts": _iso_ms(at), "level": level, "event": event, "logger": logger}
+ rec.update(DEMO_LINE_FIELDS.get(event, {}))
+ self._log.append(rec)
+ self._decisions.add([rec])
+
+ def _start_script(self, now: float) -> None:
+ self._line(now, "info", "explorer2.py", DEMO_EXPLORE_START)
+ self._script_t0, self._script_pos = now + 0.5, (0, 0)
+
+ def _stop_script(self, now: float, how: str) -> None:
+ if self._script_t0 is None:
+ return
+ for event in DEMO_STOP_LINES[how]:
+ self._line(now, "info", "wavefront_frontier_goal_selector.py", event)
+ self._script_t0 = None
+
+ def _script_values(self, cycle: int) -> dict:
+ w = self.world
+ tx, ty = w.target()
+ return {"goal": 2 * cycle + 1, "goal2": 2 * cycle + 2, "stall": cycle + 1, "gx": tx, "gy": ty,
+ "dist": max(0.1, math.hypot(tx - w.x, ty - w.y)), "cells": 20 + (7 * cycle) % 40,
+ "x": w.x, "y": w.y, "deg": math.degrees(w.yaw)}
+
+ def _script_tick(self, now: float) -> None:
+ """Log every scripted line that is due; a page away for long skips to the last cycle."""
+ if self._script_t0 is None:
+ return
+ cycle, step = self._script_pos
+ if now - (self._script_t0 + cycle * DEMO_DECISION_CYCLE_S) > 2 * DEMO_DECISION_CYCLE_S:
+ cycle, step = int((now - self._script_t0) // DEMO_DECISION_CYCLE_S) - 1, 0
+ while True:
+ offset, level, logger, line = DEMO_DECISION_SCRIPT[step]
+ at = self._script_t0 + cycle * DEMO_DECISION_CYCLE_S + offset
+ if at > now:
+ break
+ self._line(at, level, logger, line.format(**self._script_values(cycle)))
+ step += 1
+ if step == len(DEMO_DECISION_SCRIPT):
+ cycle, step = cycle + 1, 0
+ self._script_pos = (cycle, step)
+
+ def _run_id(self) -> str:
+ return (time.strftime("%Y%m%d-%H%M%S-demo-", time.localtime(self.stack["started"]))
+ + self.stack["blueprint"])
+
def _tick(self) -> float:
now = time.time()
with self._lock:
@@ -1612,6 +2271,13 @@ class DemoCockpit(Cockpit):
return now
may = not self.halted and (self.exploring or self.stack["blueprint"] == "moss-nav")
self.world.step(now, may)
+ self._script_tick(now)
+ for item in [p for p in self._pending if p[0] <= now]:
+ self._pending.remove(item)
+ self._line(*item)
+ if not self._goal_said and not self._pending and self.world.goal_reached:
+ self._goal_said = True
+ self._line(now, "info", "local_planner.py", "Reached goal position, starting final rotation")
return now
def state(self) -> dict:
@@ -1619,8 +2285,7 @@ class DemoCockpit(Cockpit):
w = self.world
running = self.stack["running"]
up = now - self.stack["started"]
- stack = ({"running": True, "run_id": time.strftime("%Y%m%d-%H%M%S-demo-", time.localtime(self.stack["started"]))
- + self.stack["blueprint"], "blueprint": self.stack["blueprint"], "pid": os.getpid(),
+ stack = ({"running": True, "run_id": self._run_id(), "blueprint": self.stack["blueprint"], "pid": os.getpid(),
"started_at": _dt.datetime.fromtimestamp(self.stack["started"], _dt.timezone.utc).isoformat(),
"uptime_s": round(up, 1), "log_dir": None} if running else {"running": False})
silent = 0.0 if running else now - (self.stack_stopped_at or now)
@@ -1697,6 +2362,14 @@ class DemoCockpit(Cockpit):
def log(self, n: int) -> list[dict]:
return list(self._log)[-n:]
+ def decisions(self, n: int) -> dict:
+ now = self._tick()
+ with self._lock:
+ running = self.stack["running"]
+ # a stopped stack keeps its run's events, as live mode does
+ return decisions_doc(list(self._decisions.events), n, running, running and self.exploring, now,
+ self._run_id() if running or self._decisions.events else None)
+
def preflight(self) -> dict:
checks = [("stack", "stack: none", ""), ("ports", "QT Py present", "by-id"),
("ports", "C1 lidar present", "by-id"), ("firmware", "telemetry", "qtpy-mdd3a-v3, mode 0 (off)"),
@@ -1726,9 +2399,14 @@ class DemoCockpit(Cockpit):
action = body.get("action")
if action not in ("start", "stop"):
raise ValueError('explore needs "action": "start" or "stop"')
+ was = self.exploring
self.exploring = action == "start"
self.halted = False if self.exploring else self.halted
self._say("info", f"demo: exploration {'started' if self.exploring else 'stopped'}")
+ if self.exploring and not was and self.stack["running"] and self.stack["blueprint"] == "moss-explore":
+ self._start_script(time.time())
+ elif not self.exploring:
+ self._stop_script(time.time(), "cmd")
return {"ok": True, "sent": "/explore_cmd#std_msgs.Bool" if self.exploring
else "/stop_explore_cmd#std_msgs.Bool"}
if name == "map":
@@ -1744,11 +2422,20 @@ class DemoCockpit(Cockpit):
self.halted = False
w.goal, w.goal_active, w.goal_reached = (x, y), True, None
self._say("info", f"demo: goal ({x:+.2f}, {y:+.2f})")
+ # exploring: the page stops the explorer first and the goal goes out GOAL_AFTER_STOP_S later
+ now = time.time()
+ at = now + GOAL_AFTER_STOP_S if self._script_t0 is not None else now
+ self._stop_script(now, "cmd")
+ self._pending = [(at, "info", "global_planner.py", "Got new goal"),
+ (at, "info", "global_planner.py", "Found safe goal.")]
+ self._goal_said = False
return {"ok": True, "sent": "/goal_request#geometry_msgs.PoseStamped",
"also": ["/stop_explore_cmd#std_msgs.Bool"], "goal": {"x": x, "y": y}}
if name == "stop":
self.exploring, self.halted, w.goal_active = False, True, False
self._say("warning", "demo: soft stop")
+ self._stop_script(time.time(), "stop")
+ self._pending, self._goal_said = [], True
return {"ok": True, "sent": "/tele_cmd_vel#geometry_msgs.Twist",
"also": ["/stop_explore_cmd#std_msgs.Bool", "/stop_movement#std_msgs.Bool"]}
if name == "episode":
@@ -1762,6 +2449,10 @@ class DemoCockpit(Cockpit):
now = time.time()
self._launch_action = action
self._launch_lines.append(f"=== moss-fly.sh {action} {time.strftime(_ISO)} === (demo)")
+ if action != "check" and not (action in ("map", "explore") and self.stack["running"]):
+ self._script_t0, self._pending, self._goal_said = None, [], True
+ if action != "stop":
+ self._decisions.clear() # a new stack: a fresh feed; a stop keeps the last run's
if action in ("map", "explore"):
if self.stack["running"]:
return {"ok": False, "error": "a dimOS stack is already running: stop it first"}
@@ -1871,6 +2562,12 @@ class Handler(BaseHTTPRequestHandler):
except ValueError:
n = 40
self._json(200, {"lines": self.cockpit.log(max(1, min(n, 500)))})
+ elif path == "/decisions.json":
+ try:
+ n = int(parse_qs(url.query).get("n", ["40"])[0])
+ except ValueError:
+ n = 40
+ self._json(200, self.cockpit.decisions(max(1, min(n, DECISIONS_KEEP))))
elif path == "/preflight.json":
self._json(200, self.cockpit.preflight())
elif path == "/launch.json":
diff --git a/tools/moss_cockpit/app.js b/tools/moss_cockpit/app.js
index 2100b79..7407401 100644
--- a/tools/moss_cockpit/app.js
+++ b/tools/moss_cockpit/app.js
@@ -7,6 +7,7 @@
* (both state and scan carry the pose: the robot marker takes the younger one, 3 Hz)
* GET /map_meta 1 Hz, then /map.png only when map_meta.stamp changed
* GET /log.json 0.5 Hz GET /preflight.json on load and after a check
+ * GET /decisions.json?n=30 1 Hz (the stack's decisions read from its log, and the word for now)
* GET /launch.json while a stack command runs
* POST /cmd/<explore|goal|stop|episode|stack>
*
@@ -15,7 +16,7 @@
*/
"use strict";
-var POLL = { state: 1000, meta: 1000, scan: 333, path: 1000, log: 2000, launch: 1000 };
+var POLL = { state: 1000, meta: 1000, scan: 333, path: 1000, log: 2000, launch: 1000, decisions: 1000 };
var FETCH_TIMEOUT_MS = 2500;
var TAP_SLOP_PX = 6; // a pointer that moved less than this is a tap, not a drag
var STALE_S = 3; // a topic older than this is shown as stale
@@ -27,6 +28,27 @@ var CAM_RETRY_MS = 3000; // reconnect the MJPEG stream this long after it e
var CAM_GRACE_MS = 6000; // a stream younger than this is not judged dead (first frame wait + a state poll)
var STALL_HOLD_S = 4; // the STALLED badge stays up at least this long after the stall count goes up
var DEPTH_COLOR = "rgba(255,193,94,.95)"; // amber: depth-camera obstacles, apart from the mint lidar scan
+var DECISION_ROWS = 30; // the decision feed shows this many, newest on top
+// The commanded-motion arrow at the robot: the path a constant (vx, wz) drives in CMD_ARROW_S,
+// so 0.45 m/s is 0.6 m on the map, drawn from the marker's edge (its nose, its tail in reverse)
+// and never shorter on screen than CMD_MIN_RUN; a pivot is an arc around the robot sweeping the
+// turn of PIVOT_ARROW_S, between PIVOT_MIN_RAD and PIVOT_MAX_RAD.
+var CMD_ARROW_S = 0.6 / 0.45;
+var PIVOT_ARROW_S = 3;
+var PIVOT_MIN_RAD = Math.PI / 4, PIVOT_MAX_RAD = 1.5 * Math.PI;
+var CMD_VX_EPS = 0.02; // m/s: slower than this the command does not translate (a pivot, or nothing)
+var CMD_WZ_EPS = 0.02; // rad/s: slower than this the command does not turn
+var PIVOT_RADIUS_M = 0.1; // a turn about a centre this close (under the robot) is drawn as a pivot
+var CMD_STEPS = 16; // segments of a drawn path
+var CMD_W = 4; // px: the arrow's line, thicker than the 2.25 px path
+var CMD_HEAD = 10; // px: arrowhead length (its half width is 0.5 of it)
+var CMD_MIN_RUN = 22; // px past the marker's edge, at least: the head and 12 px of line. At the
+ // fitted zoom (~37 px/m) 0.2 m/s is 10 px of map, which the marker would hide
+var PIVOT_R_PX = 17; // px: the pivot arc's radius, just outside the robot marker
+var MARK_L = 15, MARK_B = 8; // px: the robot marker's length and half width; nose .62 L ahead, tail .38 L behind
+var MINT = "#6CF8C7";
+var HALO = "#080F1A"; // navy (--moss-navy) outline round the arrow: it reads over white walls, and
+ // over the mint path (2.25 px, no outline) it is a separate, outlined shape
var S = {
online: false, state: null,
@@ -34,7 +56,8 @@ var S = {
preflight: null, launchBusy: false, launchAction: null,
view: { scale: 40, x: -3, y: 3 }, fitted: false,
pendingGoal: null, camPaused: false, camOk: false, camAt: 0, camLost: null, pfAll: false,
- stateAt: 0, stallLatch: { count: null, until: 0 }
+ stateAt: 0, stallLatch: { count: null, until: 0 },
+ decisions: null, decHtml: null, decKeys: null
};
function $(id) { return document.getElementById(id); }
@@ -134,6 +157,9 @@ function exploring() { return !!(S.state && S.state.nav && S.state.nav.exploring
// seconds since the last state the cockpit answered: the server's ages froze then
function offlineFor() { return S.online || !S.stateAt ? 0 : Math.max(0, (Date.now() - S.stateAt) / 1000); }
+// seconds since the last state answer, online or not: what to add to an age the state carries
+function sinceState() { return S.stateAt ? Math.max(0, (Date.now() - S.stateAt) / 1000) : 0; }
+
// a bus topic's age as the server measured it now, or null when it is silent or unknown
function topicAge(name) {
var t = S.state && S.state.topics && S.state.topics[name];
@@ -624,8 +650,12 @@ function draw() {
// pose: white triangle pointing along yaw (the younger of the two poses, freshestPose)
var p = pose();
+ // commanded motion: what the planner asks of the tracks now, out of the marker's nose (tail in reverse);
+ // drawn first so the marker stays whole where the line's round cap meets it
+ var cmd = cmdArrow(nav && nav.last_cmd_vel, p, sinceState());
+ if (cmd) drawCmd(cmd);
if (p && typeof p.x === "number") {
- var cx = sx(p.x), cy = sy(p.y), a = -(p.yaw || 0), L = 15, B = 8;
+ var cx = sx(p.x), cy = sy(p.y), a = -(p.yaw || 0), L = MARK_L, B = MARK_B;
ctx.save(); ctx.translate(cx, cy); ctx.rotate(a);
ctx.beginPath(); ctx.moveTo(L * .62, 0); ctx.lineTo(-L * .38, B); ctx.lineTo(-L * .2, 0); ctx.lineTo(-L * .38, -B); ctx.closePath();
ctx.fillStyle = (typeof p.age_s === "number" && p.age_s > STALE_S) ? "rgba(234,240,239,.5)" : "#EAF0EF";
@@ -645,6 +675,103 @@ function ring(x, y, dashed) {
ctx.restore();
}
+// The commanded motion (nav.last_cmd_vel: vx m/s, forward positive; wz rad/s, left positive;
+// age_s on the server's clock) as a shape in the world frame at pose p ({x, y, yaw}), or null
+// when there is nothing to draw: no pose, no command, a command LIVE_S old or more once
+// extraAge (s since the server answered) is added, or a zero command.
+// {kind: "path", x, y, yaw, sign, len, pts: [[x, y], ...], tip: {x, y, ux, uy}}: the path a
+// constant (vx, wz) drives in CMD_ARROW_S from the pose, straight or bent (an arc), behind the
+// robot in reverse (sign -1); len = its length, m; tip = its end and the unit direction the
+// robot moves there. drawCmd moves it to the marker's edge on screen.
+// {kind: "pivot", x, y, yaw, dir, sweep}: a turn on the spot (|vx| under CMD_VX_EPS, or a turn
+// radius |vx / wz| under PIVOT_RADIUS_M), dir 1 = left (counterclockwise), -1 = right;
+// sweep = the turn of PIVOT_ARROW_S, rad, kept between PIVOT_MIN_RAD and PIVOT_MAX_RAD
+function cmdArrow(c, p, extraAge) {
+ if (!c || !p || typeof p.x !== "number" || typeof p.y !== "number") return null;
+ var vx = c.vx, wz = c.wz;
+ if (typeof vx !== "number" || typeof wz !== "number" || !isFinite(vx) || !isFinite(wz)) return null;
+ var age = (typeof c.age_s === "number" ? c.age_s : Infinity) + (extraAge || 0);
+ if (!(age < LIVE_S)) return null;
+ var yaw = typeof p.yaw === "number" && isFinite(p.yaw) ? p.yaw : 0;
+ if (Math.abs(vx) < CMD_VX_EPS || (Math.abs(wz) >= CMD_WZ_EPS && Math.abs(vx / wz) < PIVOT_RADIUS_M)) {
+ if (Math.abs(wz) < CMD_WZ_EPS) return null;
+ return { kind: "pivot", x: p.x, y: p.y, yaw: yaw, dir: wz > 0 ? 1 : -1,
+ sweep: Math.max(PIVOT_MIN_RAD, Math.min(PIVOT_MAX_RAD, Math.abs(wz) * PIVOT_ARROW_S)) };
+ }
+ var c0 = Math.cos(yaw), s0 = Math.sin(yaw), pts = [];
+ for (var i = 0; i <= CMD_STEPS; i++) {
+ var t = CMD_ARROW_S * i / CMD_STEPS, fx, fy; // robot frame at the pose: x forward, y left
+ if (Math.abs(wz) < 1e-9) { fx = vx * t; fy = 0; }
+ else { fx = vx / wz * Math.sin(wz * t); fy = vx / wz * (1 - Math.cos(wz * t)); }
+ pts.push([p.x + c0 * fx - s0 * fy, p.y + s0 * fx + c0 * fy]);
+ }
+ var h = yaw + wz * CMD_ARROW_S, sg = vx > 0 ? 1 : -1, end = pts[CMD_STEPS];
+ return { kind: "path", x: p.x, y: p.y, yaw: yaw, sign: sg, len: Math.abs(vx) * CMD_ARROW_S, pts: pts,
+ tip: { x: end[0], y: end[1], ux: sg * Math.cos(h), uy: sg * Math.sin(h) } };
+}
+
+// A screen polyline ([[x, y], ...], px) without its last `cut` px, [] when it is not longer.
+function trimEnd(sp, cut) {
+ var total = 0, run = 0, i, d;
+ for (i = 1; i < sp.length; i++) total += Math.hypot(sp[i][0] - sp[i - 1][0], sp[i][1] - sp[i - 1][1]);
+ var keep = total - cut, out = [sp[0]];
+ if (!(keep > 0)) return [];
+ for (i = 1; i < sp.length; i++) {
+ d = Math.hypot(sp[i][0] - sp[i - 1][0], sp[i][1] - sp[i - 1][1]);
+ if (run + d >= keep) {
+ var f = d > 0 ? (keep - run) / d : 0;
+ out.push([sp[i - 1][0] + f * (sp[i][0] - sp[i - 1][0]), sp[i - 1][1] + f * (sp[i][1] - sp[i - 1][1])]);
+ return out;
+ }
+ run += d; out.push(sp[i]);
+ }
+ return out;
+}
+
+// Draws cmdArrow's shape at the robot: a navy outline, the mint line, then the arrowhead, whose
+// tip is the end of the path (or of the pivot arc) and which points the way the robot moves.
+// A path starts at the marker's edge, its nose (its tail in reverse), not under the marker, and
+// runs its metres at the map's scale past that edge, or CMD_MIN_RUN px when that is longer: the
+// same shape scaled, so the turn over it stays wz * CMD_ARROW_S. The line stops under the head.
+// Screen angles turn clockwise (y down): a world angle a is -a.
+function drawCmd(a) {
+ var line = null, tip; // line() builds the shaft; tip = [x, y, ux, uy], screen px and direction
+ if (a.kind === "pivot") {
+ var cx = sx(a.x), cy = sy(a.y), a0 = -a.yaw, a1 = a0 - a.dir * a.sweep, back = CMD_HEAD / PIVOT_R_PX;
+ var aEnd = a1 + a.dir * back * .7;
+ line = function () { ctx.beginPath(); ctx.arc(cx, cy, PIVOT_R_PX, a0, aEnd, a.dir > 0); };
+ var tx = cx + PIVOT_R_PX * Math.cos(a1), ty = cy + PIVOT_R_PX * Math.sin(a1);
+ var bx = cx + PIVOT_R_PX * Math.cos(a1 + a.dir * back), by = cy + PIVOT_R_PX * Math.sin(a1 + a.dir * back);
+ var l = Math.hypot(tx - bx, ty - by) || 1; // the head lies along the chord, on the arc
+ tip = [tx, ty, (tx - bx) / l, (ty - by) / l];
+ } else {
+ var c0 = sx(a.x), c1 = sy(a.y), edge = a.sign > 0 ? MARK_L * .62 : MARK_L * .38;
+ var px = a.len * S.view.scale, k = px > 0 ? Math.max(CMD_MIN_RUN, px) / px : 1;
+ var ox = c0 + a.sign * edge * Math.cos(a.yaw), oy = c1 - a.sign * edge * Math.sin(a.yaw);
+ var sp = a.pts.map(function (q) { return [ox + k * (sx(q[0]) - c0), oy + k * (sy(q[1]) - c1)]; });
+ tip = [sp[sp.length - 1][0], sp[sp.length - 1][1], a.tip.ux, -a.tip.uy];
+ sp = trimEnd(sp, CMD_HEAD * .7);
+ if (sp.length > 1) line = function () {
+ ctx.beginPath(); ctx.moveTo(sp[0][0], sp[0][1]);
+ for (var i = 1; i < sp.length; i++) ctx.lineTo(sp[i][0], sp[i][1]);
+ };
+ }
+ var hx = tip[0] - tip[2] * CMD_HEAD, hy = tip[1] - tip[3] * CMD_HEAD;
+ var nx = -tip[3] * CMD_HEAD * .5, ny = tip[2] * CMD_HEAD * .5;
+ function head() {
+ ctx.beginPath(); ctx.moveTo(tip[0], tip[1]); ctx.lineTo(hx + nx, hy + ny); ctx.lineTo(hx - nx, hy - ny); ctx.closePath();
+ }
+ ctx.save();
+ ctx.lineCap = "round"; ctx.lineJoin = "round";
+ ctx.strokeStyle = HALO;
+ if (line) { ctx.lineWidth = CMD_W + 4; line(); ctx.stroke(); }
+ ctx.lineWidth = 4; head(); ctx.stroke();
+ ctx.strokeStyle = MINT; ctx.fillStyle = MINT;
+ if (line) { ctx.lineWidth = CMD_W; line(); ctx.stroke(); }
+ head(); ctx.fill();
+ ctx.restore();
+}
+
function pollMeta() {
return fetchJSON("/map_meta").then(function (m) {
if (!m || m.__missing || m.error || !(m.width > 0)) {
@@ -977,6 +1104,138 @@ function pollLog() {
}, function () {});
}
+// ---------------------------------------------------------------- decisions
+
+// What the stack decides, read by the server from the stack's own log (/decisions.json): the
+// events {ts, kind, text, src, seq}, oldest first, and the word for now {word, detail, since}.
+// A kind's colour says what happened: the robot touched something (warn), something is in the
+// planner's way (caution), it heads somewhere (accent), it waits or gives a goal up (dim).
+var KIND_CLASS = {
+ contact: "warn", push: "warn", imu: "warn",
+ obstacle: "caution", replan: "caution", pivot_fallback: "caution",
+ goal: "accent", new_goal: "accent", reached: "accent",
+ waiting: "dim", goal_dropped: "dim", retry: "dim",
+ mission: ""
+};
+var KIND_LABEL = {
+ goal: "GOAL", new_goal: "NEW GOAL", reached: "REACHED", obstacle: "OBSTACLE", replan: "REPLAN",
+ contact: "CONTACT", push: "PUSH", imu: "IMU", pivot_fallback: "PIVOT", waiting: "WAITING",
+ goal_dropped: "DROPPED", retry: "RETRY", mission: "MISSION"
+};
+var WORD_CLASS = {
+ "OBSTACLE": "warn", "BACKING OFF": "warn", "STUCK": "warn", "HEADING": "accent",
+ "IDLE": "dim", "READY": "dim", "WAITING": "dim", "DONE": "dim", "STOPPED": "dim"
+};
+var WORD_DETAIL = { "IDLE": "No stack running.", "READY": "Stack up. Exploration not armed." };
+
+// A log time as epoch seconds: a number as it is, an ISO string parsed (dimOS writes UTC with
+// a "Z"; without a zone it is the browser's local time), anything else null.
+function tsSeconds(ts) {
+ if (typeof ts === "number") return isFinite(ts) ? ts : null;
+ if (typeof ts !== "string" || !/^\d{4}-\d{2}-\d{2}T/.test(ts)) return null;
+ var t = Date.parse(ts);
+ return isFinite(t) ? t / 1000 : null;
+}
+
+// minutes and seconds on the local clock: the log's hh:mm:ss without the hour
+function minSec(ts) {
+ var t = tsSeconds(ts);
+ if (t === null) { var m = /\d{2}:(\d{2}:\d{2})/.exec(String(ts == null ? "" : ts)); return m ? m[1] : "--:--"; }
+ var d = new Date(t * 1000);
+ return pad2(d.getMinutes()) + ":" + pad2(d.getSeconds());
+}
+
+// The server's events as the feed's rows, newest on top, at most n: {seq, time, cls, label, text}.
+// Ordered by seq when every event has one, else by position (the server sends oldest first).
+// Anything that is not an object is dropped.
+function decisionRows(events, n) {
+ var list = (Array.isArray(events) ? events : []).filter(function (e) { return e && typeof e === "object"; });
+ var bySeq = list.every(function (e) { return typeof e.seq === "number" && isFinite(e.seq); });
+ var rows = list.map(function (e, i) {
+ var k = String(e.kind == null ? "" : e.kind);
+ return { at: bySeq ? e.seq : i, seq: bySeq ? e.seq : null, time: minSec(e.ts), cls: KIND_CLASS[k] || "",
+ label: KIND_LABEL[k] || k.replace(/_/g, " ").toUpperCase() || "--",
+ text: e.text == null ? "" : String(e.text) };
+ });
+ rows.sort(function (a, b) { return b.at - a.at; });
+ return rows.slice(0, n).map(function (r) { delete r.at; return r; });
+}
+
+// Which rows are new since the previous answer, by what they say (time, kind, text; a repeat of
+// the same line counted from the oldest), not by seq: {keys, fresh: [bool per row]}. prevKeys
+// null (the first answer) flashes nothing; an answer with nothing in common with a non-empty
+// previous one is a new run or a restarted server, and flashes nothing either.
+function freshRows(rows, prevKeys) {
+ var seen = {}, keys = [], fresh = [], i;
+ for (i = rows.length - 1; i >= 0; i--) {
+ var k = rows[i].time + "|" + rows[i].label + "|" + rows[i].text;
+ seen[k] = (seen[k] || 0) + 1;
+ keys[i] = k + "#" + seen[k];
+ }
+ var prev = {}, common = 0;
+ (prevKeys || []).forEach(function (k) { prev[k] = true; });
+ keys.forEach(function (k) { if (prev[k]) common++; });
+ var flash = Array.isArray(prevKeys) && (prevKeys.length === 0 || common > 0);
+ keys.forEach(function (k, j) { fresh[j] = flash && !prev[k]; });
+ return { keys: keys, fresh: fresh };
+}
+
+// The feed's html; fresh[i] true makes row i flash once.
+function decisionsHtml(rows, fresh) {
+ return rows.map(function (r, i) {
+ var cls = (r.cls + (fresh && fresh[i] ? " fresh" : "")).trim();
+ return "<li" + (cls ? ' class="' + cls + '"' : "") + '><span class="t">' + esc(r.time) + '</span><span class="k">'
+ + esc(r.label) + '</span><span class="x">' + esc(r.text) + "</span></li>";
+ }).join("");
+}
+
+// The word for now as the page shows it: {word, cls, detail, since}. nowS is the server's clock
+// now (epoch s), so the time since the word was set reads on the clock that stamped it.
+function nowView(now, nowS) {
+ if (!now || typeof now !== "object" || !now.word) return { word: "--", cls: "dim", detail: "", since: "" };
+ var word = String(now.word).toUpperCase(), t = tsSeconds(now.since);
+ return { word: word, cls: WORD_CLASS[word] || "",
+ detail: now.detail ? String(now.detail) : (WORD_DETAIL[word] || ""),
+ since: t !== null && typeof nowS === "number" && isFinite(nowS) ? "for " + dur(Math.max(0, nowS - t)) : "" };
+}
+
+// the server's clock now (no NTP in the field): its last state's ts plus the time since that answer
+function serverNow() {
+ return S.state && typeof S.state.ts === "number" ? S.state.ts + sinceState() : Date.now() / 1000;
+}
+
+function pollDecisions() {
+ return fetchJSON("/decisions.json?n=" + DECISION_ROWS).then(function (d) {
+ S.decisions = d || null;
+ renderDecisions();
+ }, function () {});
+}
+
+// An answer with an error and no events ({error}, the server's 500) says nothing about the
+// decisions: the feed and its keys stay as they were, only the word and its detail say so.
+function renderDecisions() {
+ var d = S.decisions, nv, html = null;
+ if (!d) return;
+ if (d.__missing) {
+ nv = { word: "--", cls: "dim", detail: "This cockpit server has no decision feed.", since: "" };
+ html = '<li class="empty">No /decisions.json on this cockpit server.</li>';
+ } else {
+ nv = nowView(d.now, serverNow());
+ if (d.error && !d.now) nv.detail = "Decision feed error: " + String(d.error);
+ if (!d.error || Array.isArray(d.events)) {
+ var rows = decisionRows(d.events, DECISION_ROWS), fr = freshRows(rows, S.decKeys);
+ html = decisionsHtml(rows, fr.fresh);
+ S.decKeys = fr.keys;
+ if (!html) html = '<li class="empty">' + (stack().running ? "No decision yet." : "No stack, no decisions.") + "</li>";
+ }
+ }
+ setText("now-word", nv.word);
+ $("now-word").className = nv.cls;
+ setText("now-detail", nv.detail);
+ setText("now-since", nv.since);
+ if (html !== null && html !== S.decHtml) { $("decisions").innerHTML = html; S.decHtml = html; }
+}
+
// ---------------------------------------------------------------- go
if ("ResizeObserver" in window) new ResizeObserver(function () { resize(); if (!S.fitted) fitView(); }).observe(wrap);
@@ -991,3 +1250,4 @@ poller(pollMeta, POLL.meta);
poller(pollScan, POLL.scan);
poller(pollPath, POLL.path);
poller(pollLog, POLL.log);
+poller(pollDecisions, POLL.decisions);
diff --git a/tools/moss_cockpit/index.html b/tools/moss_cockpit/index.html
index 4d33b5b..5aefc1b 100644
--- a/tools/moss_cockpit/index.html
+++ b/tools/moss_cockpit/index.html
@@ -147,7 +147,7 @@
#hud { position: absolute; top: 14px; left: 0; z-index: 2; font-size: 12.5px; color: var(--ui-dim);
font-variant-numeric: tabular-nums; pointer-events: none; }
#hud > span { display: block; }
- #hud-cam { margin-top: 4px; color: #FFC15E; } /* amber, the colour of the depth-camera dots on the map */
+ #hud-cam { margin-top: 4px; color: var(--ui-caution); } /* amber, the colour of the depth-camera dots on the map */
#map-empty { position: absolute; inset: 0; display: flex; align-items: center; justify-content: center;
pointer-events: none; text-align: center; }
#map-empty[hidden] { display: none; }
@@ -219,6 +219,32 @@
padding: 15px 22px 14px; font-size: 18px; border-radius: 15px; box-shadow: 0 6px 24px rgba(8, 15, 26, .5); }
#col-right { padding-bottom: 72px; } /* the end of the column scrolls clear of the floating STOP */
+ /* ---- decisions: what the stack does now (one word), then what it decided, newest on top ---- */
+ .now { margin: 0 0 14px; }
+ #now-word { font-size: 34px; font-weight: var(--moss-w-display); line-height: 1.02; letter-spacing: .03em;
+ text-transform: uppercase; overflow-wrap: anywhere; }
+ #now-word.warn { color: var(--ui-attention); }
+ #now-word.accent { color: var(--moss-mint); }
+ #now-word.dim { color: var(--ui-dim); }
+ #now-detail { font-size: 14px; margin-top: 6px; overflow-wrap: anywhere; font-variant-numeric: tabular-nums; }
+ #now-since { margin-top: 2px; }
+ #decisions { list-style: none; margin: 0; padding: 0; font-size: 12.5px; line-height: 1.4; }
+ #decisions li { display: grid; grid-template-columns: 38px 78px 1fr; column-gap: 8px; align-items: baseline;
+ padding: 3px 0; border-bottom: 1px solid rgba(106, 135, 151, .3); }
+ #decisions li:last-child { border-bottom: 0; }
+ #decisions .t { color: var(--ui-unknown); font-variant-numeric: tabular-nums; }
+ #decisions .k { justify-self: start; padding: 1px 5px; border-radius: 3px; font-weight: 600; font-size: 10.5px;
+ letter-spacing: .1em; line-height: 1.3; white-space: nowrap; box-shadow: inset 0 0 0 1.25px currentColor; }
+ #decisions .x { overflow-wrap: anywhere; font-variant-numeric: tabular-nums; }
+ #decisions .warn .k { background: var(--ui-attention); color: var(--moss-navy); box-shadow: none; }
+ #decisions .warn .x { color: var(--ui-attention); }
+ #decisions .caution .k { color: var(--ui-caution); }
+ #decisions .accent .k { color: var(--moss-mint); }
+ #decisions .dim .k, #decisions .dim .x { color: var(--ui-dim); }
+ #decisions li.empty { display: block; color: var(--ui-dim); border: 0; }
+ #decisions li.fresh { animation: fresh 1s var(--moss-ease); }
+ @keyframes fresh { from { background: rgba(108, 248, 199, .2); } to { background: transparent; } }
+
/* ---- log ---- */
#log { list-style: none; margin: 0; padding: 0; font-size: 12px; line-height: 1.45; font-variant-numeric: tabular-nums; }
#log li { display: grid; grid-template-columns: 58px 1fr; column-gap: 8px; padding: 2px 0; }
@@ -250,10 +276,11 @@
#col-map { display: flex; order: 1; height: 86vh; min-height: 520px; margin-bottom: 28px; }
.cams { height: 24vh; column-gap: 10px; }
#mod-controls { order: 3; }
- #mod-flight { order: 4; }
- #mod-base { order: 5; }
- #mod-system { order: 6; }
- #mod-log { order: 7; }
+ #mod-decisions { order: 4; }
+ #mod-flight { order: 5; }
+ #mod-base { order: 6; }
+ #mod-system { order: 7; }
+ #mod-log { order: 8; }
}
@media (prefers-reduced-motion: reduce) {
@@ -397,7 +424,7 @@
<div class="cam-cap"><span>Nose camera (RealSense) and wrist camera (arm), 5 fps.</span><button class="btn link" id="b-cam" type="button">Pause video</button></div>
</div>
- <!-- column 3: controls, log -->
+ <!-- column 3: controls, decisions, log -->
<div class="col" id="col-right">
<section class="mod" id="mod-controls" aria-labelledby="ctl-title">
<div class="mod-head">
@@ -445,6 +472,19 @@
</div>
</section>
+ <section class="mod" id="mod-decisions" aria-labelledby="dec-title">
+ <div class="mod-head">
+ <p class="kicker">DECISIONS</p>
+ <h2 class="title" id="dec-title">What it decides.</h2>
+ </div>
+ <div class="now" data-live>
+ <p id="now-word" class="dim" role="status" aria-live="polite">--</p>
+ <p id="now-detail">Waiting for the cockpit.</p>
+ <p id="now-since" class="small dim num"></p>
+ </div>
+ <ol id="decisions" data-live aria-label="Decisions, newest first"></ol>
+ </section>
+
<section class="mod" id="mod-log" aria-labelledby="log-title">
<div class="mod-head alt">
<p class="kicker">LOG</p>
diff --git a/tools/moss_cockpit/tokens.css b/tools/moss_cockpit/tokens.css
index 681256d..83a66e7 100644
--- a/tools/moss_cockpit/tokens.css
+++ b/tools/moss_cockpit/tokens.css
@@ -72,12 +72,14 @@
ok = mint text
attention = salmon text
failure = salmon badge with navy text
+ caution = amber text
unknown = white at 0.6 */
--ui-ok: var(--moss-mint);
--ui-attention: var(--moss-salmon);
--ui-fail-bg: var(--moss-salmon);
--ui-fail-fg: var(--moss-navy);
--ui-unknown: rgba(234, 240, 239, .6);
+ --ui-caution: #FFC15E; /* the depth camera's dots on the map; obstacle / replan in the decision feed */
--ui-dim: rgba(234, 240, 239, .78); /* secondary text: 4.74:1 on studio */
--ui-control-line: rgba(234, 240, 239, .5); /* button borders: >= 3:1 on studio */
}
diff --git a/tools/moss_imu_probe.py b/tools/moss_imu_probe.py
new file mode 100755
index 0000000..6ffeb3f
--- /dev/null
+++ b/tools/moss_imu_probe.py
@@ -0,0 +1,185 @@
+#!/usr/bin/env python3
+"""Check, on the robot, that the RealSense streams colour, depth AND its IMU
+from ONE process: the way MOSS_IMU=1 opens it in the stack
+(moss_dimos/realsense_imu.py).
+
+One pipeline with depth 640x480 and colour 640x480 at 30 fps, read by
+wait_for_frames in a thread; then the motion module of THAT pipeline's
+device, opened through the sensor API (accel at its highest rate, gyro at
+--imu-hz), never a second pipeline. From the moment the motion module is
+open it counts --seconds (default 4) of frames, prints the count and rate of
+each stream, the last accel sample and the mean accel (where gravity sits:
+about -9.81 m/s^2 on y when the camera is level), then stops everything.
+
+Exit 0 when depth, colour, accel and gyro all counted frames; 1 otherwise,
+with the exception text of the step that failed.
+
+Run it with the stack DOWN: while the stack holds the camera, another process
+cannot open it ("failed to set power state"), so the probe refuses to start
+when it finds a running stack.
+
+ tools/moss_imu_probe.py # 4 s, gyro 200 Hz
+ tools/moss_imu_probe.py --seconds 10 --imu-hz 400 --serial <serial number>
+
+Nothing moves; nothing is written.
+"""
+from __future__ import annotations
+
+import argparse
+import os
+import subprocess
+import sys
+import threading
+import time
+from pathlib import Path
+
+ROOT = Path(__file__).resolve().parent.parent
+sys.path.insert(0, str(ROOT))
+
+STACK_RE = r"[b]in/dimos( [^ ]+)* run( |$)"
+WIDTH, HEIGHT, FPS = 640, 480, 30
+
+
+def stack_running() -> bool:
+ try:
+ out = subprocess.run(["pgrep", "-f", STACK_RE], capture_output=True, text=True, timeout=5).stdout
+ except Exception: # noqa: BLE001
+ return False
+ return bool(out.strip())
+
+
+def gravity_axis(mean: tuple[float, float, float]) -> str:
+ """The axis that carries gravity, signed: '-y' for (0, -9.8, 0)."""
+ i = max(range(3), key=lambda k: abs(mean[k]))
+ return ("-" if mean[i] < 0 else "+") + "xyz"[i]
+
+
+def main(argv: list[str] | None = None) -> int:
+ ap = argparse.ArgumentParser(description="colour + depth + IMU from one process, on the robot, stack down")
+ ap.add_argument("--seconds", type=float, default=4.0, help="how long to stream (default 4)")
+ ap.add_argument("--imu-hz", type=int, default=200, help="gyro rate (default 200, as MOSS_IMU)")
+ ap.add_argument("--serial", default=os.environ.get("MOSS_RS_SERIAL", "").strip() or None,
+ help="the RealSense serial number (default MOSS_RS_SERIAL, else the first camera)")
+ args = ap.parse_args(argv)
+
+ if stack_running():
+ print("a dimOS stack is running: it holds the camera, and this probe could not open it. "
+ "Stop the stack first (tools/moss-fly.sh stop).")
+ return 1
+ try:
+ import pyrealsense2 as rs
+ from moss_dimos.realsense_imu import open_motion_sensor
+ except Exception as exc: # noqa: BLE001
+ print(f"import failed: {type(exc).__name__}: {exc}")
+ print("(run it with the robot's venv; on a Jetson export GLIBC_TUNABLES as the launchers do, docs/base.md)")
+ return 1
+
+ counts = {"depth": 0, "color": 0, "accel": 0, "gyro": 0}
+ last_accel: list[float] | None = None
+ accel_sum = [0.0, 0.0, 0.0]
+ lock = threading.Lock()
+ errors: list[str] = []
+ stop = threading.Event()
+
+ def on_motion(frame) -> None: # noqa: ANN001 - a pyrealsense2 frame
+ nonlocal last_accel
+ motion = frame.as_motion_frame()
+ if not motion:
+ return
+ kind = motion.get_profile().stream_type()
+ data = motion.get_motion_data()
+ with lock:
+ if kind == rs.stream.accel:
+ counts["accel"] += 1
+ last_accel = [data.x, data.y, data.z]
+ accel_sum[0] += data.x
+ accel_sum[1] += data.y
+ accel_sum[2] += data.z
+ elif kind == rs.stream.gyro:
+ counts["gyro"] += 1
+
+ def video_loop(pipeline) -> None: # noqa: ANN001
+ while not stop.is_set():
+ try:
+ frames = pipeline.wait_for_frames(timeout_ms=1000)
+ except Exception as exc: # noqa: BLE001
+ if not stop.is_set():
+ errors.append(f"wait_for_frames: {type(exc).__name__}: {exc}")
+ return
+ with lock:
+ if frames.get_depth_frame():
+ counts["depth"] += 1
+ if frames.get_color_frame():
+ counts["color"] += 1
+
+ pipeline = rs.pipeline()
+ config = rs.config()
+ if args.serial:
+ config.enable_device(args.serial)
+ config.enable_stream(rs.stream.depth, WIDTH, HEIGHT, rs.format.z16, FPS)
+ config.enable_stream(rs.stream.color, WIDTH, HEIGHT, rs.format.bgr8, FPS)
+ sensor = None
+ thread = None
+ t_start = time.monotonic()
+ try:
+ try:
+ profile = pipeline.start(config)
+ except Exception as exc: # noqa: BLE001
+ errors.append(f"pipeline start (depth + colour): {type(exc).__name__}: {exc}")
+ profile = None
+ if profile is not None:
+ device = profile.get_device()
+ name = device.get_info(rs.camera_info.name) if device.supports(rs.camera_info.name) else "?"
+ print(f"camera: {name}, depth + colour {WIDTH}x{HEIGHT} @ {FPS} fps on one pipeline")
+ thread = threading.Thread(target=video_loop, args=(pipeline,), daemon=True)
+ thread.start()
+ try:
+ opened = open_motion_sensor(device, rs, args.imu_hz, on_motion)
+ if opened is None:
+ errors.append("this camera has no motion module")
+ else:
+ sensor, accel_p, gyro_p = opened
+ print(f"motion module open on the same device: accel {accel_p.fps()} Hz, gyro {gyro_p.fps()} Hz")
+ except Exception as exc: # noqa: BLE001
+ errors.append(f"motion module: {type(exc).__name__}: {exc}")
+ with lock: # count from here: the video must keep flowing once the motion module is open
+ counts.update(depth=0, color=0, accel=0, gyro=0)
+ accel_sum[:] = [0.0, 0.0, 0.0]
+ t_start = time.monotonic()
+ time.sleep(max(0.5, args.seconds))
+ finally:
+ stop.set()
+ if sensor is not None:
+ for step in (sensor.stop, sensor.close):
+ try:
+ step()
+ except Exception: # noqa: BLE001
+ pass
+ try:
+ pipeline.stop()
+ except Exception: # noqa: BLE001
+ pass
+ if thread is not None:
+ thread.join(timeout=3)
+ span = max(1e-6, time.monotonic() - t_start)
+
+ with lock:
+ c = dict(counts)
+ last = list(last_accel) if last_accel else None
+ mean = tuple(s / c["accel"] for s in accel_sum) if c["accel"] else None
+ for key in ("depth", "color", "accel", "gyro"):
+ print(f" {key:6s} {c[key]:6d} frames ({c[key] / span:6.1f} per s)")
+ if last:
+ print(f" last accel ({last[0]:+.3f}, {last[1]:+.3f}, {last[2]:+.3f}) m/s^2")
+ if mean:
+ print(f" mean accel ({mean[0]:+.3f}, {mean[1]:+.3f}, {mean[2]:+.3f}) m/s^2 - gravity on {gravity_axis(mean)} "
+ "(level camera, librealsense axes: -y)")
+ for err in errors:
+ print(f" error: {err}")
+ ok = all(c[k] > 0 for k in ("depth", "color", "accel", "gyro"))
+ print("video and motion together: YES" if ok else "video and motion together: NO")
+ return 0 if ok else 1
+
+
+if __name__ == "__main__":
+ sys.exit(main())