MOSS: a small, mostly 3D-printed litter-picking rover. Cleaner streets, one maker at a time.
Python
dimOS software moves into this repository: moss_dimos, tools, benches, docs
metrox7689fa180487
diff --git a/CHANGELOG.md b/CHANGELOG.mdindex d6d7e8b..4143472 100644--- a/CHANGELOG.md+++ b/CHANGELOG.md@@ -2,9 +2,10 @@Hardware revisions. Dates are when it was real, not when it was planned.+- **11 October 2026, dimOS software moves into this repository** (`moss_dimos`): base, lidar odometry, map, planner, explorer, gamepad, recorder, flight check, launcher, cockpit. dimOS itself stays an outside dependency, installed from git ([Software.md](Software.md)).- **10 October 2026, first drive at the gamepad.** The V0.5 rover, arm mounted, driven on the floor from a gamepad through dimOS: forward, reverse, turns, pivots. What it took and what it showed:- Firmware `qtpy-mdd3a-v3` on the QT Py ESP32-S3 (`firmware/MOSS_QTPY_MDD3A/`): the 25 % commissioning limit is gone, full stick is 100 % PWM (at 25 % the loaded tracks did not move at all); left track direction fixed in the firmware, encoder counts follow; `ramp UP DOWN` sets the rise and fall slopes live; a build switch for the Cytron MDD10A (PWM + DIR) next to the MDD3A build; the board names its driver in the banner and the telemetry. Host test updated.- - Teleop: the base runs as a dimOS blueprint (`moss-gamepad` in [vector-dimos](https://github.com/metrox-eth/vector-dimos)): deadman on the pad, twist to track percent, keepalive inside the firmware's 300 ms deadman, tank steering (past half turn stick the inner track reverses), one launcher with gates (pad, board, firmware, battery) and a telemetry line every two seconds while driving.+ - Teleop: the base runs as a dimOS blueprint (`moss-gamepad`, now in this repository's `moss_dimos`): deadman on the pad, twist to track percent, keepalive inside the firmware's 300 ms deadman, tank steering (past half turn stick the inner track reverses), one launcher with gates (pad, board, firmware, battery) and a telemetry line every two seconds while driving.- Seen under load: the board applies 100 %, the 3S battery drops from 11.6 V at rest to 10.8 V in a pivot. Verdict on the drive line: the MDD3A (3 A per channel) and the 56:1 motors leave no headroom for ramps. Next on the drive line: Cytron MDD10A (10 A per channel) and JGB37-520 motors at 110 rpm (higher ratio), both not installed yet; the firmware build for the MDD10A already exists.- Tracks: the double tensioner holds, the bearings keep mechanical friction low; the TPU tracks may need running in, or print settings that make them more supple.- In CAD, no part printed or fitted: hull R19 with an integrated mount for the MDD10A (rotated hole pattern 55.88 x 78.74 mm, four M3 x 6 into 2.7 mm pilot holes, no inserts), charging jack moved to clear the larger board, the motor-side drive-spacer passages opened to 9.5 mm; transmission R12-C1 (two drive-shaft spacers that were missing, 3.5 mm, PETG) and R12-C2 (matching screw-head recesses on the inner frames and inner tensioner plates, so all four tension screws become M4 x 35). CAD checks only; the production plates and the BOM are not reconciled to it, and nothing is frozen.diff --git a/NOTICE b/NOTICEindex 80c76d9..e6fb1f6 100644--- a/NOTICE+++ b/NOTICE@@ -8,6 +8,10 @@ Documentation and media (*.md, media/): CC BY 4.0.Parts derived from the SO-101 arm (TheRobotStudio/SO-ARM100, Apache-2.0) keep their upstream notice.The gripper is derived from NormaCore's parallel-jaw gripper, under NormaCore's licence.-moss_dimos/gamepad.py is derived from vector-dimos (https://github.com/metrox-eth/vector-dimos),-Copyright 2026 Laurent Genoud, Apache-2.0, itself an external blueprint for dimOS-(https://github.com/dimensionalOS/dimos), Copyright 2025 Dimensional Inc., Apache-2.0.+Parts of moss_dimos derive from earlier work by the same author, Apache-2.0. moss_dimos is an+external blueprint package for dimOS (https://github.com/dimensionalOS/dimos); portions are derived+from dimOS, Copyright 2025 Dimensional Inc., Apache-2.0.++The cockpit font, Space Grotesk (tools/moss_cockpit/fonts/), Copyright 2020 The Space Grotesk+Project Authors, is licensed under the SIL Open Font License 1.1 (OFL-1.1); the licence text ships+next to the font as SpaceGrotesk-OFL.txt.diff --git a/Software.md b/Software.mdindex b55e8a5..2c6f63e 100644--- a/Software.md+++ b/Software.md@@ -1,8 +1,6 @@# Software-The stack is dimOS. The microcontroller drives the motors; dimOS, on the Jetson or the Pi, does teleop, recording, navigation and manipulation. The policies come later, from pooled data.--**The motor firmware drove the prototype, and since 10 October the rover is driven on the floor from a gamepad through dimOS (vector-dimos `moss-gamepad`, below).**+The microcontroller drives the motors. Everything above it runs on [dimOS](https://github.com/dimensionalOS/dimos) by Dimensional, on the robot's computer: teleop, the lidar map, navigation, exploration and recording. The code for that is in this repository, in `moss_dimos/` and `tools/`.## Motor firmware, ESP32-S3@@ -18,39 +16,103 @@ The stack is dimOS. The microcontroller drives the motors; dimOS, on the Jetson| `stop` | stops and disarms || `zero` | resets the encoder counts (disarmed only) |-Telemetry every 200 ms as JSON: firmware and driver names, mode, applied and target percent, raw encoder ticks, battery voltage. Disarmed at boot, nothing moves by itself. The 25 % commissioning limit was lifted after the first drive on the floor (10 October): full stick is 100 % PWM, the host caps lower when it wants to (`MOSS_MAX_PCT`). No speed loop, no odometry in metres yet.+Telemetry every 200 ms as JSON: firmware and driver names, mode, applied and target percent, raw encoder ticks, battery voltage. Disarmed at boot, nothing moves by itself. Full stick is 100 % PWM; the host caps lower when it wants to (`MOSS_MAX_PCT`). The firmware is open loop: there is no speed loop, and odometry in metres needs the encoder calibration in [docs/base.md](docs/base.md).The board reads battery voltage through an INA219. It does not measure motor current: the shunt is not in the motor line.-## Teleop and navigation: dimOS+To reflash the QT Py, from the root of the checkout: build the images with `arduino-cli compile --fqbn esp32:esp32:adafruit_qtpy_esp32s3_nopsram:CDCOnBoot=cdc --export-binaries firmware/MOSS_QTPY_MDD3A` (they land in `firmware/MOSS_QTPY_MDD3A/build/`), add esptool to the venv with the `firmware` extra (`uv pip install --python .venv/bin/python -e '.[firmware]'`), then run `firmware/MOSS_QTPY_MDD3A/bench/flash_qtpy.sh`. The bench scripts are described in [its README](firmware/MOSS_QTPY_MDD3A/bench/README.md).++## What is in `moss_dimos/`++| Module | Role |+|---|---|+| `moss_adapter.py`, `moss_mock.py` | the tracked-base adapter (twist to track percent, keepalive inside the firmware's 300 ms deadman) and a fake firmware for runs without motors |+| `gamepad.py` | the pad: twist behind a deadman, slew-limited, sized to the base |+| `c1_serial.py`, `rplidar_c1.py` | the RPLidar C1 driver and its dimOS module |+| `lidar_odometry.py` | the pose, from the lidar (kiss-icp) |+| `costmap2d.py` | the 2D map the planner and the cockpit read |+| `recovering_planner.py`, `explorer2.py` | A* to a goal with a way out of tight spots, and frontier exploration |+| `memory.py` | the recorder: the whole run in one SQLite file |+| `robot_profile.py` | MOSS's geometry and device paths, each overridable by a `MOSS_*` variable |+| `blueprints.py` | the four dimOS blueprints: `moss-base`, `moss-gamepad`, `moss-nav`, `moss-explore` |++`tools/` holds the flight check (`moss_preflight.py`), the launchers (`moss-fly.sh`, `moss-teleop.sh`), the exploration switch (`explore_ctl.py`) and the cockpit (`moss_cockpit.py` and its page).++## Install on the robot++Python 3.12 in a venv at the root of the checkout (the launchers look for `.venv/`), made with [uv](https://docs.astral.sh/uv/), which also fetches Python 3.12 where the system ships an older one (JetPack 6 on a Jetson, Raspberry Pi OS). Then dimOS from git, then this package:++```+uv venv --python 3.12 .venv+DIMOS_ALLOW_MISSING_COCKPIT=1 uv pip install --python .venv/bin/python \+ "dimos @ git+https://github.com/dimensionalOS/dimos.git@a7d19f76158b01739b26e7a17390f89764aaef83"+uv pip install --python .venv/bin/python -e '.[nav,gamepad,camera]' # kiss-icp for the lidar odometry, pygame for the pad, pyrealsense2 for the RealSense+sudo usermod -aG dialout "$USER" # once, then log in again+```++dimOS is installed at the commit this package runs against. `DIMOS_ALLOW_MISSING_COCKPIT=1` builds it without its web UI, which is not in its git tree and which MOSS does not use (the cockpit below is its own page). dimOS main has since moved the planner and the explorer this package imports, so a newer commit must pass `benches/run_all.sh` first.++No RealSense: leave `camera` out and run with `MOSS_CAMERA=0`. On a Jetson the PyPI `pyrealsense2` wheel installs but does not import (it needs a newer glibc than JetPack 6 has): leave `camera` out and install `pyrealsense2-extended`, or a librealsense built for the board with its Python bindings. Either way, copy librealsense's `config/99-realsense-libusb.rules` to `/etc/udev/rules.d/` and run `sudo udevadm control --reload-rules && sudo udevadm trigger`, so the camera opens without root.++On a computer where dimOS sits on wheels built for that board, install this package with `uv pip install --python .venv/bin/python -e . --no-deps` so those wheels stay, and add `kiss-icp==1.3.0`, `pygame` and `pyserial` by hand (and the RealSense module above). A venv made with `python3.12 -m venv .venv` works the same way, with `.venv/bin/pip install` in place of `uv pip install --python .venv/bin/python`. The editable install registers the blueprints as `moss-dimos.moss-base`, `moss-dimos.moss-gamepad`, `moss-dimos.moss-nav` and `moss-dimos.moss-explore`; run it again after adding a blueprint.++### First run: system settings++dimOS's bus (LCM) needs multicast on the loopback interface, a `224.0.0.0/4` route on it, and 64 MB socket buffers (`net.core.rmem_max`, `net.core.rmem_default`). None of it survives a reboot. dimOS checks them at each launch and sets what is missing with sudo, so either the robot's user has passwordless sudo, or `tools/lcm-loopback.service` applies them at boot (install line in [docs/cockpit.md](docs/cockpit.md#the-cockpit-as-a-service)). Without either, a launch stops on a `CalledProcessError` from `sudo ip ...`.++## Run++```+tools/moss-fly.sh check # flight check only, nothing launched+tools/moss-fly.sh map # gates, then moss-nav: lidar map, pad teleop, recording+tools/moss-fly.sh explore # gates, then moss-explore: map, planner, explorer, NOT armed+EXPLORE=1 tools/moss-fly.sh explore # ... and arm exploration once the stack is proven+tools/moss-fly.sh stop # stop exploration, then the stack+MOCK=1 tools/moss-fly.sh map # fake motor board, no motor; the lidar must still be plugged+tools/moss-teleop.sh # the pad only: gates, then moss-gamepad+```++Pad on the dimOS path: hold R1 to drive (the deadman), left stick forward and back, right stick to turn, R2 boost. Release R1 and the tracks stop.++Every gate refuses rather than guesses, with the fix on the same line: a stack already up, a missing board, a port held by another program, a mute firmware, a battery at the BMS floor, a lidar that does not spin. Nothing moves by itself: in `map` only the pad drives; in `explore` the planner moves on a goal or once exploration is armed. The firmware stops the tracks 300 ms after the last command, whatever happens above it.++The flight check, `tools/moss_preflight.py`, is read-only: stack, ports, firmware, lidar spin, RealSense, wrist camera, pad, system, software and the state directory, one line each, exit 1 on any failure. The launcher runs it before every launch and saves it for the cockpit. A missing pad is a warning; `MOSS_REQUIRE_PAD=1` makes it a failure. With `MOSS_CAMERA=0` the RealSense line is skipped.++## The cockpit++`tools/moss-fly.sh cockpit` starts a small web server on the robot, port 8904, and prints its address. One page: the flight check, battery and track outputs, the live map with the lidar scan and the path (tap to send a goal), the two cameras, launch and stop buttons, episode markers and the log. The manual, the endpoints and the service install are in [docs/cockpit.md](docs/cockpit.md).++## Recording and calibration++Each run is recorded into one SQLite file, `~/.local/state/moss/recordings/moss.db` (`MOSS_STATE_DIR` moves it): pose, map, lidar, both cameras, commands, base telemetry and episode markers. The previous run is kept beside it. Details and how to read it back: [docs/cockpit.md](docs/cockpit.md#recording).-MOSS runs [dimOS](https://github.com/dimensionalOS) by Dimensional, on the Jetson or on the Pi. The base that drove on 10 October is the `moss-gamepad` blueprint of [vector-dimos](https://github.com/metrox-eth/vector-dimos): a gamepad module publishing a Twist behind a deadman, a tracked-base adapter that turns the twist into track percent and keeps the firmware's deadman fed, tank steering (past half turn stick the inner track reverses), a launcher with gates (pad present, board present, firmware disarmed, battery above its floor). `moss_dimos/` in this repository is the earlier skeleton of the same idea, cold-tested only. Navigation and the pick come next, then speed control and odometry.+The geometry and the base numbers are placeholders until measured; each has a `MOSS_*` environment variable. The base's three ([docs/base.md](docs/base.md)) turn a twist into track percent and encoder ticks into metres; the rest (lidar height, footprint, clearances) is in [docs/cockpit.md](docs/cockpit.md#the-robot-profile).-## Bench tool, `moss_pi/`+## Bench tool without dimOS, `moss_pi/`-Talks to the firmware from any computer over USB, to arm, drive and log telemetry during commissioning. Python standard library plus pyserial, and pygame for a pad.+Talks to the firmware from any computer over USB, to arm and drive during commissioning. Python standard library plus pyserial, and pygame for a pad. It was written for the earlier DevKitC board and its USB-UART: it opens the port with DTR low, and the current QT Py firmware sends nothing to a host that holds DTR low, so on the QT Py it gets no telemetry and no replies. On the current board, drive with `tools/moss-teleop.sh` instead.```python3 -m venv .venv && . .venv/bin/activatepip install -e '.[pi,test]'-python -m pytest tests # no robot neededsudo usermod -aG dialout "$USER" # once, then log in again-ls /dev/serial/by-id/ # find the USB-UART adapter-python -m moss_pi.teleop --port /dev/ttyUSB0 --keyboard # r arm, hold w/s/a/d, space stop, q quit-python -m moss_pi.teleop --port /dev/ttyUSB0 --gamepad # A arm, hold L1 to drive, R1 boost-python -m moss_pi.telemetry_log --port /dev/ttyUSB0 --out telemetry.jsonl+PORT=$(ls /dev/serial/by-id/*QT_Py*) # the motor board (the CP2102N one is the lidar, never this)+python -m moss_pi.teleop --port "$PORT" --keyboard # r arm, hold w/s/a/d, space stop, q quit+python -m moss_pi.teleop --port "$PORT" --gamepad # SDL button 0 arms, hold 4 to drive, 5 boost+python -m moss_pi.telemetry_log --port "$PORT" --out telemetry.jsonl```-Nothing moves until you arm with the deadman released. Hold the deadman to drive; release and it brakes, then disarms. The stick is open loop: full stick is 25 % duty. Put the rover on blocks for the first run.+This path reads raw SDL buttons: 0 arms, 4 is the deadman, 5 is boost. That is A, L1 and R1 on an Xbox-layout pad. On the MOSS pad R1 is button 7, so this numbering does not match it: use the dimOS path with that pad, and on any other pad check which physical button is 4 before driving. Nothing moves until you arm with the deadman released. Hold the deadman to drive; release and it brakes, then disarms. The stick is open loop: full stick is 25 % duty. Put the rover on blocks for the first run.## Data and policiesDemonstrations are recorded with the same camera names on every rover, so datasets pool without rework. The rules are in [docs/data.md](docs/data.md). The simulator (MuJoCo body, three recorded pickup missions) is in [moss-jev](https://github.com/metrox-eth/moss-jev).-## Tests+## Tests and benches```-python -m pytest tests+python -m pytest tests # no robot, no dimOS+benches/run_all.sh .venv/bin/python # every bench, one PASSED/FAILED line each (the robot's venv)```-Cold tests only: a fake serial port replaying a real bench session, and a Linux pseudo-terminal. The firmware's host logic test is `firmware/tests/control_test.cpp`, any C++17 compiler.+`tests/` is pytest: a fake serial port replaying a real bench session, a Linux pseudo-terminal, the pure pad and drive logic. `benches/` are scripts, one per module or tool, each feeding known values and reading back known values (metres, degrees, bytes on a named channel); run one with `PYTHONPATH=. .venv/bin/python benches/bench_<name>.py`. Most benches need dimOS, so run them in the robot's venv. Six also run without it and skip the sections that need it, with a line that says so: `bench_robot_profile_cold`, `bench_moss_adapter_cold`, `bench_explorer2_cold`, `bench_moss_preflight_cold`, `bench_moss_fly_gates_cold` and `bench_moss_cockpit_cold`. `run_all.sh` leaves out the live bench, `bench_lcm_latest_live`, unless `LIVE=1` is set: it needs LCM multicast on the machine. The firmware's host logic test is `firmware/tests/control_test.cpp`, any C++17 compiler.diff --git a/benches/bench_cost_gradient_cold.py b/benches/bench_cost_gradient_cold.pynew file mode 100644index 0000000..7efaeaa--- /dev/null+++ b/benches/bench_cost_gradient_cold.py@@ -0,0 +1,317 @@+"""Cold bench for the obstacle cost gradient: known geometry in -> known route out.++No robot, no stack. Every case is a synthetic grid at the real 0.05 m+resolution, so the numbers are metres a tape measure would agree with.++The three things the gradient has to get right at once:+ 1. given two ways round, take the wide one;+ 2. given open space beside a wall, keep away from the wall;+ 3. given a tight gap as the ONLY way through, still go through it. A lethal+ inflation rounded up to whole cells walls off doorways the body fits+ through, and the robot then explores nothing.++The corridor and gap geometry is built for a 0.50 m wide REFERENCE body (the+body the cost field was designed and swept on). Those cases pass its radii+explicitly, so they do not depend on the robot profile. The last case checks+the profile path: called with no radii, the field takes MOSS's clearances+from moss_dimos.robot_profile, read at call time.++Needs dimOS. Run from the repository root:+ python benches/bench_cost_gradient_cold.py+"""++import os+import sys+from pathlib import Path as _FsPath++import numpy as np+from scipy import ndimage++sys.path.insert(0, str(_FsPath(__file__).resolve().parent.parent))++from dimos.msgs.geometry_msgs.Pose import Pose # noqa: E402+from dimos.msgs.nav_msgs.OccupancyGrid import CostValues, OccupancyGrid # noqa: E402+from dimos.navigation.replanning_a_star.min_cost_astar import min_cost_astar # noqa: E402+from moss_dimos import robot_profile # noqa: E402+from moss_dimos.recovering_planner import ( # noqa: E402+ PIVOT_PENALTY,+ clearance_cost_map,+ clearances,+)++RES = 0.05++# The reference body: width, pivot envelope and the cross-track margin the+# follower may spend. Lethal = width/2 + margin, pivot = pivot diameter/2,+# the same formulas as robot_profile.derived().+REF_ROBOT_WIDTH_M = 0.50+REF_PIVOT_DIAMETER_M = 0.78+REF_CONTROL_MARGIN_M = 0.05+REF_LETHAL_CLEARANCE_M = REF_ROBOT_WIDTH_M / 2 + REF_CONTROL_MARGIN_M # 0.30+REF_PIVOT_CLEARANCE_M = REF_PIVOT_DIAMETER_M / 2 # 0.39+REF = {"lethal_m": REF_LETHAL_CLEARANCE_M, "pivot_m": REF_PIVOT_CLEARANCE_M}++# MOSS nominal body (robot_profile.MOSS): 0.34 m wide, 0.50 m pivot diameter,+# 0.05 m margin -> lethal 0.22 m, pivot 0.25 m.+MOSS_LETHAL_M = 0.34 / 2 + 0.05+MOSS_PIVOT_M = 0.50 / 2++# env vars that would change the profile under the bench; cleared while the+# profile case runs and restored after it+_PROFILE_ENV = ("MOSS_ROBOT_WIDTH_M", "MOSS_PIVOT_DIAMETER_M", "MOSS_CONTROL_MARGIN_M")+++def make_grid(cells: np.ndarray) -> OccupancyGrid:+ origin = Pose()+ origin.position.x, origin.position.y, origin.position.z = 0.0, 0.0, 0.0+ origin.orientation.w = 1.0+ return OccupancyGrid(+ grid=cells.astype(np.int8), resolution=RES, origin=origin, frame_id="map", ts=0.0+ )+++def world(gx: int, gy: int) -> tuple[float, float]:+ return ((gx + 0.5) * RES, (gy + 0.5) * RES)+++def path_cells(path, shape) -> list[tuple[int, int]]:+ out = []+ for pose in path.poses:+ gx, gy = int(pose.position.x / RES), int(pose.position.y / RES)+ if 0 <= gy < shape[0] and 0 <= gx < shape[1]:+ out.append((gx, gy))+ return out+++def clearance_field(cells: np.ndarray) -> np.ndarray:+ """Metres from every cell to the nearest occupied cell."""+ return ndimage.distance_transform_edt(cells < CostValues.OCCUPIED) * RES+++def usable_cells(cells: np.ndarray, row: int, lo: int, hi: int) -> int:+ """Cells of that corridor the reference body may actually stand on."""+ field = clearance_field(cells)+ return int(np.sum(field[row, lo:hi] >= REF_LETHAL_CLEARANCE_M))+++def gap_crossing(gap_m: float, **radii) -> tuple[int, int, bool]:+ """One wall across the grid with a single gap: (gap cells, passable cells+ on the wall row, does the planned path cross the wall row)."""+ gap = int(round(gap_m / RES))+ height, width = 61, 41+ cells = np.zeros((height, width), dtype=np.int8)+ wall_row = height // 2+ x0 = (width - gap) // 2+ cells[wall_row, :x0] = CostValues.OCCUPIED+ cells[wall_row, x0 + gap:] = CostValues.OCCUPIED++ costmap = clearance_cost_map(make_grid(cells), **radii)+ passable = int(np.sum(costmap.grid[wall_row] < CostValues.OCCUPIED))+ path = min_cost_astar(costmap, world(width // 2, height - 3), world(width // 2, 2))+ crossed = path is not None and any(gy == wall_row for gx, gy in path_cells(path, cells.shape))+ return gap, passable, crossed+++def test_constants_are_physically_ordered() -> None:+ """The three radii must stay in the order the design assumes: the body+ fits, the margin is real, and pivoting needs more room than driving."""+ assert REF_ROBOT_WIDTH_M / 2 < REF_LETHAL_CLEARANCE_M < REF_PIVOT_CLEARANCE_M, (+ REF_ROBOT_WIDTH_M / 2,+ REF_LETHAL_CLEARANCE_M,+ REF_PIVOT_CLEARANCE_M,+ )+ print(+ f" reference body/2 {REF_ROBOT_WIDTH_M / 2:.3f} < lethal {REF_LETHAL_CLEARANCE_M:.3f} "+ f"< pivot {REF_PIVOT_CLEARANCE_M:.3f} m"+ )+++def test_prefers_the_wide_corridor() -> None:+ """Two parallel corridors of the same length, both joining start to goal.++ Sized so that after the lethal radius one leaves 3 usable cells and the+ other 8, counted in cells the planner can really stand on. (3 and 8 raw+ cells would be 0.15 m and 0.40 m, both narrower than the 0.50 m reference+ body, so neither would be a corridor at all.)+ """+ height, width = 101, 121+ cells = np.zeros((height, width), dtype=np.int8)+ band_lo, band_hi = 25, 76 # the corridor band; rooms above and below+ narrow_lo, narrow_hi = 8, 21 # 13 cells = 0.65 m between the faces+ wide_lo, wide_hi = 34, 52 # 18 cells = 0.90 m between the faces+ cells[band_lo:band_hi, :] = CostValues.OCCUPIED+ cells[band_lo:band_hi, narrow_lo:narrow_hi] = 0+ cells[band_lo:band_hi, wide_lo:wide_hi] = 0++ mid = height // 2+ n_narrow = usable_cells(cells, mid, narrow_lo, narrow_hi)+ n_wide = usable_cells(cells, mid, wide_lo, wide_hi)+ print(f" usable width: narrow 0.65 m -> {n_narrow} cells, wide 0.90 m -> {n_wide} cells")+ assert (n_narrow, n_wide) == (3, 8), (n_narrow, n_wide)++ # start and goal sit between the two corridor mouths, so neither route is+ # shorter: narrow centre x=14, wide centre x=42.5, start x=28.+ start = world(28, 10)+ goal = world(28, height - 11)+ costmap = clearance_cost_map(make_grid(cells), **REF)+ path = min_cost_astar(costmap, goal, start)+ assert path is not None and path.poses, "no path through either corridor"++ crossing = [gx for gx, gy in path_cells(path, cells.shape) if band_lo + 10 <= gy < band_hi - 10]+ assert crossing, "path never crossed the corridor band"+ mean_x = float(np.mean(crossing))+ took_wide = mean_x > (narrow_hi + wide_lo) / 2+ print(+ f" narrow centre x={(narrow_lo + narrow_hi) / 2 * RES:.2f} m, "+ f"wide centre x={(wide_lo + wide_hi) / 2 * RES:.2f} m, "+ f"path crossed at x={mean_x * RES:.2f} m -> {'WIDE' if took_wide else 'NARROW'}"+ )+ assert took_wide, "path took the narrow corridor with a wide one available"+++def test_the_penalty_is_what_chooses_the_wide_corridor() -> None:+ """Pin the constant to the behaviour it buys.++ With the penalty off, dimOS's ridge cost alone is indifferent: it puts cost+ 0 on the medial axis of BOTH corridors, so the tie falls to length and the+ narrow one wins. This is the measurement that justifies PIVOT_PENALTY+ existing at all - if this test ever passes at penalty 0, the term is dead+ weight and should go.+ """+ height, width = 101, 121+ cells = np.zeros((height, width), dtype=np.int8)+ cells[25:76, :] = CostValues.OCCUPIED+ cells[25:76, 8:21] = 0 # 0.65 m+ cells[25:76, 34:52] = 0 # 0.90 m+ grid = make_grid(cells)+ start, goal = world(28, 10), world(28, height - 11)++ for penalty, expected in ((0, "NARROW"), (PIVOT_PENALTY, "WIDE")):+ path = min_cost_astar(clearance_cost_map(grid, penalty=penalty, **REF), goal, start)+ assert path is not None and path.poses, f"no path at penalty {penalty}"+ crossing = [gx for gx, gy in path_cells(path, cells.shape) if 35 <= gy < 66]+ got = "WIDE" if float(np.mean(crossing)) > 27 else "NARROW"+ print(f" penalty {penalty:>4} -> {got}")+ assert got == expected, f"penalty {penalty} took the {got} corridor"+++def test_keeps_away_from_a_lone_wall() -> None:+ """One wall with open space beside it: the path must stop hugging it.++ Start and goal sit 0.325 m off the wall - passable, but closer than the+ 0.39 m reference pivot radius. Without a gradient A* runs straight along+ that line. With one it has to bow away wherever there is room; the ends+ are pinned by the start and goal, so the >= 0.35 m rule is checked on the+ middle.+ """+ height, width = 61, 121+ cells = np.zeros((height, width), dtype=np.int8)+ cells[0, :] = CostValues.OCCUPIED # the wall, along y = 0+ grid = make_grid(cells)+ field = clearance_field(cells)++ start, goal = world(6, 6), world(width - 7, 6) # 0.325 m off the wall: passable, under the pivot radius+ flat = min_cost_astar(grid, goal, start) # no gradient at all+ graded = min_cost_astar(clearance_cost_map(grid, **REF), goal, start)+ assert flat is not None and graded is not None++ def stats(path):+ pts = path_cells(path, cells.shape)+ keep = pts[len(pts) // 6 : -len(pts) // 6] # drop the pinned ends+ vals = [field[gy, gx] for gx, gy in keep]+ return float(np.mean(vals)), float(np.min(vals))++ flat_mean, flat_min = stats(flat)+ graded_mean, graded_min = stats(graded)+ print(+ f" middle-of-path clearance: no gradient mean {flat_mean:.3f} min {flat_min:.3f} m"+ f" -> gradient mean {graded_mean:.3f} min {graded_min:.3f} m"+ )+ assert graded_min >= 0.35, f"path came within {graded_min:.3f} m of the wall"+ assert graded_min > flat_min, "the gradient bought no clearance at all"+++def test_still_takes_a_tight_gap_when_it_is_the_only_way() -> None:+ """A gap and nothing else. The reference body is 0.50 m wide, so 0.55 m+ must go through (11 free cells: the wall cells on either side are 0.60 m+ apart, centre to centre, and the middle cell sits exactly ON the lethal+ radius).++ Regression guard. dimOS's own 1.1x inflation walls these off - every gap+ up to 0.62 m plans "no path" with it - and the robot is then left in+ front of doorways it fits through.+ """+ for gap_m, must_pass in (+ (0.40, False), (0.45, False), (0.50, False),+ (0.55, True), (0.62, True), (0.80, True),+ ):+ gap, passable, crossed = gap_crossing(gap_m, **REF)+ print(+ f" gap {gap_m:.2f} m ({gap} cells): {passable} passable cell(s), "+ f"path {'CROSSES' if crossed else 'does not cross'}"+ )+ if must_pass:+ assert crossed, (f"{gap_m:.2f} m gap walled off - the reference body is "+ f"{REF_ROBOT_WIDTH_M:.2f} m wide")+ else:+ assert not crossed, f"{gap_m:.2f} m gap accepted - narrower than the body"+++def test_default_radii_follow_the_robot_profile() -> None:+ """No radii given (what the planner does on every A*): the field takes the+ clearances of the active profile, read at call time.++ MOSS nominal: lethal 0.22 m, pivot 0.25 m. Overriding MOSS_ROBOT_WIDTH_M+ and MOSS_PIVOT_DIAMETER_M with the reference body's numbers must give the+ reference field, cell for cell, with no restart.+ """+ saved = {k: os.environ.pop(k, None) for k in _PROFILE_ENV}+ try:+ lethal, pivot = clearances()+ d = robot_profile.derived(robot_profile.current())+ print(f" profile {robot_profile.current().name}: clearances() = ({lethal:.3f}, {pivot:.3f}) m")+ assert (lethal, pivot) == (d.lethal_clearance_m, d.pivot_clearance_m), (lethal, pivot, d)+ assert abs(lethal - MOSS_LETHAL_M) < 1e-9 and abs(pivot - MOSS_PIVOT_M) < 1e-9, (lethal, pivot)++ # the gap table again, under MOSS's own radii: a 0.34 m body needs 0.44 m+ expected = {0.40: (0, False), 0.45: (1, True), 0.50: (2, True),+ 0.55: (3, True), 0.62: (4, True), 0.80: (8, True)}+ for gap_m, (want_cells, must_pass) in expected.items():+ gap, passable, crossed = gap_crossing(gap_m)+ print(f" MOSS gap {gap_m:.2f} m ({gap} cells): {passable} passable cell(s), "+ f"path {'CROSSES' if crossed else 'does not cross'}")+ assert passable == want_cells, f"{gap_m:.2f} m: {passable} passable cells, expected {want_cells}"+ assert crossed is must_pass, f"{gap_m:.2f} m: crossed={crossed}, expected {must_pass}"++ os.environ["MOSS_ROBOT_WIDTH_M"] = str(REF_ROBOT_WIDTH_M)+ os.environ["MOSS_PIVOT_DIAMETER_M"] = str(REF_PIVOT_DIAMETER_M)+ lethal, pivot = clearances()+ print(f" MOSS_ROBOT_WIDTH_M=0.50, MOSS_PIVOT_DIAMETER_M=0.78 -> ({lethal:.3f}, {pivot:.3f}) m")+ assert abs(lethal - REF_LETHAL_CLEARANCE_M) < 1e-9 and abs(pivot - REF_PIVOT_CLEARANCE_M) < 1e-9+ cells = np.zeros((41, 41), dtype=np.int8)+ cells[20, :14] = CostValues.OCCUPIED+ cells[20, 26:] = CostValues.OCCUPIED # a 0.60 m gap+ grid = make_grid(cells)+ assert np.array_equal(clearance_cost_map(grid).grid, clearance_cost_map(grid, **REF).grid), \+ "the env override must give the reference field exactly"+ print(" env override read at call time: default field == explicit reference field")+ finally:+ for k, v in saved.items():+ if v is None:+ os.environ.pop(k, None)+ else:+ os.environ[k] = v+++if __name__ == "__main__":+ for test in (+ test_constants_are_physically_ordered,+ test_prefers_the_wide_corridor,+ test_the_penalty_is_what_chooses_the_wide_corridor,+ test_keeps_away_from_a_lone_wall,+ test_still_takes_a_tight_gap_when_it_is_the_only_way,+ test_default_radii_follow_the_robot_profile,+ ):+ print(test.__name__)+ test()+ print("TEST PASSED")diff --git a/benches/bench_costmap2d_cold.py b/benches/bench_costmap2d_cold.pynew file mode 100644index 0000000..d97d2dd--- /dev/null+++ b/benches/bench_costmap2d_cold.py@@ -0,0 +1,478 @@+"""Cold bench for costmap2d: the scored grid and MossCostMap, written as numbers.++Known input -> known output, in metres and cells:+ 1. a table leg at (1.0, 0) seen from two places -> occupied; the floor along+ the ray (0.3..0.9 m) -> free; nothing else touched+ 2. a ramp: 50 hits from the SAME spot -> never occupied (one viewpoint)+ 3. a chair: occupied, then lidar revolutions see through it -> free+ 4. a ceiling of reinforcement: 30 viewpoints -> score 10, then bounded+ unlearning (never a wall for a day)+ 5. a parked rover: a wall raised to HIT_CAP by 12 viewpoints, then+ revolutions from the spot it is parked on and 0.5 m of creep -> still+ HIT_CAP, still occupancy 100 (the viewpoint rule caps the increase, it+ never lowers a score)+ 6. body_clear: the MOSS footprint (0.20 x 0.16 m half extents) clears 63+ cells at the origin; MOSS_BODY_CLEAR_M moves it at call time+ 7. the lidar tag: a float32 point at exactly the profile's lidar height+ (0.20 m) is a lidar return; 0.37 m, 0.2005 m and the floor are not;+ MOSS_LIDAR_HEIGHT_M moves the tag at call time+ 8. the mission gate: 20 revolutions and a passer-by on a PARKED rover ->+ 0 cells; 6 cm of displacement -> the map opens and the room's wall at+ 2.5 m is mapped; "exploration complete" and "exploration stopped" ->+ the map keeps writing (lidar is MOSS's only obstacle source)+ 9. the explore order: parked revolutions write nothing until explore_cmd,+ then the same parked revolutions carve free floor up to the wall+ 10. what goes out: one OccupancyGrid every 5 revolutions on global_costmap,+ 0.05 m cells, frame `world`, the wall at 2.5 m = 100 in the grid+ 11. the mission signals land on the topics the rest of the stack uses: one+ Bool class for explore_cmd / explore_done / stop_explore_cmd across+ costmap2d, explorer2 and tools/explore_ctl.py+ 12. the cost of one revolution (400 rays, 0.5-12 m) stays under 40 ms++Needs dimOS (the messages are real).++Run: PYTHONPATH=. .venv/bin/python benches/bench_costmap2d_cold.py+"""++import asyncio+import importlib.util+import math+import os+import sys+import time+import traceback+from pathlib import Path++sys.path.insert(0, str(Path(__file__).resolve().parent.parent))++import numpy as np # noqa: E402++# MOSS's own numbers, whatever the shell exported+for _var in [v for v in os.environ if v.startswith("MOSS_")]:+ del os.environ[_var]++from dimos_lcm.std_msgs import Bool # noqa: E402++from moss_dimos import costmap2d as cmod # noqa: E402+from moss_dimos.costmap2d import ( # noqa: E402+ FREE_FLOOR, HIT_CAP, MISSION_START_M, OCCUPIED_AT, PUBLISH_EVERY, RESOLUTION_M,+ MossCostMap, ScoredGrid, lidar_returns,+)+from moss_dimos.robot_profile import MOSS # noqa: E402++ROOT = Path(__file__).resolve().parent.parent+LEG = np.array([[1.0, 0.0]])+LIDAR_Z = MOSS.lidar_height_m # 0.20 m: where lidar_odometry lifts every return+++def fresh() -> ScoredGrid:+ return ScoredGrid(span_m=6.0)+++def _cloud(pts: np.ndarray):+ """A world cloud on the wire, float32 like the real one."""+ from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2+ return PointCloud2.from_numpy(pts.astype(np.float32), frame_id="world", timestamp=time.time())+++def _lidar_cloud(xy: np.ndarray, z: float = LIDAR_Z) -> np.ndarray:+ """(x, y) hits lifted to the lidar's own tag height (see lidar_returns)."""+ return np.column_stack([xy, np.full(len(xy), np.float32(z))])+++def _odom(x: float, y: float, yaw: float = 0.0):+ """A pose on the `odom` stream."""+ from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped+ p = PoseStamped(frame_id="world", ts=time.time())+ p.position.x, p.position.y, p.position.z = x, y, 0.0+ p.orientation.w, p.orientation.z = math.cos(yaw / 2), math.sin(yaw / 2)+ return p+++class Sink:+ def __init__(self):+ self.msgs = []++ def publish(self, msg):+ self.msgs.append(msg)+++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+++def costmap() -> MossCostMap:+ """A real MossCostMap; its Out is a Sink so the bench reads what it publishes."""+ m = MossCostMap()+ m.global_costmap = Sink()+ MODULES.append(m)+ return m+++MODULES: list = []+ANG = np.radians(np.arange(0, 360, 2.0))+ROOM = np.stack([2.5 * np.cos(ANG), 2.5 * np.sin(ANG)], 1) # a 5 m round room+++def test_leg_needs_two_viewpoints_and_ray_is_free() -> None:+ """An obstacle is REAL when seen from two viewpoints: a passer-by hammered+ from one parked spot must never become a wall."""+ g = fresh()+ for _ in range(10):+ g.lidar_revolution(LEG, (0.0, 0.0))+ assert g.value_at(1.0, 0.0) == 0, "any number of hits from ONE spot: not a wall"+ g.lidar_revolution(LEG, (0.0, 0.15)) # second viewpoint (>= NEW_VIEWPOINT_M)+ assert g.value_at(1.0, 0.0) == 100, "one hit from a SECOND viewpoint: obstacle"+ for x in (0.3, 0.5, 0.7, 0.9):+ assert g.value_at(x, 0.0) == 0, f"floor along the ray at {x} m must be free"+ assert g.value_at(1.3, 0.0) == -1, "behind the leg: never seen"+ print(" 10 hits one spot -> free; +1 hit second viewpoint -> occupied; ray -> free")+++def test_ramp_hit_from_one_spot_never_becomes_a_wall() -> None:+ g = fresh()+ for _ in range(50):+ g.lidar_revolution(LEG, (0.0, 0.0))+ gx, gy = g.cell(np.array([1.0]), np.array([0.0]))+ assert g.value_at(1.0, 0.0) == 0 and g.lidar[gy[0], gx[0]] == 1, "same spot forever: capped BELOW occupied"+ g.lidar_revolution(LEG, (0.0, 0.15))+ assert g.value_at(1.0, 0.0) == 100, "the second viewpoint makes it a wall"+ n = 0+ while g.value_at(1.0, 0.0) == 100:+ g.lidar_revolution(np.array([[3.0, 0.0]]), (0.0, 0.0)); n += 1+ assert n <= 4, n+ print(f" 50 hits from the same spot -> score 2, forgotten after {n} clear revolutions (never a wall)")+++def test_chair_moved_is_forgotten_by_lidar_rays() -> None:+ g = fresh()+ for i in range(3):+ g.lidar_revolution(LEG, (0.0, 0.15 * i))+ assert g.value_at(1.0, 0.0) == 100+ far = np.array([[3.0, 0.0]]) # the chair is gone: the ray now reaches the wall at 3 m+ n = 0+ while g.value_at(1.0, 0.0) == 100:+ g.lidar_revolution(far, (0.0, 0.0)); n += 1+ assert n < 20+ assert g.value_at(1.0, 0.0) == 0+ print(f" chair (score 3) -> free after {n} revolutions seeing through it")+++def test_reinforcement_is_capped_and_unlearning_bounded() -> None:+ g = fresh()+ for i in range(30):+ g.lidar_revolution(LEG, (0.0, 0.15 * i))+ gx, gy = g.cell(np.array([1.0]), np.array([0.0]))+ assert g.lidar[gy[0], gx[0]] == HIT_CAP+ n = 0+ while g.value_at(1.0, 0.0) == 100:+ g.lidar_revolution(np.array([[3.0, 0.0]]), (0.0, 0.0)); n += 1+ assert n <= 2 * HIT_CAP and g.lidar[gy[0], gx[0]] >= FREE_FLOOR+ print(f" 30 viewpoints -> score capped at {HIT_CAP}; forgotten after {n} clear revolutions (bounded)")+++def test_parked_rover_never_erases_an_established_wall() -> None:+ """The viewpoint rule caps the INCREASE, not the value: a hit from a spot+ the cell already knows adds nothing and takes nothing away. Capping the+ value would collapse a wall built by 12 approach viewpoints to 1 on the+ next revolution taken from a parked rover."""+ g = fresh()+ for i in range(12): # approach: 12 viewpoints, 0.15 m apart+ g.lidar_revolution(LEG, (0.0, 0.15 * i))+ gx, gy = g.cell(np.array([1.0]), np.array([0.0]))+ assert g.lidar[gy[0], gx[0]] == HIT_CAP and g.value_at(1.0, 0.0) == 100, "12 viewpoints -> a wall"+ parked = (0.0, 0.15 * 11) # the rover stops where it last looked from+ g.lidar_revolution(LEG, parked)+ assert g.lidar[gy[0], gx[0]] == HIT_CAP, "a same-viewpoint hit adds nothing - and takes nothing"+ assert g.value_at(1.0, 0.0) == 100, "the wall in front of a parked rover stays a wall"+ # driving slowly is the same case: 0.2 m/s at 10 Hz = 0.02 m per+ # revolution, so 4 revolutions out of 5 come from a viewpoint the cell knows+ for k in range(1, 26): # 0.5 m of creep+ g.lidar_revolution(LEG, (0.02 * k, parked[1]))+ assert g.value_at(1.0, 0.0) == 100, f"wall published FREE while creeping, revolution {k}"+ assert g.lidar[gy[0], gx[0]] == HIT_CAP+ print(f" wall at 1.0 m, 12 viewpoints -> score {HIT_CAP}; parked + 1 revolution and 0.5 m of "+ f"creep -> still {HIT_CAP}, occupancy 100")+++def test_body_clear_is_the_moss_footprint() -> None:+ """body_clear reads the profile at call time: MOSS 0.20 x 0.16 m half+ extents -> 9 x 7 cells of 5 cm at the origin (63), every one free and seen."""+ g = fresh()+ gx, gy = g.cell(np.array([0.10]), np.array([0.05]))+ g.lidar[gy[0], gx[0]] = HIT_CAP # a ghost under the body+ g.body_clear((0.0, 0.0, 0.0))+ ys, xs = np.nonzero(g.seen)+ x0, x1 = g.ox + xs.min() * g.res, g.ox + (xs.max() + 1) * g.res+ y0, y1 = g.oy + ys.min() * g.res, g.oy + (ys.max() + 1) * g.res+ assert int(g.seen.sum()) == 63, int(g.seen.sum())+ assert abs(x0 + 0.25) < 1e-9 and abs(x1 - 0.20) < 1e-9 and abs(y0 + 0.20) < 1e-9 and abs(y1 - 0.15) < 1e-9, (x0, x1, y0, y1)+ assert g.lidar[gy[0], gx[0]] == FREE_FLOOR and g.value_at(0.10, 0.05) == 0, "the ghost under the body is cleared"+ os.environ["MOSS_BODY_CLEAR_M"] = "0.30 0.20"+ try:+ h = fresh()+ h.body_clear((0.0, 0.0, 0.0))+ wide = int(h.seen.sum())+ finally:+ del os.environ["MOSS_BODY_CLEAR_M"]+ assert wide == 117, wide+ r = fresh()+ r.body_clear((1.0, 0.0, math.pi / 2)) # turned 90 deg: the footprint turns too+ ys, xs = np.nonzero(r.seen)+ assert int(r.seen.sum()) == 63 and xs.max() - xs.min() + 1 == 7 and ys.max() - ys.min() + 1 == 9, \+ (int(r.seen.sum()), xs.max() - xs.min() + 1, ys.max() - ys.min() + 1)+ assert abs(r.ox + xs.min() * r.res - 0.85) < 1e-9 and abs(r.oy + ys.min() * r.res + 0.25) < 1e-9+ print(f" MOSS footprint: 63 cells over [-0.25, 0.20) x [-0.20, 0.15) m; MOSS_BODY_CLEAR_M='0.30 0.20' "+ f"-> {wide} cells; turned 90 deg at (1, 0) -> 7 x 9 cells over [0.85, 1.20) x [-0.25, 0.20) m")+++def test_the_lidar_tag_is_one_exact_height() -> None:+ """lidar_odometry lifts every return to exactly lidar_height_m; anything+ else on the cloud (a depth camera's points, at their true height) is not+ a lidar return, even millimetres away from the scan plane."""+ cloud = np.array([[1.0, 0.0, np.float32(0.20)], [2.0, 0.0, 0.2005], [3.0, 0.0, 0.37],+ [4.0, 0.0, 0.0]], dtype=np.float32).astype(np.float64)+ assert list(lidar_returns(cloud)) == [True, False, False, False], list(lidar_returns(cloud))+ os.environ["MOSS_LIDAR_HEIGHT_M"] = "0.37"+ try:+ moved = list(lidar_returns(cloud))+ finally:+ del os.environ["MOSS_LIDAR_HEIGHT_M"]+ assert moved == [False, False, True, False], moved+ # through the module: one cloud with 180 wall returns at 0.20 m and a+ # point 2 mm above the scan plane - only the wall is mapped+ m = costmap()+ asyncio.run(m.handle_odom(_odom(0.0, 0.0)))+ asyncio.run(m.handle_explore_cmd(Bool(data=True)))+ stray = np.array([[1.0, 0.5, LIDAR_Z + 0.002]])+ for y in (0.0, 0.15):+ asyncio.run(m.handle_odom(_odom(0.0, y)))+ asyncio.run(m.handle_lidar(_cloud(np.vstack([_lidar_cloud(ROOM), stray]))))+ assert m._grid.value_at(2.5, 0.0) == 100, "the wall is mapped"+ assert m._grid.value_at(1.0, 0.5) != 100, "the stray point is not"+ n = m._revolutions+ asyncio.run(m.handle_lidar(_cloud(stray)))+ assert m._revolutions == n, "a cloud with no lidar return is not a revolution"+ print(" float32 0.20 m -> lidar; 0.2005 / 0.37 / 0.0 -> not; MOSS_LIDAR_HEIGHT_M=0.37 moves the tag; "+ "a point 2 mm off the plane never reaches the map")+++def test_the_map_writes_only_during_the_mission() -> None:+ """A rover parked with a spinning lidar must not write: it thickens the+ obstacles around its one viewpoint and records whoever walks past.++ Known input -> known output, in metres and in cells: revolutions injected+ while parked -> 0 cells; 6 cm of displacement -> cells written; the end of+ the exploration -> the map keeps writing."""+ spy, real = LogSpy(), cmod.logger+ cmod.logger = spy+ try:+ m = costmap()+ assert m._mission_frozen and m._write_frozen(), "the pre-mission gate is closed by default"+ passer_by = np.array([[0.8, 0.0]]) # somebody standing 0.8 m away+ for _ in range(20): # parked at the first pose+ asyncio.run(m.handle_odom(_odom(0.0, 0.0)))+ asyncio.run(m.handle_lidar(_cloud(_lidar_cloud(np.vstack([ROOM, passer_by])))))+ assert m._grid is not None, "the grid is built on the first odom"+ assert (m._grid.ox, m._grid.oy) == (-12.0, -12.0), "24 m grid centred on the first pose"+ parked_cells = int(m._grid.seen.sum())+ assert parked_cells == 0, f"a parked rover wrote {parked_cells} cells before departing"+ assert m._grid.value_at(0.8, 0.0) == -1, "the passer-by is not in the map"+ assert not m.global_costmap.msgs, "nothing is published while the gate is closed"++ asyncio.run(m.handle_odom(_odom(0.04, 0.0)))+ assert m._mission_frozen, "4 cm (< 5 cm) does not open the map"+ asyncio.run(m.handle_odom(_odom(0.06, 0.0)))+ assert not m._mission_frozen, "6 cm from the first pose (>= 5 cm) opens the map"+ assert any(line.startswith("costmap: the rover left its boot pose (6 cm)") for line in spy.lines), spy.lines+ for k in range(6): # driving on, two viewpoints+ asyncio.run(m.handle_odom(_odom(0.06 + 0.04 * k, 0.0)))+ asyncio.run(m.handle_lidar(_cloud(_lidar_cloud(ROOM))))+ driving_cells = int(m._grid.seen.sum())+ assert driving_cells > 0, "on the mission, the revolutions land in the map"+ assert m._grid.value_at(2.5, 0.0) == 100, "the wall of the room is mapped while driving"++ asyncio.run(m.handle_explore_done(Bool(data=True)))+ asyncio.run(m.handle_stop_explore_cmd(Bool(data=True)))+ assert not m._write_frozen(), "the end of the exploration does not close the map"+ assert "costmap: exploration complete - map writing continues" in spy.lines+ assert "costmap: exploration stopped - map writing continues" in spy.lines+ for k in range(6):+ asyncio.run(m.handle_odom(_odom(1.0, 0.3 * k)))+ asyncio.run(m.handle_lidar(_cloud(_lidar_cloud(ROOM + [1.0, 0.0]))))+ after = int(m._grid.seen.sum())+ assert after > driving_cells, "the map keeps writing after the explorer stopped"+ assert m._grid.value_at(3.5, 0.0) == 100, "a wall seen after the stop is mapped"++ n = costmap()+ asyncio.run(n.handle_odom(_odom(0.0, 0.0)))+ asyncio.run(n.handle_explore_done(Bool(data=False)))+ asyncio.run(n.handle_stop_explore_cmd(Bool(data=False)))+ asyncio.run(n.handle_explore_cmd(Bool(data=False)))+ assert n._mission_frozen, "only data=True counts: Bool(False) opens nothing"+ asyncio.run(n.handle_stop_explore_cmd(Bool(data=True)))+ asyncio.run(n.handle_odom(_odom(1.0, 0.0)))+ assert not n._write_frozen() and int(n._grid.seen.sum()) > 0, (+ "a stop BEFORE the map opened: the 5 cm gate still opens it, and body_clear writes")+ finally:+ cmod.logger = real+ print(f" parked: 20 revolutions + a passer-by -> {parked_cells} cells; after 6 cm -> {driving_cells} cells "+ f"and the wall at 2.5 m mapped; after 'exploration complete' + 'stopped' -> {after} cells, still writing")+++def test_explore_order_opens_the_map_before_any_motion() -> None:+ """Displacement-only opening deadlocks a fresh map (no cell -> no frontier+ -> no goal -> no motion -> never 5 cm). The explore order opens it.++ Known input -> known output: parked revolutions -> 0 cells; explore_cmd ->+ the SAME parked revolutions carve free floor up to the wall at 2.5 m."""+ spy, real = LogSpy(), cmod.logger+ cmod.logger = spy+ try:+ m = costmap()+ for _ in range(5):+ asyncio.run(m.handle_odom(_odom(0.0, 0.0)))+ asyncio.run(m.handle_lidar(_cloud(_lidar_cloud(ROOM))))+ assert int(m._grid.seen.sum()) == 0, "still closed before the order"+ asyncio.run(m.handle_explore_cmd(Bool(data=True)))+ assert not m._mission_frozen, "the explore ORDER opens the map, no motion needed"+ assert spy.lines.count("costmap: explore_cmd received - map writing OPEN for the mission") == 1, spy.lines+ for _ in range(3):+ asyncio.run(m.handle_odom(_odom(0.0, 0.0)))+ asyncio.run(m.handle_lidar(_cloud(_lidar_cloud(ROOM))))+ assert int(m._grid.seen.sum()) > 0, "the parked revolutions now land in the map"+ # One viewpoint never makes a wall - what the explorer NEEDS is free+ # floor along the rays: its first frontier before one wheel turn.+ assert m._grid.value_at(1.25, 0.0) == 0, "the ray to the wall carved free floor from the start pose"+ assert m._grid.value_at(2.5, 0.0) == 0, "one viewpoint: the wall cell is seen, not yet an obstacle"+ asyncio.run(m.handle_explore_done(Bool(data=True)))+ asyncio.run(m.handle_explore_cmd(Bool(data=True)))+ assert not m._mission_frozen, "a new order after the end changes nothing: still open"+ assert spy.lines.count("costmap: explore_cmd received - map writing OPEN for the mission") == 1, \+ "the open line is said once"+ finally:+ cmod.logger = real+ print(" parked: 0 cells; explore_cmd: free floor carved to the wall before one wheel turn; "+ "'costmap: explore_cmd received' logged once")+++def test_what_goes_out_on_global_costmap() -> None:+ """One cropped OccupancyGrid every PUBLISH_EVERY revolutions."""+ from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid+ m = costmap()+ asyncio.run(m.handle_odom(_odom(0.0, 0.0)))+ asyncio.run(m.handle_explore_cmd(Bool(data=True)))+ for k in range(2 * PUBLISH_EVERY):+ asyncio.run(m.handle_odom(_odom(0.0, 0.15 * (k % 2))))+ asyncio.run(m.handle_lidar(_cloud(_lidar_cloud(ROOM))))+ out = m.global_costmap.msgs+ assert len(out) == 2, f"{2 * PUBLISH_EVERY} revolutions -> {len(out)} grids (one every {PUBLISH_EVERY})"+ og = out[-1]+ assert isinstance(og, OccupancyGrid) and og.frame_id == "world" and abs(og.resolution - RESOLUTION_M) < 1e-9+ grid = np.asarray(og.grid)+ ox, oy = og.origin.position.x, og.origin.position.y+ i, j = int(math.floor((0.0 - oy) / RESOLUTION_M)), int(math.floor((2.5 - ox) / RESOLUTION_M))+ assert grid[i, j] == 100, f"the wall at (2.5, 0) is 100 in the published grid ({grid[i, j]})"+ i0, j0 = int(math.floor((0.0 - oy) / RESOLUTION_M)), int(math.floor((1.0 - ox) / RESOLUTION_M))+ assert grid[i0, j0] == 0, "the floor at (1.0, 0) is free"+ assert ox <= -2.5 - 20 * RESOLUTION_M + 1e-9 and grid.shape[0] < 480, \+ f"cropped to the seen area + 20 cells of margin ({grid.shape}, origin ({ox:+.2f}, {oy:+.2f}))"+ print(f" {2 * PUBLISH_EVERY} revolutions -> 2 grids; {grid.shape[1]}x{grid.shape[0]} cells at "+ f"({ox:+.2f}, {oy:+.2f}), wall at 2.5 m = 100, floor at 1.0 m = 0")+++def test_the_mission_signals_cross_the_process_boundary() -> None:+ """The mission streams must land on the topics the rest of the stack uses.++ dimOS gives a stream the canonical `/<name>` topic only while (name, type)+ is unique across the blueprint; the same name with a DIFFERENT message+ class gives every module on it a random topic instead. So: one Bool class+ (dimos_lcm.std_msgs.Bool) in costmap2d, explorer2 and explore_ctl.py."""+ from dimos.core.coordination.blueprints import BlueprintAtom+ from dimos.core.transport_factory import transport_topic+ from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped+ from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid+ from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2+ from dimos.protocol.pubsub.impl.lcmpubsub import Topic as LCMTopic++ cm = {(s.name, s.type): s.direction for s in BlueprintAtom.create(MossCostMap, {}).streams}+ want = {("lidar", PointCloud2): "in", ("odom", PoseStamped): "in", ("explore_cmd", Bool): "in",+ ("explore_done", Bool): "in", ("stop_explore_cmd", Bool): "in",+ ("global_costmap", OccupancyGrid): "out"}+ assert cm == want, f"stream table: extra {set(cm) - set(want)}, missing {set(want) - set(cm)}"+ assert MossCostMap.name == "mosscostmap", MossCostMap.name+ assert {"start", "stop"} <= set(MossCostMap.rpcs)++ from moss_dimos import explorer2 as e2+ ex = {(s.name, s.type): s.direction for s in BlueprintAtom.create(e2.Explorer2, {}).streams}+ assert ex[("explore_done", Bool)] == "out", "explorer2 is the producer of the completion"+ assert ex[("stop_explore_cmd", Bool)] == "in", "and a consumer of the stop, like us"+ for name in ("explore_cmd", "explore_done", "stop_explore_cmd"):+ types_ = {t for (n, t) in list(cm) + list(ex) if n == name}+ assert types_ == {Bool}, f"{name} declared with {len(types_)} classes -> random topics: {types_}"++ want_topic = str(LCMTopic("/stop_explore_cmd", Bool))+ assert transport_topic("/stop_explore_cmd") == "/stop_explore_cmd"+ assert want_topic == "/stop_explore_cmd#std_msgs.Bool", want_topic+ spec = importlib.util.spec_from_file_location("explore_ctl", ROOT / "tools" / "explore_ctl.py")+ ctl = importlib.util.module_from_spec(spec)+ spec.loader.exec_module(ctl)+ saved = {v: os.environ.pop(v, None) for v in ("DIMOS_TRANSPORT", "TRANSPORT")} # the default: LCM+ try:+ assert ctl.topic("stop") == want_topic, f"explore_ctl stop -> {ctl.topic('stop')} != {want_topic}"+ assert ctl.topic("start") == str(LCMTopic("/explore_cmd", Bool)), ctl.topic("start")+ finally:+ for v, val in saved.items():+ if val is not None:+ os.environ[v] = val+ print(f" costmap: 5 Ins + global_costmap, one Bool class; explorer2 agrees; "+ f"stop lands on {want_topic} (explore_ctl.py publishes there)")+++def test_revolution_cost() -> None:+ ang = np.radians(np.arange(0, 360, 0.9)); r = np.random.default_rng(0).uniform(0.5, 12.0, len(ang))+ hits = np.stack([r * np.cos(ang), r * np.sin(ang)], 1)+ g = ScoredGrid(span_m=24.0)+ t0 = time.perf_counter()+ for i in range(10):+ g.lidar_revolution(hits, (0.01 * i, 0.0))+ ms = (time.perf_counter() - t0) / 10 * 1000+ assert ms < 40, ms+ print(f" one revolution (400 rays, 0.5-12 m, carving every other one) = {ms:.0f} ms")+++TESTS = (test_leg_needs_two_viewpoints_and_ray_is_free, test_ramp_hit_from_one_spot_never_becomes_a_wall,+ test_chair_moved_is_forgotten_by_lidar_rays, test_reinforcement_is_capped_and_unlearning_bounded,+ test_parked_rover_never_erases_an_established_wall, test_body_clear_is_the_moss_footprint,+ test_the_lidar_tag_is_one_exact_height, test_the_map_writes_only_during_the_mission,+ test_explore_order_opens_the_map_before_any_motion, test_what_goes_out_on_global_costmap,+ test_the_mission_signals_cross_the_process_boundary, test_revolution_cost)++if __name__ == "__main__":+ OK = KO = 0+ for t in TESTS:+ print(t.__name__)+ try:+ t()+ OK += 1+ except Exception: # noqa: BLE001 - one KO line per test, the bench goes on+ KO += 1+ print(" KO " + traceback.format_exc().strip().splitlines()[-1])+ traceback.print_exc()+ # Open3D (behind PointCloud2) reports every cloud still alive at+ # interpreter exit as a leak and turns the exit status to 1: drop them first.+ for m in MODULES:+ m.global_costmap.msgs.clear()+ MODULES.clear()+ import gc+ gc.collect()+ print()+ 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/benches/bench_explore_ctl_cold.py b/benches/bench_explore_ctl_cold.pynew file mode 100644index 0000000..4d20642--- /dev/null+++ b/benches/bench_explore_ctl_cold.py@@ -0,0 +1,193 @@+"""Cold bench for tools/explore_ctl.py: exploration start/stop must land on the run's LCM bus.++Known input -> known output, here in bytes on a named channel (what a bus+actually carries). Groups:++ A. channels - the exact LCM channel for start and stop; the bus read in+ dimOS's order: DIMOS_TRANSPORT, then TRANSPORT (neither = lcm)+ B. payload - Bool(True) is 9 known bytes (8-byte LCM fingerprint of+ std_msgs.Bool + 0x01), and decodes back to True+ C. the stack - those channels are the ones dimOS's own factory derives for the+ streams the explorer subscribes to, and the bus explore_ctl reads+ is the one dimOS's GlobalConfig picks for every pair of+ DIMOS_TRANSPORT / TRANSPORT (needs dimOS, skipped without)+ D. lcm - a real publish lands on /explore_cmd#std_msgs.Bool byte for byte+ E. refusal - any other bus: publish raises naming the variable it read and+ the fix, nothing reaches the LCM bus, and the CLI exits non-zero+ (DIMOS_TRANSPORT=zenoh wins over TRANSPORT=lcm, as in dimOS)++End to end into a live explorer is a check on the robot.++Run: PYTHONDONTWRITEBYTECODE=1 PYTHONPATH=. .venv/bin/python benches/bench_explore_ctl_cold.py+"""++import os+import subprocess+import sys+import time+from pathlib import Path++ROOT = Path(__file__).resolve().parent.parent+sys.path.insert(0, str(ROOT))+sys.path.insert(0, str(ROOT / "tools"))++import lcm # noqa: E402+from dimos_lcm.std_msgs import Bool # noqa: E402++import explore_ctl # noqa: E402++try:+ from dimos.core.global_config import GlobalConfig, global_config+ from dimos.core.transport_factory import transport_topic+ from dimos.protocol.pubsub.impl.lcmpubsub import Topic as LCMTopic+ HAVE_DIMOS = True+except Exception: # noqa: BLE001 - no dimOS on a laptop+ HAVE_DIMOS = False++TRUE_BYTES = b"\x1e\xbe\xf0jP\x95\x0e>\x01" # std_msgs.Bool fingerprint + data=True+LCM_START = "/explore_cmd#std_msgs.Bool"+LCM_STOP = "/stop_explore_cmd#std_msgs.Bool"+OK = 0+KO = 0+++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+++def set_bus(dimos_transport=None, transport=None):+ """Both variables dimOS reads, set or unset together: every check names the pair."""+ for name, value in (("DIMOS_TRANSPORT", dimos_transport), ("TRANSPORT", transport)):+ if value is None:+ os.environ.pop(name, None)+ else:+ os.environ[name] = value+++def listen(bus, heard, wait_s):+ deadline = time.monotonic() + wait_s+ while not heard and time.monotonic() < deadline:+ bus.handle_timeout(100)+++saved_bus = (os.environ.get("DIMOS_TRANSPORT"), os.environ.get("TRANSPORT"))++# (DIMOS_TRANSPORT, TRANSPORT) -> the bus dimOS runs on: the first one set wins+PAIRS = [((None, None), "lcm"), ((None, "lcm"), "lcm"), ((None, "zenoh"), "zenoh"),+ (("zenoh", None), "zenoh"), (("lcm", None), "lcm"), (("zenoh", "lcm"), "zenoh"),+ (("lcm", "zenoh"), "lcm")]++try:+ print("A. channels")+ set_bus(None, None)+ check("neither DIMOS_TRANSPORT nor TRANSPORT set -> lcm", explore_ctl.transport() == "lcm",+ explore_ctl.transport())+ check(f"unset: start -> {LCM_START}", explore_ctl.topic("start") == LCM_START, explore_ctl.topic("start"))+ check(f"unset: stop -> {LCM_STOP}", explore_ctl.topic("stop") == LCM_STOP, explore_ctl.topic("stop"))+ set_bus(None, "lcm")+ check(f"lcm: start -> {LCM_START}", explore_ctl.topic("start") == LCM_START, explore_ctl.topic("start"))+ check(f"lcm: stop -> {LCM_STOP}", explore_ctl.topic("stop") == LCM_STOP, explore_ctl.topic("stop"))+ set_bus(None, "LCM")+ check("TRANSPORT=LCM -> lcm (read case-insensitively)", explore_ctl.transport() == "lcm",+ explore_ctl.transport())+ got = {}+ for (dimos_tr, tr), want in PAIRS:+ set_bus(dimos_tr, tr)+ got[(dimos_tr, tr)] = (explore_ctl.transport(), want)+ check("this process's environment, dimOS's order: DIMOS_TRANSPORT first, then TRANSPORT (7 pairs)",+ all(a == b for a, b in got.values()), str({k: v[0] for k, v in got.items()}))+ env = {"DIMOS_TRANSPORT": "zenoh", "TRANSPORT": "lcm"}+ set_bus(None, "lcm")+ check("an explicit environment (the cockpit passes a stack's): DIMOS_TRANSPORT=zenoh TRANSPORT=lcm -> "+ "('DIMOS_TRANSPORT', 'zenoh'), whatever this process has",+ explore_ctl.transport_setting(env) == ("DIMOS_TRANSPORT", "zenoh") and explore_ctl.transport(env) == "zenoh"+ and explore_ctl.transport() == "lcm", str(explore_ctl.transport_setting(env)))+ check("an explicit empty environment -> ('', 'lcm')", explore_ctl.transport_setting({}) == ("", "lcm"))+ check("generic channel: goal_request + geometry_msgs.PoseStamped -> /goal_request#geometry_msgs.PoseStamped",+ explore_ctl.channel("goal_request", "geometry_msgs.PoseStamped")+ == "/goal_request#geometry_msgs.PoseStamped")++ print("B. payload")+ payload = Bool(data=True).lcm_encode()+ check(f"Bool(True) -> {TRUE_BYTES!r}", payload == TRUE_BYTES, repr(payload))+ check("decodes back to data=True", Bool.lcm_decode(payload).data is True)++ print("C. the stack's own channels" if HAVE_DIMOS else "C. the stack's own channels - SKIPPED (no dimOS)")+ if HAVE_DIMOS:+ global_config.update(transport="lcm")+ stack_lcm = {c: str(LCMTopic(transport_topic(f"/{n}"), Bool))+ for c, n in explore_ctl.CHANNELS.items()}+ check("lcm channels match dimOS's factory",+ stack_lcm == {"start": LCM_START, "stop": LCM_STOP}, str(stack_lcm))+ both = {}+ for (dimos_tr, tr), want in PAIRS:+ set_bus(dimos_tr, tr)+ both[(dimos_tr, tr)] = (GlobalConfig().transport, explore_ctl.transport(), want)+ check("for every pair, explore_ctl's bus == the bus dimOS's GlobalConfig picks (7 pairs, "+ "DIMOS_TRANSPORT=zenoh TRANSPORT=lcm -> zenoh in both)",+ all(d == e == w for d, e, w in both.values()), str(both))++ print("D. lcm delivery")+ set_bus(None, "lcm")+ bus = lcm.LCM()+ heard = []+ bus.subscribe(LCM_START, lambda ch, data: heard.append((ch, data)))+ used = explore_ctl.publish("start")+ listen(bus, heard, 2.0)+ check(f"publish('start') says {LCM_START}", used == LCM_START, used)+ check("a subscriber on that channel got exactly the known bytes",+ heard == [(LCM_START, TRUE_BYTES)], str(heard))++ print("E. refusal on any other transport")+ heard_stop = []+ bus.subscribe(LCM_STOP, lambda ch, data: heard_stop.append((ch, data)))+ errors = {}+ for dimos_tr, tr in ((None, "zenoh"), ("zenoh", "lcm")):+ set_bus(dimos_tr, tr)+ try:+ explore_ctl.publish("stop")+ errors[(dimos_tr, tr)] = None+ except explore_ctl.TransportError as exc:+ errors[(dimos_tr, tr)] = str(exc)+ e1, e2 = errors[(None, "zenoh")], errors[("zenoh", "lcm")]+ check("TRANSPORT=zenoh: publish('stop') raises TransportError naming TRANSPORT=zenoh and the fix",+ e1 is not None and e1.startswith("TRANSPORT=zenoh:") and "lcm" in e1, str(e1))+ check("DIMOS_TRANSPORT=zenoh TRANSPORT=lcm: publish('stop') raises, naming DIMOS_TRANSPORT=zenoh "+ "(the variable dimOS obeys)",+ e2 is not None and e2.startswith("DIMOS_TRANSPORT=zenoh:") and "lcm" in e2, str(e2))+ listen(bus, heard_stop, 0.5)+ check("and nothing reached /stop_explore_cmd on the LCM bus", heard_stop == [], str(heard_stop))+ clean = {k: v for k, v in os.environ.items() if k not in ("DIMOS_TRANSPORT", "TRANSPORT")}+ for extra, named in (({"TRANSPORT": "zenoh"}, "TRANSPORT=zenoh"),+ ({"DIMOS_TRANSPORT": "zenoh", "TRANSPORT": "lcm"}, "DIMOS_TRANSPORT=zenoh")):+ env = dict(clean, PYTHONDONTWRITEBYTECODE="1", **extra)+ proc = subprocess.run([sys.executable, str(ROOT / "tools" / "explore_ctl.py"), "stop"],+ capture_output=True, text=True, env=env, timeout=30)+ check(f"CLI 'explore_ctl.py stop' under {' '.join(f'{k}={v}' for k, v in extra.items())} -> exit 1, "+ f"the reason on stderr naming {named}, nothing claimed",+ proc.returncode == 1 and "LCM only" in proc.stderr and f"explore_ctl: {named}:" in proc.stderr+ and "published" not in proc.stdout, f"rc {proc.returncode}, stderr {proc.stderr.strip()[:120]!r}")+ listen(bus, heard_stop, 0.5)+ check("and still nothing on /stop_explore_cmd", heard_stop == [], str(heard_stop))+ env = dict(clean, PYTHONDONTWRITEBYTECODE="1", TRANSPORT="lcm")+ proc = subprocess.run([sys.executable, str(ROOT / "tools" / "explore_ctl.py"), "stop"],+ capture_output=True, text=True, env=env, timeout=30)+ listen(bus, heard_stop, 2.0)+ check(f"CLI 'explore_ctl.py stop' under TRANSPORT=lcm -> exit 0, 'published {LCM_STOP} on lcm', known bytes",+ proc.returncode == 0 and proc.stdout.strip() == f"published {LCM_STOP} on lcm"+ and heard_stop == [(LCM_STOP, TRUE_BYTES)], f"rc {proc.returncode} {proc.stdout.strip()!r} {heard_stop}")+ proc = subprocess.run([sys.executable, str(ROOT / "tools" / "explore_ctl.py"), "dance"],+ capture_output=True, text=True, env=env, timeout=30)+ check("CLI with an unknown command -> exit 1 and the usage line",+ proc.returncode == 1 and "usage: explore_ctl.py start|stop" in proc.stderr, proc.stderr.strip())+finally:+ set_bus(*saved_bus)++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/benches/bench_explorer2_cold.py b/benches/bench_explorer2_cold.pynew file mode 100644index 0000000..4bdcc44--- /dev/null+++ b/benches/bench_explorer2_cold.py@@ -0,0 +1,767 @@+"""Cold bench for explorer2: known maps in, known decisions out.++Every check is a known input giving a known output in physical units+(metres, seconds, a named directive) - not "it returned something". Groups:++ A. frontier + goal placement - where the target lands on a plain room+ B. prefer-forward - two mirror-image clusters, the one ahead wins+ C. failed-target memory - an exclusion WAITS, never ends the run, and+ reopens on TRIGGERS (map changed / new+ viewpoint), never on a clock+ D. born cornered - the pocket signature gives ONE back-off+ E. blocked region - a block published as occupied is never targeted+ F. the end of a run - None exactly when there is nothing left+ H. revisits - fresh ground beats ground already worked+ I. shut in - pinched is not finished+ G. purity - the costmap, the pose and the rest of the+ state come back untouched, and the function+ is deterministic+ J. the loop around it - which goal a bare goal_reached is credited to++No robot and no LCM. A to I need numpy and scipy only (the geometry is built on+the default Tuning, a 0.50 m wide reference body). Only J touches the module+class, and it skips itself where dimOS is not installed.++Run from the repository root: python benches/bench_explorer2_cold.py+"""+import math+import sys+import threading+import time+from pathlib import Path++import numpy as np++ROOT = Path(__file__).resolve().parent.parent+sys.path.insert(0, str(ROOT))++# moss_dimos/__init__.py imports nothing, so this works with or without dimOS+import moss_dimos.explorer2 as _explorer2_module # noqa: E402+from moss_dimos.explorer2 import ( # noqa: E402+ DIRECTIVE_BACK_OFF, DIRECTIVE_FRONTIER, DIRECTIVE_WAIT, DEFAULT_TUNING,+ ExploreState, PoseStamped, Tuning, WAIT_REOPEN_POLLS, next_target,+)++FREE, UNKNOWN, OCCUPIED = 0, -1, 100+RES = 0.05++OK = 0+KO = 0+++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+++# --- the smallest thing that looks like a costmap --------------------------++class Grid:+ """Everything next_target reads off an OccupancyGrid, and nothing else."""++ def __init__(self, grid, res=RES, ox=0.0, oy=0.0, ts=0.0):+ self.grid = grid+ self.resolution = res+ self.frame_id = "world"+ self.ts = ts+ self.origin = type("O", (), {+ "position": type("P", (), {"x": ox, "y": oy, "z": 0.0})()})()+++def pose(x, y, yaw=0.0):+ p = PoseStamped(ts=0.0, frame_id="world")+ p.position.x, p.position.y = x, y+ p.orientation.z, p.orientation.w = math.sin(yaw / 2), math.cos(yaw / 2)+ return p+++def cell(x, y, ox=0.0, oy=0.0, res=RES):+ return int((y - oy) / res), int((x - ox) / res)+++def room(n=140, x0=30, x1=110, y0=30, y1=110, walls=True):+ """A square of free floor in the middle of unknown, walled all round.++ n x n cells at 5 cm; with the defaults that is 7 x 7 m of grid holding a+ 4 x 4 m room. Openings are punched into the wall afterwards.+ """+ g = np.full((n, n), UNKNOWN, dtype=np.int8)+ g[y0:y1, x0:x1] = FREE+ if walls:+ g[y0 - 1:y1 + 1, x0 - 1] = OCCUPIED+ g[y0 - 1:y1 + 1, x1] = OCCUPIED+ g[y0 - 1, x0 - 1:x1 + 1] = OCCUPIED+ g[y1, x0 - 1:x1 + 1] = OCCUPIED+ return g+++def door(g, side, centre, width_cells, x0=30, x1=110, y0=30, y1=110):+ """Punch an opening of `width_cells` in one wall, centred on `centre`."""+ half = width_cells // 2+ if side == "east":+ g[centre - half:centre + half, x1] = UNKNOWN+ elif side == "west":+ g[centre - half:centre + half, x0 - 1] = UNKNOWN+ elif side == "north":+ g[y1, centre - half:centre + half] = UNKNOWN+ elif side == "south":+ g[y0 - 1, centre - half:centre + half] = UNKNOWN+ return g+++def w_of(cell_index):+ """Cell index -> the world coordinate of its centre, origin at 0."""+ return (cell_index + 0.5) * RES+++# ===========================================================================+print("A. a room with one door: the target stands on free floor, at the door")++g = door(room(), "east", 70, 24)+cm = Grid(g)+here = pose(w_of(70), w_of(70), 0.0) # middle of the room, facing +x+st = ExploreState()+target = next_target(cm, here, st, now=100.0)++check("a target comes back", target is not None)+assert target is not None+check("directive is 'frontier'", target.directive == DIRECTIVE_FRONTIER, target.directive)+gy, gx = cell(target.position.x, target.position.y)+check("the target stands on FREE floor, not in the unknown",+ g[gy, gx] == FREE and not target.on_frontier, f"grid value {g[gy, gx]}")+check("the target is at the door, not at the centroid of the room",+ target.position.x > w_of(100), f"x = {target.position.x:.2f} m")+check("the look-at point is in the unknown beyond the door",+ g[cell(*target.look_at_xy)] == UNKNOWN)+nearest_wall = min(math.hypot(target.position.x - w_of(cx), target.position.y - w_of(cy))+ for cy, cx in zip(*np.nonzero(g == OCCUPIED)))+check("it is at least a body radius from any wall",+ nearest_wall >= DEFAULT_TUNING.lethal_clearance_m,+ f"{nearest_wall:.2f} m >= {DEFAULT_TUNING.lethal_clearance_m} m")+check("path cost is the real walking distance, not a straight line",+ target.path_cost_m >= abs(target.position.x - here.position.x) - 1e-6,+ f"{target.path_cost_m:.2f} m")+check("state: one target issued, one visited, one observed pose",+ (st.targets_issued, len(st.visited), len(st.observed)) == (1, 1, 1),+ f"{st.targets_issued}, {len(st.visited)}, {len(st.observed)}")+++# ===========================================================================+print("B. prefer-forward: two mirror-image doors, the one ahead wins")++g = door(door(room(), "east", 70, 24), "west", 70, 24)+cm = Grid(g)+centre = w_of(70)++east = next_target(cm, pose(centre, centre, 0.0), ExploreState(), now=100.0)+west = next_target(cm, pose(centre, centre, math.pi), ExploreState(), now=100.0)+check("facing +x picks the east door", east is not None and east.position.x > centre,+ f"x = {east.position.x:.2f}" if east else "None")+check("facing -x picks the west door", west is not None and west.position.x < centre,+ f"x = {west.position.x:.2f}" if west else "None")+check("the two doors really are the same cluster size and the same distance",+ east is not None and west is not None+ and east.info_cells == west.info_cells+ and abs(east.path_cost_m - west.path_cost_m) < 0.11,+ f"{east.info_cells} vs {west.info_cells} cells, "+ f"{east.path_cost_m:.2f} vs {west.path_cost_m:.2f} m" if east and west else "")+sideways = next_target(cm, pose(centre, centre, math.pi / 2), ExploreState(), now=100.0)+check("facing sideways, neither bonus applies and one is still chosen",+ sideways is not None and sideways.directive == DIRECTIVE_FRONTIER)+++# ===========================================================================+print("C. failed-target memory: an exclusion WAITS, reopens on triggers, never a clock")++g = door(room(), "east", 70, 24)+cm = Grid(g)+here = pose(centre, centre, 0.0)++probe = next_target(cm, here, ExploreState(), now=0.0)+assert probe is not None+gx, gy = probe.position.x, probe.position.y++st = ExploreState()+st.note_failed(gx, gy, (centre, centre), cm)+waited = next_target(cm, here, st, now=10.0)+check("the only cluster is excluded -> NOT None", waited is not None)+assert waited is not None+check("directive is 'wait'", waited.directive == DIRECTIVE_WAIT, waited.directive)+check("the rover is told to stay where it is",+ (waited.position.x, waited.position.y) == (here.position.x, here.position.y))+check("the cluster is still counted as a frontier, not as absence",+ waited.n_clusters == 1 and waited.n_excluded == 1,+ f"{waited.n_clusters} clusters, {waited.n_excluded} excluded")++much_later = next_target(cm, here, st, now=1e9)+check("NO clock reopens it: same map, same pose, a billion seconds later -> still wait",+ much_later is not None and much_later.directive == DIRECTIVE_WAIT,+ much_later.directive if much_later else "None")++# starvation escape: after WAIT_REOPEN_POLLS silent asks the oldest entry reopens+starved = ExploreState()+starved.note_failed(gx, gy, (centre, centre), cm)+out = None+for _ in range(WAIT_REOPEN_POLLS + 2):+ out = next_target(cm, here, starved, now=10.0)+ if out is not None and out.directive == DIRECTIVE_FRONTIER:+ break+check(f"starved {WAIT_REOPEN_POLLS} asks -> the oldest failed goal reopens and is targeted",+ out is not None and out.directive == DIRECTIVE_FRONTIER and starved.failed == [],+ out.directive if out else "None")++# trigger 1: the rover stands a viewpoint away from where it failed+moved_st = ExploreState()+moved_st.note_failed(gx, gy, (centre, centre), cm)+moved = next_target(cm, pose(centre - 1.2, centre, 0.0), moved_st, now=10.0)+check("the rover 1.2 m from where it failed -> the exclusion reopens (viewpoint trigger)",+ moved is not None and moved.directive == DIRECTIVE_FRONTIER,+ moved.directive if moved else "None")+check("and the reopened entry is pruned from the state", moved_st.failed == [],+ f"{moved_st.failed}")++# trigger 2: the map around the goal changed (new cells observed)+changed_st = ExploreState()+changed_st.note_failed(gx, gy, (centre, centre), cm)+g_changed = g.copy()+cgx, cgy = int((gx - cm.origin.position.x) / cm.resolution), int((gy - cm.origin.position.y) / cm.resolution)+patch = g_changed[cgy - 4:cgy + 5, cgx - 4:cgx + 5]+patch[patch == UNKNOWN] = FREE # the world got observed there+changed = next_target(Grid(g_changed), here, changed_st, now=10.0)+check("new observations around the failed goal -> the exclusion reopens (map trigger)",+ changed is not None and changed.directive == DIRECTIVE_FRONTIER,+ changed.directive if changed else "None")++# two exclusions: both held -> wait; one held -> the other is simply used+g2 = door(door(room(), "east", 70, 24), "west", 70, 24)+cm2 = Grid(g2)+e = next_target(cm2, pose(centre, centre, 0.0), ExploreState(), now=0.0)+wst = next_target(cm2, pose(centre, centre, math.pi), ExploreState(), now=0.0)+assert e is not None and wst is not None+both = ExploreState()+both.note_failed(e.position.x, e.position.y, (centre, centre), cm2)+both.note_failed(wst.position.x, wst.position.y, (centre, centre), cm2)+waited2 = next_target(cm2, pose(centre, centre, 0.0), both, now=45.0)+check("with two exclusions held -> wait, both counted",+ waited2 is not None and waited2.directive == DIRECTIVE_WAIT+ and waited2.n_excluded == 2,+ f"{waited2.n_excluded if waited2 else None} excluded")++one = ExploreState()+one.note_failed(e.position.x, e.position.y, (centre, centre), cm2)+other = next_target(cm2, pose(centre, centre, 0.0), one, now=45.0)+check("only one of two excluded -> the other is targeted, no wait",+ other is not None and other.directive == DIRECTIVE_FRONTIER+ and other.position.x < centre,+ f"{other.directive} x={other.position.x:.2f}" if other else "None")+++# ===========================================================================+print("D. born cornered: the pocket signature gives ONE back-off")++# 3 x 3 free cells (0.0225 m2) sealed by a ring of obstacle, unknown beyond.+g = np.full((40, 40), UNKNOWN, dtype=np.int8)+g[19:22, 19:22] = FREE+g[18:23, 18] = OCCUPIED+g[18:23, 22] = OCCUPIED+g[18, 18:23] = OCCUPIED+g[22, 18:23] = OCCUPIED+cm = Grid(g)+here = pose(w_of(20), w_of(20), 0.0)+st = ExploreState()++first = next_target(cm, here, st, now=100.0)+check("a cornered rover gets a directive, not None", first is not None)+assert first is not None+check("directive is 'back_off'", first.directive == DIRECTIVE_BACK_OFF, first.directive)+check("the back-off is 0.22 m straight behind the heading",+ abs(first.position.x - (here.position.x - 0.22)) < 1e-9+ and abs(first.position.y - here.position.y) < 1e-9,+ f"({first.position.x:.3f}, {first.position.y:.3f})")+check("it reports the pocket it measured: 9 cells = 0.0225 m2",+ abs(first.reachable_free_m2 - 0.0225) < 1e-9, f"{first.reachable_free_m2}")+check("state remembers the back-off went out", st.back_off_issued is True)++second = next_target(cm, here, st, now=101.0)+check("the SAME pocket does not give a second back-off: it gives None",+ second is None, str(second))++# facing another way, the back-off follows the heading+st3 = ExploreState()+turned = next_target(cm, pose(w_of(20), w_of(20), math.pi / 2), st3, now=100.0)+check("heading +y -> back-off is 0.22 m in -y",+ turned is not None and abs(turned.position.y - (w_of(20) - 0.22)) < 1e-9+ and abs(turned.position.x - w_of(20)) < 1e-9,+ f"({turned.position.x:.3f}, {turned.position.y:.3f})" if turned else "None")+++# ===========================================================================+print("E. blocked region: a block published as occupied is never targeted")++# The room's only door leads into a corridor the map publishes as occupied (100),+# which is all the explorer ever sees of an obstacle.+g = door(room(), "east", 70, 24)+keep = np.zeros_like(g, dtype=bool)+keep[58:82, 110:118] = True # the corridor behind the door+g[keep] = OCCUPIED+cm = Grid(g)+st = ExploreState()+blocked = next_target(cm, pose(centre, centre, 0.0), st, now=100.0)++check("with the only way out forbidden, exploration ends: None",+ blocked is None, str(blocked))++# now give the room a second, legal door and check the target never lands in+# the forbidden block, nor within a body radius of it+g2 = door(g.copy(), "north", 70, 24)+cm2 = Grid(g2)+seen_in_zone = 0+closest = math.inf+st2 = ExploreState()+for i in range(12):+ t = next_target(cm2, pose(centre, centre, 0.0), st2, now=100.0 + i)+ if t is None or t.directive != DIRECTIVE_FRONTIER:+ break+ ty, tx = cell(t.position.x, t.position.y)+ if keep[ty, tx]:+ seen_in_zone += 1+ for ky, kx in zip(*np.nonzero(keep)):+ closest = min(closest, math.hypot(t.position.x - w_of(kx), t.position.y - w_of(ky)))+check("no target ever lands inside the forbidden block", seen_in_zone == 0,+ f"{seen_in_zone} did")+check("and none comes within the lethal clearance of it",+ closest >= DEFAULT_TUNING.lethal_clearance_m,+ f"closest {closest:.2f} m >= {DEFAULT_TUNING.lethal_clearance_m} m")+++# ===========================================================================+print("F. the end of a run: None exactly when there is nothing left")++# a completely known map: free room, wall, and solid obstacle everywhere else+g = np.full((140, 140), OCCUPIED, dtype=np.int8)+g[30:110, 30:110] = FREE+cm = Grid(g)+st = ExploreState()+done = next_target(cm, pose(centre, centre, 0.0), st, now=100.0)+check("a map with no unknown cell at all -> None", done is None, str(done))+check("and it is not the cornered branch: the floor is 16 m2, not a pocket",+ st.back_off_issued is False and st.last_directive == "")++# one unknown cell short of complete, but too small to be a frontier cluster+g2 = g.copy()+g2[60:62, 60:62] = UNKNOWN # 4 unknown cells, min cluster is 6+check("an unknown speck below min_frontier_perimeter is not a frontier -> None",+ next_target(Grid(g2), pose(centre, centre, 0.0), ExploreState(), now=100.0) is None)++g3 = g.copy()+g3[60:70, 60:70] = UNKNOWN # 100 unknown cells in the middle of the room+kept = next_target(Grid(g3), pose(w_of(100), w_of(100), 0.0), ExploreState(), now=100.0)+check("a real unknown pocket inside the room IS a frontier -> a target",+ kept is not None and kept.directive == DIRECTIVE_FRONTIER,+ kept.directive if kept else "None")+++# ===========================================================================+print("H. fresh ground beats ground already worked")++# Two mirror-image doors. Test B has already shown that, facing east, the east+# door wins. Now the rover has already been sent to the east door once - and+# has been somewhere else since - so the east door is a REVISIT.+g = door(door(room(), "east", 70, 24), "west", 70, 24)+cm = Grid(g)+centre = w_of(70)++plain = next_target(cm, pose(centre, centre, 0.0), ExploreState(), now=100.0)+assert plain is not None+east_goal = (plain.position.x, plain.position.y)+check("(setup) facing east with no memory, the east door is the pick",+ east_goal[0] > centre, f"x = {east_goal[0]:.2f}")++faded = next_target(cm, pose(centre, centre, 0.0),+ ExploreState(visited=[east_goal, (-9.0, -9.0)]), now=100.0)+check("a door already published, with a goal since, loses to the fresh one",+ faded is not None and faded.position.x < centre,+ f"x = {faded.position.x:.2f}" if faded else "None")++# ... but the goal it was driving to when it last stopped is not a revisit:+# that one is unfinished business and must not be dropped.+current = next_target(cm, pose(centre, centre, 0.0),+ ExploreState(visited=[(-9.0, -9.0), east_goal]), now=100.0)+check("the goal just attempted keeps its score: no fade on unfinished business",+ current is not None and current.position.x > centre,+ f"x = {current.position.x:.2f}" if current else "None")++# The fade is the SQUARE of the distance: at half the revisit radius a cluster+# keeps a quarter of its score, not a half. One door, so the same cluster comes+# back either way and only the fade differs.+one_door = Grid(door(room(), "east", 70, 24))+clean = next_target(one_door, pose(centre, centre, 0.0), ExploreState(), now=100.0)+assert clean is not None+half = (clean.position.x - DEFAULT_TUNING.revisit_radius_m / 2, clean.position.y)+faded_half = next_target(one_door, pose(centre, centre, 0.0),+ ExploreState(visited=[half, (-9.0, -9.0)]), now=100.0)+ratio = (faded_half.score / clean.score) if faded_half is not None else None+check("a cluster half a revisit radius from an older goal keeps a QUARTER of its score",+ ratio is not None and abs(ratio - 0.25) < 1e-9, f"ratio {ratio}")+++# ===========================================================================+print("H2. the gain is what the viewpoint can SEE, not unknown behind the wall")++# Same room, two doors. Behind the east door, a cupboard: 0.35 m of unknown+# closed by walls. Behind the west door, open unknown. The two frontiers have+# the same cell count and the same distance, and the rover faces EAST, so a+# gain that counts unknown through a wall picks the cupboard.+g = door(door(room(), "east", 70, 24), "west", 70, 24)+g[57:83, 117] = OCCUPIED # cupboard back wall, 0.35 m beyond the door+g[57, 110:118] = OCCUPIED # and its two sides+g[82, 110:118] = OCCUPIED+cm = Grid(g)+seen = next_target(cm, pose(centre, centre, 0.0), ExploreState(), now=100.0)+check("facing the cupboard, the rover still takes the door with a room behind it",+ seen is not None and seen.position.x < centre,+ f"x = {seen.position.x:.2f}" if seen else "None")+check("and the gain it reports is a fraction of a disc, not a cell count",+ seen is not None and 0.0 <= seen.info_gain <= 1.0, f"{seen.info_gain}" if seen else "")+++# ===========================================================================+print("I. shut in is not finished")++# A room the body cannot leave: its only opening is 0.50 m wide, and the default+# (reference body) clearance needs 0.60 m to pass. Beyond it, a corridor and+# real unknown.+def pinched_map():+ g = np.full((160, 160), OCCUPIED, dtype=np.int8)+ g[30:110, 30:110] = FREE # the room the rover is in+ g[65:75, 110:140] = FREE # a 0.50 m x 1.50 m throat, floor seen+ g[30:110, 140:155] = UNKNOWN # the room beyond, never seen+ return g++g = pinched_map()+cm = Grid(g)+st = ExploreState()+shut_in = next_target(cm, pose(centre, centre, 0.0), st, now=100.0)+check("a rover shut in with frontiers left on the map does NOT report completion",+ shut_in is not None, str(shut_in))+assert shut_in is not None+check("it gets one back-off: reverse once and look again",+ shut_in.directive == DIRECTIVE_BACK_OFF, shut_in.directive)+check("and it says how many clusters the map still holds",+ getattr(shut_in, "n_on_the_map", 0) >= 1, f"{getattr(shut_in, 'n_on_the_map', 0)}")+# NOT the born-cornered case: the 4 x 4 m room, minus the 0.30 m the body+# cannot use along each wall, is a 3.50 x 3.50 m square = 12.25 m2, plus the+# six cells at the mouth of the throat where the wall opens out and the+# clearance rises back over 0.30 m: 12.265 m2, twenty-four times the 0.5 m2+# that means "born cornered".+check("this is NOT the born-cornered case: 12.265 m2 of floor under it",+ abs(getattr(shut_in, "reachable_free_m2", 0.0) - 12.265) < 1e-9,+ f"{getattr(shut_in, 'reachable_free_m2', float('nan')):.4f} m2")+again = next_target(cm, pose(centre, centre, 0.0), st, now=101.0)+check("one back-off per pocket and no more: then the run ends", again is None, str(again))+++# ===========================================================================+print("I2. a frontier the body cannot walk to is looked at from where it can stand")++# The same throat, but the unknown starts right at its mouth: the frontier is+# 0.55 m from floor the body can stand on. Dropping a frontier like this would+# let the run declare itself finished with it still on the map.+g = np.full((160, 160), OCCUPIED, dtype=np.int8)+g[30:110, 30:110] = FREE+g[65:75, 110:116] = FREE # 0.50 m throat, 0.30 m of seen floor+g[62:78, 116:150] = UNKNOWN # the unmapped room, opening out+cm = Grid(g)+st = ExploreState()+peek = next_target(cm, pose(centre, centre, 0.0), st, now=100.0)+check("a target comes back rather than None", peek is not None, str(peek))+assert peek is not None+check("it is a frontier goal", peek.directive == DIRECTIVE_FRONTIER, peek.directive)+gy, gx = cell(peek.position.x, peek.position.y)+check("the rover is told to stand on floor it can reach, not in the unknown",+ g[gy, gx] == FREE and not peek.on_frontier, f"grid {g[gy, gx]}")+nearest_wall = min(math.hypot(peek.position.x - w_of(cx), peek.position.y - w_of(cy))+ for cy, cx in zip(*np.nonzero(g == OCCUPIED)))+check("and it stands a body radius clear of the walls",+ nearest_wall >= DEFAULT_TUNING.lethal_clearance_m,+ f"{nearest_wall:.2f} m")+check("the look-at point is the unknown behind the throat",+ peek.look_at_xy[0] > w_of(110), f"{peek.look_at_xy}")+++# ===========================================================================+print("I3. a frontier is retired by being SEEN, not by a viewpoint being used")++# One door, one cluster. The rover drives to the target and decides from there;+# the map does not change (the shadow the 2D scan plane never enters). Standing+# in plain view of it and learning nothing is the end of that frontier - this+# is the 300-goals-and-184-m protection, and it must still hold.+g = door(room(), "east", 70, 24)+cm = Grid(g)+st = ExploreState()+first = next_target(cm, pose(centre, centre, 0.0), st, now=100.0)+assert first is not None+arrived = next_target(cm, pose(first.position.x, first.position.y, 0.0), st, now=101.0)+check("looked straight at it from close up and nothing changed: that cluster is done",+ arrived is None, str(arrived))++# Same distance, same everything, one wall of difference. A closed room with+# one unknown pocket in the middle of its floor, and a spot the rover has+# already decided from, 0.70 m from that pocket.+def pocket_room(partition: bool):+ g = np.full((140, 140), OCCUPIED, dtype=np.int8)+ g[30:110, 30:110] = FREE+ g[66:74, 66:74] = UNKNOWN # 0.40 x 0.40 m of unknown floor+ if partition:+ g[60:80, 80] = OCCUPIED # a screen between the spot and the pocket+ return g++stood_at = (w_of(84), w_of(70)) # 0.50 m east of the pocket's edge+in_view = next_target(Grid(pocket_room(False)), pose(*stood_at, 0.0),+ ExploreState(observed=[stood_at]), now=100.0)+check("a pocket in plain view of a spot already decided from is retired",+ in_view is None, str(in_view))+hidden = next_target(Grid(pocket_room(True)), pose(*stood_at, 0.0),+ ExploreState(observed=[stood_at]), now=100.0)+check("the same pocket behind a screen is NOT retired: it was never seen",+ hidden is not None and hidden.directive == DIRECTIVE_FRONTIER,+ (hidden.directive if hidden else "None"))+if hidden is not None:+ check("and the viewpoint it is given is on the pocket's side of the screen",+ hidden.position.x < w_of(80), f"x = {hidden.position.x:.2f} m")+++# ===========================================================================+print("I4. and it always terminates: no map, however awkward, spins for ever")++for label, grid_in in (("one door", door(room(), "east", 70, 24)),+ ("three doors", door(door(door(room(), "east", 70, 24),+ "west", 70, 24), "north", 70, 24)),+ ("shut in", pinched_map())):+ st = ExploreState()+ here = pose(centre, centre, 0.0)+ calls = 0+ for i in range(60):+ calls += 1+ t = next_target(Grid(grid_in), here, st, now=100.0 + i)+ if t is None:+ break+ if t.directive == DIRECTIVE_FRONTIER:+ here = pose(t.position.x, t.position.y, 0.0) # the rover arrives+ check(f"{label}: the map runs out and the function says so ({calls} calls)",+ t is None and calls < 60, f"{calls} calls, last {t}")+++# ===========================================================================+print("G. purity: nothing but the state's own fields changes")++g = door(room(), "east", 70, 24)+grid_before = g.copy()+cm = Grid(g)+here = pose(centre, centre, 0.3)+pose_before = (here.position.x, here.position.y, here.orientation.z, here.orientation.w)++st = ExploreState(back_off_issued=False)+st.note_failed(1.0, 2.0, (here.position.x, here.position.y), cm) # robot has not moved since+before = st.copy()+out = next_target(cm, here, st, now=60.0)++check("the costmap is not written to", np.array_equal(g, grid_before))+check("the pose is not written to",+ (here.position.x, here.position.y, here.orientation.z, here.orientation.w) == pose_before)+check("failed entries whose triggers have not fired are kept as they were",+ st.failed == before.failed, f"{st.failed}")+check("back_off_issued untouched when nothing was cornered",+ st.back_off_issued == before.back_off_issued)+check("heading was refreshed from the pose (0.3 rad)", abs(st.heading - 0.3) < 1e-6,+ f"{st.heading}")+check("visited / observed / targets_issued / last_directive are the only other changes",+ (len(st.visited), len(st.observed), st.targets_issued, st.last_directive)+ == (1, 1, 1, DIRECTIVE_FRONTIER))++reopened = ExploreState()+reopened.note_failed(1.0, 2.0, (here.position.x - 5.0, here.position.y), cm) # failed 5 m from here+next_target(cm, here, reopened, now=61.0)+check("a failed entry whose rover has since moved a viewpoint away is pruned",+ reopened.failed == [], f"{reopened.failed}")++a = next_target(cm, here, ExploreState(), now=100.0)+b = next_target(cm, here, ExploreState(), now=100.0)+check("same map + same pose + same state -> same answer",+ a is not None and b is not None+ and (a.position.x, a.position.y) == (b.position.x, b.position.y)+ and a.score == b.score)++tuned = next_target(cm, here, ExploreState(), now=100.0,+ tuning=Tuning(forward_bonus=0.0, revisit_radius_m=0.1))+check("tuning is honoured and is not global state",+ tuned is not None+ and next_target(cm, here, ExploreState(), now=100.0).score == a.score)++noclock = ExploreState()+noclock.note_failed(gx, gy, (here.position.x, here.position.y), cm)+check("no clock is read when `now` is given: a 1970 timestamp behaves",+ next_target(cm, here, noclock, now=-1e9) is not None)+++# ===========================================================================+print("J. the loop: a verdict owed to the PREVIOUS goal never excludes the new one")+# ===========================================================================+#+# The only group that needs dimOS: it drives Explorer2._run_exploration_loop+# itself, with next_target stubbed (A-I own the decisions, this owns the+# plumbing) and a scripted planner. goal_reached is a bare Bool with no goal+# identity in it, so the cancel of a goal that timed out unanswered could be+# credited to the goal published a millisecond earlier - an exclusion on ground+# the rover never drove to, killing every cluster within 0.6 m of it.++Explorer2 = getattr(_explorer2_module, "Explorer2", None)++if Explorer2 is None:+ print(" -- skipped: no dimOS here, the module class does not exist")+else:+ GOAL_TIMEOUT_S = 0.4+ STUB_GOAL = (2.0, 3.0) # metres, the only target the stub hands out+ START = (1.0, 3.0) # 1.00 m from it+ STEP_M = 0.4 # what the rover covers per goal: > GOAL_PROGRESS_M++ FRONTIER_LEFT = [True] # flipped by `explored()`: next_target then returns None++ def stub_target(costmap, odom, state, **kwargs):+ if not FRONTIER_LEFT[0]:+ return None # "exploration complete: no reachable frontier left"+ t = PoseStamped(ts=0.0, frame_id="world")+ t.position.x, t.position.y = STUB_GOAL+ return t++ class DoneOut:+ """The `explore_done` Out, recording what the module publishes on it.++ `transport` None reproduces a module built outside a blueprint - the+ same guard `bump` uses."""++ def __init__(self, wired=True):+ self.transport = object() if wired else None+ self.sent = []++ def publish(self, msg):+ self.sent.append(bool(getattr(msg, "data", False)))++ class Planner:+ """The goal_request Out plus the planner behind it: `script` says what+ the planner does the instant a goal is published - the worst case for+ the race."""++ transport = object()++ def __init__(self, script):+ self.script = script+ self.goals = []+ self.published_at = []+ self.explorer = None++ def publish(self, goal):+ self.goals.append((round(float(goal.position.x), 2), round(float(goal.position.y), 2)))+ self.published_at.append(time.perf_counter())+ act = self.script.get(len(self.goals))+ if act is not None:+ act(self.explorer)++ def gave_up(ex):+ """The planner publishes goal_reached=False (no path / goal abandoned)."""+ ex._on_goal_reached(type("M", (), {"data": False})())++ def drove(ex):+ """The rover covers STEP_M toward the goal; the planner says nothing."""+ o = ex.latest_odometry+ ex.latest_odometry = pose(float(o.position.x) + STEP_M, float(o.position.y))++ def last(ex):+ ex.exploration_active = False++ def explored(ex):+ """The map is fully explored: the NEXT next_target returns None."""+ FRONTIER_LEFT[0] = False++ def acts(*fns):+ return lambda ex: [f(ex) for f in fns]++ def run_loop(script, wired=True):+ FRONTIER_LEFT[0] = True+ ex = object.__new__(Explorer2)+ ex._state = ExploreState()+ ex._last_goal = None+ ex._last_goal_ok = None+ ex._late_answer_owed = False+ ex._tuning = DEFAULT_TUNING+ ex.goal_reached_event = threading.Event()+ ex.stop_event = threading.Event()+ ex.exploration_active = True+ ex.latest_costmap = Grid(door(room(), "east", 70, 24))+ ex.latest_odometry = pose(*START)+ ex.config = type("Cfg", (), {"goal_timeout": GOAL_TIMEOUT_S})()+ ex.bump = type("Bump", (), {"transport": None})()+ ex.explore_done = DoneOut(wired)+ ex.goal_request = Planner(script)+ ex.goal_request.explorer = ex+ real_next_target = _explorer2_module.next_target+ _explorer2_module.next_target = stub_target+ try:+ ex._run_exploration_loop()+ finally:+ _explorer2_module.next_target = real_next_target+ return ex, ex.goal_request, time.perf_counter()++ # Goal 1 times out while the rover is still closing on it (0.40 m of 1.00 m),+ # so it is re-decided, not excluded - and the planner still owes it a verdict.+ # That verdict lands the instant goal 2 goes out, while the rover drives to+ # goal 2 exactly as the planner plans for it.+ ex, planner, ended = run_loop({1: drove, 2: acts(gave_up, drove, last)})+ check("two goals went out", planner.goals == [STUB_GOAL, STUB_GOAL], f"{planner.goals}")+ check("the late verdict excludes nothing: the rover never failed at (2.00, 3.00)",+ ex._state.failed == [], f"{ex._state.failed}")+ check("the goal in flight keeps its own 0.4 s timeout instead of an instant verdict",+ ended - planner.published_at[1] >= GOAL_TIMEOUT_S,+ f"{ended - planner.published_at[1]:.2f} s")+ check("and the timeout then judges on the gap closed: 0.40 m of 1.00 m, so no exclusion",+ ex._state.failed == [] and abs(float(ex.latest_odometry.position.x)+ - (START[0] + 2 * STEP_M)) < 1e-6)++ # Nothing is owed on the first goal of a run: a no-path verdict on it is the+ # planner talking about THAT goal, and it must still bite at once.+ ex, planner, ended = run_loop({1: acts(gave_up, last)})+ check("a no-path verdict on the first goal is still credited",+ [(round(f[0], 2), round(f[1], 2)) for f in ex._state.failed] == [STUB_GOAL],+ f"{ex._state.failed}")+ check("and it still wakes the loop at once (no 0.4 s slept for nothing)",+ ended - planner.published_at[0] < GOAL_TIMEOUT_S,+ f"{ended - planner.published_at[0]:.3f} s")++ # An ANSWERED goal owes nothing either: two verdicts in a row are both real.+ ex, planner, ended = run_loop({1: gave_up, 2: acts(gave_up, last)})+ check("two answered goals in a row are both credited (the swallow is one deep)",+ len(ex._state.failed) == 2, f"{ex._state.failed}")++ # -- the end of the mission, SAID on the bus ------------------------------+ # The base loop only sets exploration_active False, logs one line and+ # breaks: nothing crosses the process boundary. explore_done is what tells+ # the other processes (the cockpit) that the run is over.+ ex, planner, ended = run_loop({1: acts(explored, gave_up)})+ check("the loop ends on 'no reachable frontier left'", ex.exploration_active is False)+ check("and explore_done=True goes out exactly once",+ ex.explore_done.sent == [True], f"{ex.explore_done.sent}")+ ex, planner, ended = run_loop({1: acts(gave_up, last)})+ check("a run stopped from outside publishes NO explore_done "+ "(that end already crosses on stop_explore_cmd)",+ ex.explore_done.sent == [], f"{ex.explore_done.sent}")+ ex, planner, ended = run_loop({1: acts(explored, gave_up)}, wired=False)+ check("an unwired Out is never published to, and does not crash the loop",+ ex.explore_done.sent == [] and ex.exploration_active is False)+++print(f"{OK} OK, {KO} KO")+sys.exit(1 if KO else 0)diff --git a/benches/bench_gamepad_cold.py b/benches/bench_gamepad_cold.pynew file mode 100644index 0000000..878f05e--- /dev/null+++ b/benches/bench_gamepad_cold.py@@ -0,0 +1,770 @@+"""Cold bench: gamepad mapping (no pygame) + the headless hot-plug cycle.++Known input -> known output in m/s, rad/s and seconds. Sections:+ A. axes_to_twist - known axis values -> known m/s and rad/s. Pure, no pygame.+ E. the MOSS envelope - the module fed moss_adapter's scale and envelope:+ full forward + full turn passes untouched, a stray strafe shares the rim.+ B. the module's hot-plug loop driven by a FAKE pygame: absent pad -> pad+ plugged in -> deadman released and pressed again -> pad pulled out ->+ pad back -> at 50 Hz, the 0.5 s brake then SILENCE. Still no pygame needed.+ H. brake cancel on a deadman press + the brake window knob (MOSS_BRAKE_S).+ C. real pygame with no display, if it is installed: the dummy SDL drivers+ have to make init()/event.pump() work on a headless computer.+ F. the safety nets: trust gate, absolute ceilings, rim envelope.+ G. the slew ramp.++Needs dimOS (the module imports it). No pad, no display, no robot.++Run: PYTHONDONTWRITEBYTECODE=1 PYTHONPATH=. .venv/bin/python benches/bench_gamepad_cold.py+"""+import logging+import os+import subprocess+import sys+import time+import types+from pathlib import Path++REPO_ROOT = Path(__file__).resolve().parent.parent+sys.path.insert(0, str(REPO_ROOT))++os.environ.pop("MOSS_BRAKE_S", None)+for _var in ("MOSS_MPS_AT_LIMIT", "MOSS_TRACK_M", "MOSS_TURN_GAIN", "MOSS_MAX_PCT"):+ os.environ.pop(_var, None)++from moss_dimos import gamepad as gp # noqa: E402+from moss_dimos.gamepad import GamepadTeleop, TeleopConfig, axes_to_twist # noqa: E402++OK = KO = 0+++def check(cond, label):+ global OK, KO+ if cond:+ OK += 1+ else:+ KO += 1+ print((" OK " if cond else " KO ") + label)+++def close(a, b, tol=1e-12):+ return abs(a - b) <= tol+++def twist_close(got, expected, tol=1e-12):+ return all(close(g, e, tol) for g, e in zip(got, expected))+++def wait_for(predicate, timeout=6.0, tick=0.02):+ """Poll until true or timeout. The waiting loop re-scans every 2 s."""+ deadline = time.monotonic() + timeout+ while time.monotonic() < deadline:+ if predicate():+ return True+ time.sleep(tick)+ return bool(predicate())+++# --- A. pure mapping: known axes -> known physical units ------------------+# Defaults: 0.6 m/s and 1.2 rad/s at full stick, deadzone 0.32, boost x2,+# ceilings 0.45 m/s and 0.8 rad/s.+CFG = TeleopConfig()+LIN, ANG = gp.CLAMP_LINEAR_MS, gp.CLAMP_ANGULAR_RADS+print("A. axes_to_twist (no pygame)")++check("pygame" not in sys.modules, "mapping is testable with pygame unimported")+check((LIN, ANG) == (0.45, 0.8), f"ceilings: {LIN} m/s, {ANG} rad/s")+# The pad contract as literals: every press below goes through+# gp.DEADMAN_BUTTON, so without these a moved constant would still pass.+check(gp.DEADMAN_BUTTON == 7,+ f"deadman = R1 = SDL button 7 on the MOSS pad (moss_pi's L1 is a different path): {gp.DEADMAN_BUTTON}")+check((gp.AXIS_LEFT_X, gp.AXIS_LEFT_Y, gp.AXIS_RIGHT_X, gp.AXIS_R2) == (0, 1, 2, 5),+ "strafe = left X (axis 0), throttle = left Y (axis 1), turn = right X (axis 2), boost = R2 (axis 5)")++# The module pins SDL with setdefault, so this cannot be asserted in THIS+# process: a shell that exported SDL_VIDEODRIVER=x11 would fail it, and+# honouring that export is the documented intent. Import it in a subprocess+# with a controlled environment instead - hermetic either way.+SDL_PROBE = ("import os, moss_dimos.gamepad; "+ "print(os.environ['SDL_VIDEODRIVER'], "+ "os.environ['SDL_AUDIODRIVER'])")+++def sdl_after_import(**preset):+ """SDL_VIDEODRIVER/SDL_AUDIODRIVER once the module is imported fresh."""+ env = {k: v for k, v in os.environ.items()+ if k not in ("SDL_VIDEODRIVER", "SDL_AUDIODRIVER")}+ env["PYTHONPATH"] = str(REPO_ROOT) + os.pathsep + env.get("PYTHONPATH", "")+ env.update(preset)+ done = subprocess.run([sys.executable, "-c", SDL_PROBE], env=env,+ capture_output=True, text=True)+ if done.returncode != 0:+ print(done.stderr.strip()[-400:])+ # the last line: dimOS may log above it+ lines = done.stdout.strip().splitlines()+ return lines[-1] if lines else ""+++unset = sdl_after_import()+check(unset == "dummy dummy",+ f"with SDL unset, importing the module pins the dummy (headless) "+ f"drivers: {unset!r}")+exported = sdl_after_import(SDL_VIDEODRIVER="x11")+check(exported == "x11 dummy",+ f"an operator who exported SDL_VIDEODRIVER=x11 keeps their display "+ f"(setdefault, not assignment): {exported!r}")++# SDL: stick up = -1. Full forward + R2 held -> 0.6 * 2.0 = 1.2 m/s asked.+check(twist_close(axes_to_twist(0.0, -1.0, 0.0, 1.0, CFG), (LIN, 0.0, 0.0)),+ f"stick fully forward + boost (1.2 m/s asked) -> vx CLAMPED at {LIN} m/s")+check(twist_close(axes_to_twist(0.0, -1.0, 0.0, -1.0, CFG), (LIN, 0.0, 0.0)),+ f"stick fully forward (0.6 m/s asked) -> vx CLAMPED at {LIN} m/s")+check(twist_close(axes_to_twist(0.0, -0.5, 0.0, -1.0, CFG), (0.3, 0.0, 0.0)),+ "half stick forward -> vx = 0.3 m/s (proportional below the ceiling)")+check(twist_close(axes_to_twist(0.0, 1.0, 0.0, -1.0, CFG), (-LIN, 0.0, 0.0)),+ f"stick fully back -> vx CLAMPED at -{LIN} m/s")++# Deadzone 0.32: below it the axis is dead, above it goes through unscaled.+check(twist_close(axes_to_twist(0.05, -0.10, 0.11, -1.0, CFG), (0.0, 0.0, 0.0)),+ "all sticks inside the deadzone -> 0.0 on every axis")+check(twist_close(axes_to_twist(0.0, -0.20, 0.0, -1.0, CFG), (0.0, 0.0, 0.0)),+ "0.20 is INSIDE the 0.32 deadzone -> 0.0")+check(twist_close(axes_to_twist(0.0, -0.35, 0.0, -1.0, CFG), (0.21, 0.0, 0.0)),+ "0.35 just outside the deadzone -> vx = 0.21 m/s (no rescaling)")++# Rotation: right stick right (+1) turns clockwise = negative wz.+check(twist_close(axes_to_twist(0.0, 0.0, 1.0, -1.0, CFG), (0.0, 0.0, -ANG)),+ f"right stick X = +1 (1.2 rad/s asked) -> wz CLAMPED at -{ANG} rad/s")+check(twist_close(axes_to_twist(0.0, 0.0, 1.0, 1.0, CFG), (0.0, 0.0, -ANG)),+ f"boost does not touch rotation -> wz stays at the -{ANG} rad/s ceiling")++# Strafe: left stick left (-1) strafes to +y.+check(twist_close(axes_to_twist(-1.0, 0.0, 0.0, -1.0, CFG), (0.0, LIN, 0.0)),+ f"left stick X = -1 -> vy CLAMPED at +{LIN} m/s (strafe left)")+check(twist_close(axes_to_twist(1.0, 0.0, 0.0, -1.0, CFG), (0.0, -LIN, 0.0)),+ f"left stick X = +1 -> vy CLAMPED at -{LIN} m/s (strafe right)")+check(twist_close(axes_to_twist(-0.5, -0.5, 0.5, -1.0, CFG),+ (0.15, 0.15, -0.3)),+ "diagonal + turn (half sticks: 0.3 + 0.3 + 0.5 x 0.6 = 0.9 m/s rim) -> the "+ "default envelope (0.45 m/s) halves it to (0.15, 0.15, -0.3)")++# A non-default envelope must scale, nothing hard-coded.+SLOW = TeleopConfig(linear_speed=0.25, angular_speed=0.5, deadzone=0.2,+ boost_multiplier=3.0)+vx, vy, wz = axes_to_twist(0.0, -1.0, -1.0, 1.0, SLOW)+rim = abs(vx) + abs(vy) + gp.DEFAULT_LEVER_M * abs(wz)+check(abs(rim - gp.DEFAULT_RIM_MS) < 1e-9 and abs(vx / wz - 0.45 / 0.5) < 1e-9,+ "custom cfg full fwd+turn+boost -> rim held at the envelope, gesture proportions kept")+++# --- E. the MOSS envelope: moss_adapter's scale and ceilings --------------+print("\nE. the MOSS envelope (default geometry: track 0.266 m, 0.45 m/s at 100 %, turn gain 2)")+from moss_dimos.moss_adapter import MossGeometry, teleop_envelope, teleop_speeds # noqa: E402++G = MossGeometry.from_env()+m_lin, m_ang = teleop_speeds(G)+m_env = teleop_envelope(G)+check(close(m_lin, 0.45, 1e-9) and close(m_ang, 2.0 * 2.0 * 0.45 / 0.266, 1e-9),+ f"scale: {m_lin:.3f} m/s and {m_ang:.4f} rad/s at full stick (6.7669 = 2 x the spin rate)")+check(set(m_env) == {"linear", "angular", "lever", "rim"},+ f"teleop_envelope keys = TwistEnvelope's fields: {sorted(m_env)}")+moss_pad = GamepadTeleop(linear_speed=m_lin, angular_speed=m_ang, envelope=m_env)+check(isinstance(moss_pad.envelope, gp.TwistEnvelope)+ and close(moss_pad.envelope.rim, 1.35, 1e-9) and close(moss_pad.envelope.lever, 0.133, 1e-9),+ f"the envelope dict (what a blueprint carries) becomes a TwistEnvelope: "+ f"lever {moss_pad.envelope.lever:.3f} m, rim {moss_pad.envelope.rim:.3f} m/s")+MCFG = moss_pad.cfg+got = axes_to_twist(0.0, -1.0, 1.0, -1.0, MCFG)+check(twist_close(got, (0.45, 0.0, -m_ang), 1e-9),+ f"full forward + full turn -> ({got[0]:.3f} m/s, {got[2]:.4f} rad/s): NOT scaled, "+ f"the rim (0.45 + 0.133 x 6.7669 = 1.35) is exactly the envelope")+got = axes_to_twist(-1.0, -1.0, 1.0, -1.0, MCFG)+check(twist_close(got, (0.3375, 0.3375, -0.75 * m_ang), 1e-9),+ f"... plus full left stick X -> rim 1.80, the whole command x 0.75: "+ f"({got[0]:.4f}, {got[1]:.4f}, {got[2]:.4f}); the tracks drop vy, the scale stays")+check(twist_close(axes_to_twist(0.0, -1.0, 0.0, 1.0, MCFG), (0.45, 0.0, 0.0), 1e-9),+ "boost on the MOSS scale -> vx still 0.45 m/s (the base's limit is the ceiling)")+++# --- B. hot-plug cycle on a fake pygame -----------------------------------+print("\nB. hot-plug loop (fake pygame, no display, no pad)")+++class FakeError(Exception):+ """Stand-in for pygame.error."""+++class FakePad:+ def __init__(self, name, axes, buttons=8):+ self.name, self.axes = name, axes+ self.buttons = [0] * buttons++ def init(self):+ pass++ def get_name(self):+ return self.name++ def get_numaxes(self):+ return len(self.axes)++ def get_axis(self, index):+ return self.axes[index]++ def get_numbuttons(self):+ return len(self.buttons)++ def get_button(self, index):+ return self.buttons[index]+++class FakeJoystickModule:+ """SDL joystick subsystem stand-in; the test plugs `pad` in and out."""++ def __init__(self):+ self.pad = None+ self.rescans = 0++ def init(self):+ self.rescans += 1++ def quit(self):+ pass++ def get_count(self):+ return 0 if self.pad is None else 1++ def Joystick(self, index): # noqa: N802 - pygame's own name+ if self.pad is None:+ raise FakeError("no joystick at index %d" % index)+ return self.pad+++class LogSpy:+ """Records what the module logs (dimOS's logger is structlog, not stdlib)."""++ def __init__(self):+ self.lines = []++ def _record(self, msg, *args, **_kw):+ self.lines.append(msg % args if args else msg)++ info = warning = error = debug = _record++ def count(self, needle):+ return sum(needle in line for line in self.lines)+++js = FakeJoystickModule()+fake_pygame = types.SimpleNamespace(+ error=FakeError,+ init=lambda: (5, 0),+ quit=lambda: None,+ event=types.SimpleNamespace(pump=lambda: None),+ joystick=js,+)+sys.modules["pygame"] = fake_pygame+spy = LogSpy()+real_logger, gp.logger = gp.logger, spy++pad_module = GamepadTeleop(rate_hz=200.0)+published = []+pad_module.tele_cmd_vel.subscribe(published.append)+pad_module.start()+check(pad_module._thread.name == "moss-gamepad", f"worker thread named {pad_module._thread.name!r}")++# 1. no pad at startup: the module waits instead of returning.+check(wait_for(lambda: spy.count("waiting for gamepad (index 0)") == 1, 3.0),+ "no pad at startup -> logs 'waiting for gamepad (index 0)'")+check(wait_for(lambda: js.rescans >= 3, 5.0),+ f"keeps re-scanning while waiting ({js.rescans} scans in ~5 s)")+check(pad_module._thread.is_alive() and not published,+ "loop still alive after several empty scans, nothing published")+check(spy.count("waiting for gamepad (index 0)") == 1,+ "the waiting line is logged ONCE, not once per scan")++# 2. pad appears WITH FULL-DEFLECTION AXES (an uninitialised userspace+# driver): the trust gate must hold everything at zero until neutral is seen.+js.pad = FakePad("Fake DS4", [0.0, -1.0, -1.0, 0.0, 0.0, -1.0])+check(wait_for(lambda: spy.count("Gamepad connected: Fake DS4") == 1, 5.0),+ "pad plugged in -> logs 'Gamepad connected: <name>' (the launcher greps it)")+time.sleep(0.3)+check(all(close(t.linear.x, 0.0) for t in published[-10:]) if published else True,+ "full deflection at connection -> ZEROS only (trust gate holds)")+# neutral seen once -> trust earned. Axis 2 is the rotation stick: its rest+# is 0.0.+js.pad.axes = [0.0, 0.0, 0.0, 0.0, 0.0, -1.0]+check(wait_for(lambda: spy.count("axes seen at neutral") == 1, 3.0),+ "sticks at rest -> trust earned (logged once)")+# deadman NOT held: still zeros even with a deflected stick+js.pad.axes = [0.0, -1.0, 0.0, 0.0, 0.0, -1.0]+time.sleep(0.3)+check(all(close(t.linear.x, 0.0) for t in published[-5:]),+ "stick forward WITHOUT the deadman held -> still zeros")+# deadman held -> motion, CLAMPED at the ceiling+js.pad.buttons[gp.DEADMAN_BUTTON] = 1+check(wait_for(lambda: published and close(published[-1].linear.x, LIN), 3.0),+ f"deadman held + stick forward -> vx = {LIN} m/s CEILING (got {published[-1].linear.x if published else None})")+check(close(published[-1].linear.y, 0.0) and close(published[-1].angular.z, 0.0),+ "and vy = 0.0 m/s, wz = 0.0 rad/s on the other axes")++# 2b. deadman RELEASED at speed, then re-pressed with the sticks centred: the+# slew state has to restart from rest. If prev_vx kept the last driven+# 0.45 m/s, the re-press would publish 0.438 m/s with the sticks at rest and+# the rover would leave on its own for ~0.75 s.+js.pad.buttons[gp.DEADMAN_BUTTON] = 0+js.pad.axes = [0.0, 0.0, 0.0, 0.0, 0.0, -1.0] # sticks back to neutral+check(wait_for(lambda: close(published[-1].linear.x, 0.0), 3.0),+ "deadman released while driving at the ceiling -> zeros published")+re_press = len(published) # index taken BEFORE the re-press+js.pad.buttons[gp.DEADMAN_BUTTON] = 1+check(wait_for(lambda: len(published) - re_press >= 20, 3.0),+ "deadman pressed again -> the loop keeps publishing")+after = published[re_press:]+peak = max(abs(m.linear.x) for m in after)+check(all(close(m.linear.x, 0.0) and close(m.linear.y, 0.0)+ and close(m.angular.z, 0.0) for m in after),+ f"re-press with the sticks centred -> 0.000 m/s from the FIRST command "+ f"(peak |vx| = {peak:.3f} m/s over {len(after)} messages; 0.438 with a stale ramp)")+# and the ramp climbs again from rest to the ceiling (drive state restored)+js.pad.axes = [0.0, -1.0, 0.0, 0.0, 0.0, -1.0]+check(wait_for(lambda: close(published[-1].linear.x, LIN), 3.0),+ f"stick forward again -> ramps back up to the {LIN} m/s ceiling")++# 3. pad pulled out: exactly one zero Twist, then back to waiting.+sent_before_unplug = len(published)+js.pad = None+check(wait_for(lambda: spy.count("back to waiting") == 1, 3.0),+ "pad unplugged -> logs the loss once")+check(wait_for(lambda: close(published[-1].linear.x, 0.0), 2.0),+ "publishes a zero Twist on loss")+zeros = [m for m in published[sent_before_unplug:] if close(m.linear.x, 0.0)]+check(len(zeros) == 1, f"exactly ONE zero Twist on loss (got {len(zeros)})")+check(pad_module._thread.is_alive(), "loop survives the unplug")+check(spy.count("waiting for gamepad (index 0)") == 2,+ "goes back to the waiting state")++# 4. pad comes back: trust must be RE-EARNED at neutral, then deadman,+# then boost + turn - both CLAMPED at the ceilings.+js.pad = FakePad("Fake DS5", [0.0, 0.0, 0.0, 0.0, 0.0, -1.0])+check(wait_for(lambda: spy.count("Gamepad connected: Fake DS5") == 1, 5.0),+ "pad plugged back in -> logs the new name")+check(wait_for(lambda: spy.count("axes seen at neutral") == 2, 3.0),+ "re-connection re-earns trust at neutral")+js.pad.buttons[gp.DEADMAN_BUTTON] = 1+js.pad.axes = [0.0, -1.0, 1.0, 0.0, 0.0, 1.0] # full fwd + full turn (axis 2) + boost+expected_k = gp.DEFAULT_RIM_MS / (LIN + gp.DEFAULT_LEVER_M * ANG)+check(wait_for(lambda: published and close(published[-1].linear.x, LIN * expected_k), 3.0),+ f"boost + full turn -> vx enveloped to {LIN * expected_k:.3f} m/s "+ f"(the rim, not each axis, is the ceiling; got {published[-1].linear.x if published else None})")+check(close(published[-1].angular.z, -ANG * expected_k),+ "... and wz enveloped with the SAME factor (proportions of the gesture kept)")++# 5. a pad that has no R2 axis at all must read as "no boost", not crash.+js.pad = None+check(wait_for(lambda: spy.count("waiting for gamepad (index 0)") == 3, 4.0),+ "pad pulled again -> waiting")+js.pad = FakePad("Fake 4-axis", [0.0, 0.0, 0.0, 0.0])+check(wait_for(lambda: spy.count("Gamepad connected: Fake 4-axis") == 1, 5.0),+ "4-axis pad (no R2) accepted")+check(wait_for(lambda: spy.count("axes seen at neutral") == 3, 3.0),+ "4-axis pad earns trust at neutral")+js.pad.buttons[gp.DEADMAN_BUTTON] = 1+js.pad.axes = [0.0, -1.0, 0.0, 0.0]+check(wait_for(lambda: close(published[-1].linear.x, LIN), 3.0)+ and pad_module._thread.is_alive(),+ f"missing R2 = no boost; full stick -> the {LIN} m/s ceiling, loop alive")++t0 = time.monotonic()+pad_module.stop()+stop_s = time.monotonic() - t0+check(not pad_module._thread.is_alive() and stop_s < 2.0,+ f"stop() joins the loop in {stop_s:.2f} s")+check(close(published[-1].linear.x, 0.0) and close(published[-1].angular.z, 0.0),+ "last message on shutdown is a zero Twist")++# 6. BRAKE THEN SILENCE, at the real 50 Hz rate. dimOS's MovementManager reads+# EVERY tele_cmd_vel message - zeros included - as "a human is driving": it+# cancels the nav goal and mutes nav_cmd_vel for tele_cooldown_sec (1.0 s).+# A pad at rest publishing 50 zeros/s would stop exploration for good. The+# released pad brakes for 0.5 s (<= 25 messages at 50 Hz) and then says+# NOTHING at all.+js.pad = FakePad("Fake 50Hz", [0.0, 0.0, 0.0, 0.0, 0.0, -1.0])+brake_module = GamepadTeleop(rate_hz=50.0)+brake_pub = []+brake_module.tele_cmd_vel.subscribe(brake_pub.append)+brake_module.start()+check(wait_for(lambda: spy.count("Gamepad connected: Fake 50Hz") == 1, 5.0),+ "50 Hz module: pad connected")+check(wait_for(lambda: spy.count("axes seen at neutral") == 4, 3.0),+ "50 Hz module: trust earned at neutral")+check(not brake_pub, f"deadman never held -> not a single message published "+ f"(got {len(brake_pub)})")+js.pad.buttons[gp.DEADMAN_BUTTON] = 1+js.pad.axes = [0.0, -1.0, 0.0, 0.0, 0.0, -1.0]+check(wait_for(lambda: close(brake_pub[-1].linear.x, LIN) if brake_pub else False, 3.0),+ f"deadman held -> the 50 Hz flow ramps to the {LIN} m/s ceiling")+js.pad.buttons[gp.DEADMAN_BUTTON] = 0 # release, at speed+time.sleep(1.2) # 0.5 s of brake + 0.7 s past its end+last_drive = max(i for i, m in enumerate(brake_pub) if not close(m.linear.x, 0.0))+brake = brake_pub[last_drive + 1:]+check(all(close(m.linear.x, 0.0) and close(m.linear.y, 0.0)+ and close(m.angular.z, 0.0) for m in brake),+ f"release -> every message after the last driven one is a zero Twist "+ f"({len(brake)} of them)")+check(1 <= len(brake) <= 25,+ f"... and at most 25 of them (BRAKE_S {gp.BRAKE_S} s x 50 Hz): got {len(brake)}")+silent_from = len(brake_pub)+time.sleep(0.6) # 30 ticks at 50 Hz+check(len(brake_pub) == silent_from,+ f"then SILENCE: 0 messages in the next 0.6 s (got "+ f"{len(brake_pub) - silent_from}; 30 zeros would mute autonomy)")+# and the flow restarts on the next press - the pad is not disarmed, just quiet+js.pad.buttons[gp.DEADMAN_BUTTON] = 1+check(wait_for(lambda: len(brake_pub) - silent_from >= 20, 2.0),+ f"deadman pressed again -> the 50 Hz flow resumes "+ f"({len(brake_pub) - silent_from} messages)")+restart = brake_pub[silent_from:]+step = gp.SLEW_LINEAR_MS2 / 50.0 # 0.012 m/s per tick at 50 Hz+check(abs(restart[0].linear.x) <= step + 1e-9,+ f"... from rest: first vx = {restart[0].linear.x:.3f} m/s <= one slew "+ f"step ({step:.3f}), not the 0.438 of a stale ramp")+check(wait_for(lambda: close(brake_pub[-1].linear.x, LIN), 3.0),+ f"... and ramps back to the {LIN} m/s ceiling (stick still forward)")+brake_module.stop()+++# --- H. brake cancel on a deadman press + the brake window knob -----------+# Two behaviours for two complaints a pilot can have: a quick release and+# re-press must not leave a zero behind the new command, and a released pad+# can be told to keep ordering a stop for longer before it falls silent.+print("\nH. brake cancel on deadman + the brake window knob")++# H1. the decision itself, pure. A window 0.3 s in the future, and the three+# states a tick can be in.+WINDOW = time.monotonic() + 0.3+check(gp.BRAKE_CANCEL_ON_DEADMAN is True,+ "BRAKE_CANCEL_ON_DEADMAN ships True (False = the window always runs to its end)")+check(gp.brake_window_after_press(WINDOW, True, False) == 0.0,+ "RISING edge (released -> held) -> window cancelled to 0.0, i.e. the past: "+ "the brake path cannot publish one more zero behind the new command")+check(gp.brake_window_after_press(WINDOW, True, True) == WINDOW,+ "deadman simply HELD -> window untouched (each driving tick re-arms its "+ "own, and that one must live its full brake_s after the NEXT release)")+check(gp.brake_window_after_press(WINDOW, False, True) == WINDOW,+ "the release itself -> window untouched (the brake has to happen)")+check(gp.brake_window_after_press(WINDOW, False, False) == WINDOW,+ "pad at rest -> window untouched")++# H2. the flag OFF: identity function.+gp.BRAKE_CANCEL_ON_DEADMAN = False+try:+ check(gp.brake_window_after_press(WINDOW, True, False) == WINDOW,+ "BRAKE_CANCEL_ON_DEADMAN=False -> the rising edge changes NOTHING "+ "(the window always runs to its end)")+ check(all(gp.brake_window_after_press(WINDOW, d, p) == WINDOW+ for d in (True, False) for p in (True, False)),+ "... and neither does any other combination: pure identity")+finally:+ gp.BRAKE_CANCEL_ON_DEADMAN = True++# H3. the knob on the window length, in SECONDS. Known value in, known out.+check(gp.BRAKE_S_ENV == "MOSS_BRAKE_S", f"the knob is {gp.BRAKE_S_ENV}")+check(gp.resolve_brake_s() == gp.BRAKE_S == 0.5,+ f"no argument, no env -> {gp.BRAKE_S} s, the default")+check(gp.resolve_brake_s(1.0) == 1.0, "brake_s=1.0 -> 1.00 s")+check(gp.resolve_brake_s(None, {"MOSS_BRAKE_S": "1.0"}) == 1.0,+ "MOSS_BRAKE_S=1.0 -> 1.00 s (the operator's lever: dimOS builds this "+ "module from a blueprint)")+check(gp.resolve_brake_s(0.8, {"MOSS_BRAKE_S": "0.2"}) == 0.8,+ "an explicit argument beats the environment (0.8 s wins over 0.2 s)")+check(gp.resolve_brake_s(None, {"MOSS_BRAKE_S": "3.0"}) == gp.BRAKE_S_MAX == 1.0,+ f"3.0 s clamped to {gp.BRAKE_S_MAX} s - each release mutes autonomy for "+ f"the window + dimOS's 1.0 s tele_cooldown_sec, at most 2.0 s")+check(gp.resolve_brake_s(None, {"MOSS_BRAKE_S": "-1"}) == 0.0,+ "a negative window clamped to 0.0 s (= no brake at all, but never a "+ "window in the past)")+check(gp.resolve_brake_s(None, {"MOSS_BRAKE_S": "abc"}) == gp.BRAKE_S,+ f"a mistyped MOSS_BRAKE_S gives the default window ({gp.BRAKE_S} s), "+ f"never a random one")+check(GamepadTeleop(rate_hz=50.0).brake_s == gp.BRAKE_S,+ f"a module built with no argument carries {gp.BRAKE_S} s")+check(GamepadTeleop(rate_hz=50.0, brake_s=1.0).brake_s == 1.0,+ "a module built with brake_s=1.0 carries 1.00 s")+++def run_release_repress(module, watch_s, repress_after_s=None):+ """Drive a module to the ceiling, release the deadman, optionally press it+ again `repress_after_s` later, and watch for `watch_s` from the release.++ Returns [(dt seconds since the release, vx m/s)] - the zero Twist that+ Module.stop() publishes on shutdown is EXCLUDED, it belongs to the+ teardown, not to the brake.+ """+ js.pad = FakePad("Fake H", [0.0, 0.0, 0.0, 0.0, 0.0, -1.0])+ log = []+ module.tele_cmd_vel.subscribe(lambda t: log.append((time.monotonic(), t.linear.x)))+ module.start()+ time.sleep(0.4) # trust earned at neutral+ js.pad.buttons[gp.DEADMAN_BUTTON] = 1+ js.pad.axes = [0.0, -1.0, 0.0, 0.0, 0.0, -1.0] # full forward+ wait_for(lambda: bool(log) and close(log[-1][1], LIN), 3.0)+ js.pad.buttons[gp.DEADMAN_BUTTON] = 0 # RELEASE at the ceiling+ t_rel = time.monotonic()+ if repress_after_s is not None:+ time.sleep(repress_after_s)+ js.pad.buttons[gp.DEADMAN_BUTTON] = 1 # RE-PRESS+ time.sleep(max(0.0, watch_s - repress_after_s))+ else:+ time.sleep(watch_s)+ seen = len(log) # cut BEFORE the shutdown zero+ module.stop()+ return [(t - t_rel, vx) for t, vx in log[:seen] if t > t_rel]+++# H4. THE ordered case, at the real 50 Hz: released at speed, deadman pressed+# again at t = 0.200 s -> at t = 0.250 s the published command is the STICK'S,+# not a zero. (Physical units: vx in m/s; at 0.250 s the slew has had ~3 ticks+# at 0.6 m/s2 / 50 Hz = 0.012 m/s each, so vx is small but NOT zero.)+after = run_release_repress(GamepadTeleop(rate_hz=50.0), 0.45, repress_after_s=0.200)+at_250 = [(dt, vx) for dt, vx in after if 0.235 <= dt <= 0.275]+check(bool(at_250), f"a message exists around t = 0.250 s ({len(at_250)} of them)")+check(all(vx > 0.0 for _, vx in at_250),+ f"re-press at 0.200 s -> at 0.250 s the published command is the STICK'S, "+ f"not zero: {[f'{dt:.3f}s {vx:+.3f}m/s' for dt, vx in at_250]}")+# The invariant that matters on the bus: once the command flows again, NOTHING+# zero comes behind it. (Not "no zero after t=0.200": a tick reads the button at+# its start and publishes up to one period later, so a zero decided at 0.198 s+# can legitimately land at 0.218 s. That one is ahead of the command, not+# behind it, and no software can un-decide it.)+first_cmd = next((i for i, (dt, vx) in enumerate(after) if not close(vx, 0.0)), None)+check(first_cmd is not None, "the command flow restarts after the press")+behind = [(dt, vx) for dt, vx in after[first_cmd + 1:] if close(vx, 0.0)] if first_cmd is not None else [None]+check(not behind,+ f"... and NOT ONE zero comes BEHIND the restarted command "+ f"({len(behind)} zeros after the first command)")+brake_zeros = [dt for dt, vx in after if dt <= 0.200 and close(vx, 0.0)]+check(bool(brake_zeros) and max(brake_zeros) <= 0.205,+ f"the brake did run during the window: {len(brake_zeros)} zeros between "+ f"the release and the press")++# H5. no re-press at all: zeros for brake_s, then TOTAL silence. Physical+# units: 0.5 s x 50 Hz = 25 messages maximum, then nothing for 0.4 s.+lone = run_release_repress(GamepadTeleop(rate_hz=50.0), 0.9)+zeros = [dt for dt, vx in lone if close(vx, 0.0)]+check(bool(zeros) and all(close(vx, 0.0) for _, vx in lone),+ f"deadman released and never pressed again -> every message after it is a "+ f"zero Twist ({len(lone)} of them)")+check(bool(zeros) and max(zeros) <= gp.BRAKE_S + 0.05,+ f"... the last one lands at {max(zeros, default=0.0):.3f} s <= BRAKE_S "+ f"({gp.BRAKE_S} s), not later")+check(len(zeros) <= int(gp.BRAKE_S * 50) + 2,+ f"... i.e. at most {int(gp.BRAKE_S * 50) + 2} messages at 50 Hz: got {len(zeros)}")+late = [dt for dt in zeros if dt > gp.BRAKE_S + 0.05]+check(not late, f"then SILENCE: nothing published past {gp.BRAKE_S + 0.05:.2f} s "+ f"({len(late)} stragglers)")++# H6. the longer window, measured in seconds: brake_s = 1.0 s keeps the base+# being TOLD to stop for twice as long before the module goes quiet.+long_brake = run_release_repress(GamepadTeleop(rate_hz=50.0, brake_s=1.0), 1.4)+long_zeros = [dt for dt, vx in long_brake if close(vx, 0.0)]+check(bool(long_zeros) and 0.80 <= max(long_zeros) <= 1.05,+ f"brake_s=1.0 -> the zeros run to {max(long_zeros, default=0.0):.3f} s (0.80-1.05 s "+ f"window), twice the default 0.5 s")+check(len(long_zeros) > len(zeros),+ f"... and there are more of them than at 0.5 s ({len(long_zeros)} vs "+ f"{len(zeros)} at 50 Hz)")++# H7. the flag OFF, same scenario: the default behaviour does not depend on it.+# The module already publishes the stick command from the first tick after+# the press without the cancel; what the cancel buys is the right to run H6's+# longer window with no stale zero behind a fresh command.+gp.BRAKE_CANCEL_ON_DEADMAN = False+try:+ before_flag = run_release_repress(GamepadTeleop(rate_hz=50.0), 0.45,+ repress_after_s=0.200)+ at_250_off = [(dt, vx) for dt, vx in before_flag if 0.235 <= dt <= 0.275]+ check(bool(at_250_off) and all(vx > 0.0 for _, vx in at_250_off),+ f"BRAKE_CANCEL_ON_DEADMAN=False -> SAME result at 0.250 s "+ f"({[f'{vx:+.3f}m/s' for _, vx in at_250_off]}): the flag does not "+ f"change the default behaviour, it protects the longer window")+finally:+ gp.BRAKE_CANCEL_ON_DEADMAN = True+++# H8. why the brake-cancel lines live BELOW the trust gate.+# A single missed joystick read - the pad is physically still there, one poll+# lies - while the pilot is rolling at the ceiling with the deadman held. The+# module drops the pad, re-acquires it, and has to re-earn trust at neutral;+# with the sticks still pushed it never does, so the ONLY thing it may still+# publish is the brake: zeros, until brake_until, then silence. That repetition+# is the whole point of BRAKE_S on a latest-only bus (LCM/zenoh).+# With the cancel running ABOVE the gate, the held deadman would look like a+# rising edge on the first tick after the re-acquisition, cancel the window,+# and the untrusted path would have nothing to publish in its place: 25 zeros+# over 0.486 s would become 1 zero at 0.011 s.+def drop_one_read(then_axes=None):+ """Make exactly ONE joystick poll fail - a single missed read, pad still+ plugged in. If `then_axes` is given, the sticks take that value at the very+ instant of the glitch (deterministic: no tick can slew in between)."""+ real_count = js.get_count+ fired = {"n": 0}++ def once():+ if fired["n"] == 0:+ fired["n"] = 1+ if then_axes is not None:+ js.pad.axes = list(then_axes)+ return 0 # "joystick disappeared" for this tick only+ return real_count()++ js.get_count = once+ return fired+++def run_dropout(module, watch_s, then_axes=None):+ """Drive to the ceiling with the deadman HELD, glitch one read, watch.++ Returns [(dt seconds since the glitch, vx m/s)], the shutdown zero excluded.+ """+ js.pad = FakePad("Fake H8", [0.0, 0.0, 0.0, 0.0, 0.0, -1.0])+ log = []+ module.tele_cmd_vel.subscribe(lambda t: log.append((time.monotonic(), t.linear.x)))+ module.start()+ time.sleep(0.4) # trust earned at neutral+ js.pad.buttons[gp.DEADMAN_BUTTON] = 1+ js.pad.axes = [0.0, -1.0, 0.0, 0.0, 0.0, -1.0] # full forward+ wait_for(lambda: bool(log) and close(log[-1][1], LIN), 3.0)+ drop_one_read(then_axes) # ONE missed read+ t_glitch = time.monotonic()+ try:+ time.sleep(watch_s)+ seen = len(log) # cut BEFORE the shutdown zero+ finally:+ module.stop()+ try:+ del js.get_count # back to the real fake+ except AttributeError:+ pass+ return [(t - t_glitch, vx) for t, vx in log[:seen] if t > t_glitch]+++drop = run_dropout(GamepadTeleop(rate_hz=50.0), 0.9) # sticks stay pushed+drop_zeros = [dt for dt, vx in drop if close(vx, 0.0)]+check(all(close(vx, 0.0) for _, vx in drop),+ f"one missed read at the 0.45 m/s ceiling -> NOTHING but zeros comes out "+ f"(the pad has to re-earn trust at neutral): {len(drop)} messages")+check(len(drop_zeros) >= 15,+ f"... and the stop order is REPEATED, not said once: {len(drop_zeros)} "+ f"zero Twists at 50 Hz (1 if the cancel ran above the trust gate)")+check(bool(drop_zeros) and 0.40 <= max(drop_zeros) <= gp.BRAKE_S + 0.08,+ f"... the last of them lands at {max(drop_zeros, default=0.0):.3f} s, i.e. the full "+ f"BRAKE_S window ({gp.BRAKE_S} s) and not 0.011 s")+check(not [dt for dt in drop_zeros if dt > gp.BRAKE_S + 0.08],+ "... then SILENCE, as before: the window still ends on its own")++# H9. the slew state must not survive a pad loss. Same single missed read,+# but the sticks come back at NEUTRAL - so trust IS re-earned at once and the+# module drives again. Known input -> known output, in m/s: prev_vx must be 0,+# so the command is 0.000 m/s. A ramp that survived the loss would still hold+# 0.45 m/s and the first published command would be +0.438 m/s with nobody+# touching the sticks.+NEUTRAL = [0.0, 0.0, 0.0, 0.0, 0.0, -1.0]+drop_n = run_dropout(GamepadTeleop(rate_hz=50.0), 0.9, then_axes=NEUTRAL)+peak = max((abs(vx) for _, vx in drop_n), default=0.0)+check(bool(drop_n), f"the module publishes again after the glitch ({len(drop_n)} messages)")+check(close(peak, 0.0),+ f"missed read at the ceiling + sticks at NEUTRAL -> peak |vx| = "+ f"{peak:.3f} m/s (0.438 m/s with a stale ramp; the ramp dies with the pad)")++# H10. the operator's lever on a run, end to end: the ENVIRONMENT of the+# process -> a module built with no brake_s argument, as a blueprint builds it.+os.environ["MOSS_BRAKE_S"] = "1.0"+try:+ check(GamepadTeleop(rate_hz=50.0).brake_s == 1.0,+ "MOSS_BRAKE_S=1.0 in the real environment -> a module built with no "+ "argument at all carries a 1.00 s window")+finally:+ del os.environ["MOSS_BRAKE_S"]+check(GamepadTeleop(rate_hz=50.0).brake_s == gp.BRAKE_S,+ f"... and with the variable gone again: back to {gp.BRAKE_S} s")++# no pygame at all: the module says so, with an install line that works+# (pygame is on PyPI; this package is not, so no "pip install <package>[extra]")+sys.modules["pygame"] = None # makes `import pygame` raise ImportError+spy.lines.clear()+GamepadTeleop(rate_hz=50.0)._worker() # returns at once without pygame+check(spy.count("pygame is not installed - gamepad teleop is off (pip install pygame)") == 1,+ f"no pygame -> one error line naming 'pip install pygame' ({spy.lines[-1:] if spy.lines else 'nothing logged'})")++gp.logger = real_logger+del sys.modules["pygame"]+++# --- C. the real pygame, headless -----------------------------------------+print("\nC. real pygame with no display")+try:+ import pygame+except ImportError:+ print(" .. pygame not installed here - headless check skipped")+else:+ logging.disable(logging.CRITICAL) # keep SDL chatter out of the report+ n_ok, n_fail = pygame.init()+ pygame.joystick.init()+ pygame.event.pump() # the call that needs a display+ before = pygame.joystick.get_count()+ pygame.joystick.quit()+ pygame.joystick.init() # the hot-plug re-scan+ after_count = pygame.joystick.get_count()+ pygame.quit()+ logging.disable(logging.NOTSET)+ check(n_fail == 0, f"pygame.init() with SDL_VIDEODRIVER=dummy ({n_ok} modules up)")+ check(before == after_count, f"event.pump() and joystick re-scan work headless "+ f"({after_count} pad(s) connected)")++print("\nF. the safety nets: trust gate, ceilings, rim envelope")+from moss_dimos.gamepad import ( # noqa: E402+ CLAMP_ANGULAR_RADS, CLAMP_LINEAR_MS, DEFAULT_LEVER_M, DEFAULT_RIM_MS, axes_neutral, clamp_twist,+)++cfg = TeleopConfig()+# axes uninitialised at full deflection: neutral was never observed+check(axes_neutral(1.0, -1.0, 1.0, cfg.deadzone) is False,+ "full deflection = NOT neutral (the trust gate refuses it)")+check(axes_neutral(0.01, -0.02, 0.0, cfg.deadzone) is True,+ "pad at rest = neutral (the gate opens)")+# absolute ceilings: even an insane config never exceeds them+wild = TeleopConfig(linear_speed=5.0, angular_speed=9.0, boost_multiplier=4.0)+vx, vy, wz = axes_to_twist(-1.0, -1.0, 1.0, 1.0, wild)+check(abs(vx) <= CLAMP_LINEAR_MS + 1e-9, f"full stick + boost + insane config -> |vx| <= {CLAMP_LINEAR_MS}")+check(abs(vy) <= CLAMP_LINEAR_MS + 1e-9, f"... and |vy| <= {CLAMP_LINEAR_MS}")+check(abs(wz) <= CLAMP_ANGULAR_RADS + 1e-9, f"... and |wz| <= {CLAMP_ANGULAR_RADS}")+# clamp_twist ends with the rim envelope (translation and rotation ADD at the rim)+vx, vy, wz = clamp_twist(9.0, -9.0, 9.0)+check(abs(abs(vx) + abs(vy) + DEFAULT_LEVER_M * abs(wz) - DEFAULT_RIM_MS) < 1e-9,+ "pure clamp_twist: 9 m/s everywhere -> rim exactly at the envelope")+check(clamp_twist(9.0, 0.0, 0.0) == (0.45, 0.0, 0.0),+ "no envelope given -> the conservative default ceiling, 0.45 m/s")+# a single stick at full travel loses NOTHING (the envelope = the feel of one stick alone)+check(clamp_twist(0.45, 0.0, 0.0) == (0.45, 0.0, 0.0), "pure forward 0.45 -> unchanged")+check(clamp_twist(0.0, 0.0, 0.8) == (0.0, 0.0, 0.8), "pure rotation 0.8 -> unchanged (rim 0.40 < 0.45)")+# full forward + full rotation on the default envelope+vx, vy, wz = clamp_twist(0.45, 0.0, 0.8)+rim = abs(vx) + DEFAULT_LEVER_M * abs(wz)+check(abs(rim - DEFAULT_RIM_MS) < 1e-9,+ f"forward+rotation mix -> rim {rim:.3f} = envelope {DEFAULT_RIM_MS} (0.85 unclamped)")+check(abs(vx / wz - 0.45 / 0.8) < 1e-9,+ "... and the proportions of the gesture are kept (vx/wz constant)")+++print("\nG. the teleop ramp")+from moss_dimos.gamepad import slew # noqa: E402+check(abs(slew(0.0, 0.45, 0.1) - 0.06) < 1e-9,+ "no jump: 0 -> full stick limited to 0.06 m/s per 0.1 s step")+check(abs(slew(0.45, 0.0, 0.1) - 0.39) < 1e-9, "ramp down limited too (0.45 -> 0)")+check(slew(0.05, 0.06, 0.1) == 0.06, "a near target is reached exactly")++print()+print(f"{OK} OK, {KO} KO")+print("TEST " + ("PASSED" if KO == 0 else "FAILED"))+raise SystemExit(0 if KO == 0 else 1)diff --git a/benches/bench_latest_only_cold.py b/benches/bench_latest_only_cold.pynew file mode 100644index 0000000..0ceaedd--- /dev/null+++ b/benches/bench_latest_only_cold.py@@ -0,0 +1,222 @@+"""Cold bench: velocity commands are consumed latest-only, on any transport.++lcm_latest clamps the LCM queue of command channels to one message, but under+TRANSPORT=zenoh the commands ride ZenohPubSub, which has no queue bound and no+capacity knob to set (section D checks that this is still true of the+installed dimOS). A coordinator that falls behind then executes the whole+backlog of stale twists, and the rover drives far past what it was asked.+The fix is consumer side: moss_dimos.latest_only.++Sections:+ A. the wrapper alone - 100 twists burst in while the consumer is busy, ONE is+ executed (the newest), 99 dropped. Known values in m/s.+ B. the production path - the callback MossControlCoordinator subscribes for+ twist_command, fed the same burst: the base joint velocities actually+ commanded are the newest twist's, once. Other streams stay unwrapped.+ C. without the wrapper - the same burst through dimOS's raw callback: all+ 101 execute, the newest one last, behind 100 stale commands.+ D. dimOS's zenoh subscriber has no queue capacity to set (why the fix is a+ consumer-side wrapper and not a transport setting).++Needs dimOS. starlette/uvicorn are stubbed only when they are not installed+(dimOS's viewer bridge imports them; nothing here serves HTTP).++Run: PYTHONDONTWRITEBYTECODE=1 PYTHONPATH=. .venv/bin/python benches/bench_latest_only_cold.py+"""+import importlib.util+import inspect+import sys+import threading+import time+import types+from pathlib import Path++REPO_ROOT = Path(__file__).resolve().parent.parent+sys.path.insert(0, str(REPO_ROOT))++for _name in ("starlette", "starlette.applications", "starlette.responses",+ "starlette.routing", "uvicorn"):+ if importlib.util.find_spec(_name.split(".")[0]) is not None:+ continue+ _stub = types.ModuleType(_name)+ _stub.__getattr__ = lambda attr, _n=_name: type(attr, (), {}) # type: ignore[method-assign]+ sys.modules.setdefault(_name, _stub)++from dimos.control.components import HardwareType # noqa: E402+from dimos.control.coordinator import ControlCoordinator # noqa: E402+from dimos.control.routing import Routing # noqa: E402+from dimos.msgs.geometry_msgs.Twist import Twist # noqa: E402++from moss_dimos.blueprints import MossControlCoordinator # noqa: E402+from moss_dimos.latest_only import LatestOnly # noqa: E402++OK = KO = 0+++def check(cond, label):+ global OK, KO+ print((" OK " if cond else " KO ") + label)+ if cond:+ OK += 1+ else:+ KO += 1+++def wait_for(predicate, timeout=3.0, tick=0.005):+ deadline = time.monotonic() + timeout+ while time.monotonic() < deadline:+ if predicate():+ return True+ time.sleep(tick)+ return bool(predicate())+++# The rover's own numbers: a stop, then a burst that accelerates to 1.00 m/s+# while turning at 0.50 rad/s. Only the last of the burst may ever be executed.+HOLD = 0.0+BURST = [round(0.01 * i, 4) for i in range(1, 101)] # 0.01 .. 1.00 m/s+LAST = BURST[-1] # 1.00 m/s+++class Gate:+ """Parks the consumer inside its FIRST message until released, so the burst+ lands on a consumer that is provably busy - no sleeps, no thresholds."""++ def __init__(self):+ self.seen = []+ self.entered = threading.Event()+ self.release = threading.Event()++ def consume(self, value):+ self.seen.append(value)+ if len(self.seen) == 1:+ self.entered.set()+ self.release.wait(5.0)+++print("A. the wrapper alone: 100 twists in, the newest one out")+gate = Gate()+latest = LatestOnly(gate.consume, name="bench")+latest(HOLD)+check(gate.entered.wait(2.0), "the consumer is parked inside the first command")+for v in BURST:+ latest(v)+check(len(gate.seen) == 1, f"nothing executed while the consumer is busy (executed {len(gate.seen)})")+check(latest.received == 101, f"101 commands received ({latest.received})")+check(latest.dropped == 99, f"99 of the 100 burst commands dropped ({latest.dropped})")+gate.release.set()+check(wait_for(lambda: len(gate.seen) == 2), "the consumer wakes on the pending command")+check(gate.seen == [HOLD, LAST],+ f"executed {gate.seen} m/s - the stop, then only the last of the burst")+check(latest.delivered == 2 and latest.dropped == 99,+ f"mux log: received {latest.received}, delivered {latest.delivered}, dropped {latest.dropped}")+latest.stop()+check(not latest._thread.is_alive(), "the drain thread stops on request")+++def bench_coordinator(handler, cls):+ """A coordinator instance with one BASE, one route, and no transport: just+ enough for the real _make_stream_cb / _map_twist_to_base_joints path."""+ inst = object.__new__(cls)+ inst._hardware = {"base": types.SimpleNamespace(+ component=types.SimpleNamespace(hardware_type=HardwareType.BASE),+ joint_names=["base/vx", "base/vy", "base/wz"])}+ inst._hardware_lock = threading.Lock()+ inst._task_lock = threading.Lock()+ task = types.SimpleNamespace(name="vel_base",+ claim=lambda: types.SimpleNamespace(joints=set()))+ inst._routes = {"joint_command": [(task, handler, Routing.BROADCAST)]}+ inst._stream_pre_hooks = {"twist_command": inst._map_twist_to_base_joints}+ return inst+++def twist(v):+ """Forward v m/s, turning at v/2 rad/s."""+ return Twist(linear=[v, 0.0, 0.0], angular=[0.0, 0.0, v / 2])+++print("\nB. the production path: what the coordinator subscribes for twist_command")+gate_b = Gate()+++def base_handler(msg, t_now):+ gate_b.consume(dict(zip(msg.name, msg.velocity)))+++def section_b():+ coord = bench_coordinator(base_handler, MossControlCoordinator)+ cb = coord._make_stream_cb("twist_command")+ check(isinstance(cb, LatestOnly), f"twist_command is wrapped latest-only ({type(cb).__name__})")+ cb(twist(HOLD))+ check(gate_b.entered.wait(2.0), "the base is parked inside the first command")+ for v in BURST:+ cb(twist(v))+ check(len(gate_b.seen) == 1, f"no stale twist reaches the base ({len(gate_b.seen)} executed)")+ gate_b.release.set()+ check(wait_for(lambda: len(gate_b.seen) == 2), "the base wakes on the pending command")+ commanded = gate_b.seen[-1] if len(gate_b.seen) > 1 else {}+ check(commanded.get("base/vx") == LAST,+ f"base/vx commanded = {commanded.get('base/vx')} m/s (the newest twist, {LAST})")+ check(commanded.get("base/wz") == LAST / 2,+ f"base/wz commanded = {commanded.get('base/wz')} rad/s (the newest twist, {LAST / 2})")+ dropped_b = getattr(cb, "dropped", 0) # 0 when the callback is not wrapped at all+ check(len(gate_b.seen) == 2 and dropped_b == 99,+ f"2 of 101 twists executed, {dropped_b} dropped")++ before = len(gate_b.seen)+ joint_cb = coord._make_stream_cb("joint_command")+ check(not isinstance(joint_cb, LatestOnly), "other streams stay unwrapped")+ for _ in range(3):+ joint_cb(types.SimpleNamespace(name=["base/vx"], velocity=[0.0]))+ check(len(gate_b.seen) == before + 3,+ f"joint_command still delivers every message, in the caller's thread ({len(gate_b.seen) - before}/3)")+ check(coord._make_stream_cb("twist_command") is cb,+ "a re-subscription reuses the same wrapper (one drain thread per coordinator)")+ if isinstance(cb, LatestOnly):+ cb.stop()+++section_b()++print("\nC. without the wrapper: the same burst through dimOS's raw callback")+gate_c = Gate()+++def base_handler_c(msg, t_now):+ gate_c.consume(dict(zip(msg.name, msg.velocity)))+++stock = bench_coordinator(base_handler_c, ControlCoordinator)+raw = ControlCoordinator._make_stream_cb(stock, "twist_command")+check(not isinstance(raw, LatestOnly), "dimOS's own callback is unwrapped")+msgs = [twist(HOLD)] + [twist(v) for v in BURST]+# One FIFO delivery thread = what both transports do; it blocks on the parked+# consumer, which is exactly how the backlog builds up.+feeder = threading.Thread(target=lambda: [raw(m) for m in msgs], daemon=True)+feeder.start()+check(gate_c.entered.wait(2.0), "the base is parked inside the first command")+gate_c.release.set()+feeder.join(10.0)+check(wait_for(lambda: len(gate_c.seen) == 101),+ f"the whole backlog executes: {len(gate_c.seen)}/101 twists reach the base")+check(len(gate_c.seen) == 101 and gate_c.seen[-1].get("base/vx") == LAST,+ f"the newest twist runs LAST, behind {len(gate_c.seen) - 1} stale ones")+print(f" .. unwrapped {len(gate_c.seen)} executed vs {len(gate_b.seen) - 3} with the wrapper")++print("\nD. dimOS's zenoh subscriber offers no queue capacity to clamp")+from dimos.protocol.pubsub.impl import zenohpubsub # noqa: E402++params = list(inspect.signature(zenohpubsub.ZenohPubSubBase.subscribe).parameters)+check(params == ["self", "topic", "callback"],+ f"ZenohPubSubBase.subscribe{tuple(params)} takes no capacity/handler argument")+src = inspect.getsource(zenohpubsub)+check("RingChannel" not in src and "FifoChannel" not in src,+ "no zenoh RingChannel/FifoChannel handler anywhere in the module")+qos_fields = set(zenohpubsub.ZenohQoS.__dataclass_fields__)+check(qos_fields == {"reliability", "congestion_control"},+ f"Topic.qos is publisher-side only: {sorted(qos_fields)}")++print()+print(f"{OK} OK, {KO} KO")+print("TEST PASSED" if KO == 0 else "TEST FAILED")+raise SystemExit(1 if KO else 0)diff --git a/benches/bench_lcm_latest_live.py b/benches/bench_lcm_latest_live.pynew file mode 100644index 0000000..0046c1d--- /dev/null+++ b/benches/bench_lcm_latest_live.py@@ -0,0 +1,75 @@+"""Live bench for moss_dimos.lcm_latest (needs dimOS and LCM multicast on this+machine; no robot, no motor): a slow subscriber on a command channel must+never accumulate a backlog once lcm_latest is installed.++Known in: 100 twists published in 1 s to a handler that takes 100 ms each.+Known out: with a 1-deep queue the handler sees ~10 messages and at most ONE+arrives after the publisher stopped; with dimOS's 10000-deep queue it grinds+through the backlog for seconds after the publisher has stopped.++Run: PYTHONDONTWRITEBYTECODE=1 PYTHONPATH=. .venv/bin/python benches/bench_lcm_latest_live.py # patched (default)+ PYTHONDONTWRITEBYTECODE=1 PYTHONPATH=. .venv/bin/python benches/bench_lcm_latest_live.py --stock # dimOS default, for comparison+"""+import sys+import time+from pathlib import Path++sys.path.insert(0, str(Path(__file__).resolve().parent.parent))+stock = "--stock" in sys.argv++OK = KO = 0+++def check(cond, label):+ global OK, KO+ print((" OK " if cond else " KO ") + label)+ if cond:+ OK += 1+ else:+ KO += 1+++if not stock:+ from moss_dimos.lcm_latest import capacity_for, install+ check(install() is True, "lcm_latest.install() found dimOS's LCM and patched it")+ check(capacity_for("/cmd_vel") == 1 and capacity_for("/tele_cmd_vel") == 1+ and capacity_for("/twist_command") == 1 and capacity_for("/odom") is None,+ "command channels (cmd_vel, tele_cmd_vel, twist_command) -> capacity 1; /odom untouched")++from dimos.core.transport_factory import make_transport # noqa: E402+from dimos.msgs.geometry_msgs.Twist import Twist # noqa: E402++TOPIC = "cmd_vel_latest_bench" # contains "cmd_vel" -> clamped when patched+handled = []+++def slow_handler(msg):+ handled.append(time.monotonic())+ time.sleep(0.1)+++sub = make_transport(TOPIC, Twist)+sub.subscribe(slow_handler)+pub = make_transport(TOPIC, Twist)+time.sleep(0.5)+t0 = time.monotonic()+for i in range(100):+ pub.broadcast(None, Twist(linear=[0.1, 0, 0], angular=[0, 0, 0]))+ time.sleep(0.01)+t_stop = time.monotonic()+time.sleep(3.0)+late = [t for t in handled if t > t_stop + 0.15]+mode = "STOCK" if stock else "PATCHED"+if handled:+ print(f" .. {mode}: published 100 in {t_stop - t0:.2f} s; handled {len(handled)}; handled after the "+ f"publisher stopped (+150 ms): {len(late)}; last handled {max(handled) - t_stop:+.2f} s after stop")+check(len(handled) > 0, "the subscriber received twists (LCM multicast works on this machine)")+if stock:+ print(f" .. stock mode: {len(late)} twists executed after the publisher stopped (no pass/fail on this number)")+else:+ check(len(late) <= 1, f"at most one twist handled after the publisher stopped ({len(late)})")++print()+print(f"{OK} OK, {KO} KO")+print("TEST PASSED" if KO == 0 else "TEST FAILED")+raise SystemExit(1 if KO else 0)diff --git a/benches/bench_lidar_odometry_cold.py b/benches/bench_lidar_odometry_cold.pynew file mode 100644index 0000000..c26edcb--- /dev/null+++ b/benches/bench_lidar_odometry_cold.py@@ -0,0 +1,359 @@+"""Cold bench for lidar_odometry: the pose and the clouds it publishes, in physical units.++kiss-icp is faked (the bench commands the pose its registration reports);+the dimOS messages are real. Known input -> known output, in metres and degrees:++ A. the module as built: streams, rpcs, defaults, and start() building+ kiss-icp with its configuration and logging "lidar odometry up".+ B. roundtrip 73 deg: a revolution registered at (1.20, -0.70, +73.0 deg)+ comes out as odom (1.20, -0.70, z = 0.00) heading 73.0 deg, a world cloud+ whose point 2.0 m ahead lands at (1.7847, 1.2126, 0.20), and a tf with+ base_link->lidar_link at z = 0.20 m. One cloud per revolution.+ C. the lidar height contract with costmap2d: MOSS_LIDAR_HEIGHT_M=0.215 moves+ the cloud and the tf to 0.215 m, and costmap2d.lidar_returns tags those+ points (float32 on the wire) exactly when both read the same height.+ D. the gyro prior: a yaw rate of 0.5 rad/s for 0.2 s (gyro y = -0.5 on the+ default "-y" axis) -> the next revolution turns 5.73 deg; with the prior+ off, kiss-icp's constant-velocity model is used instead.+ E. the optional depth path: 1.000 m of depth at the principal point, rover+ at the origin -> a camera point at (1.156, 0, 0.075) m on the next lidar+ cloud, which costmap2d.lidar_returns does NOT tag as lidar.++Needs dimOS (kiss-icp is not needed: faked through sys.modules).++Run: PYTHONPATH=. .venv/bin/python benches/bench_lidar_odometry_cold.py+"""++import asyncio+import gc+import math+import os+import sys+import threading+import time+import types+from pathlib import Path++sys.path.insert(0, str(Path(__file__).resolve().parent.parent))++import numpy as np # noqa: E402++# MOSS's own numbers, whatever the shell exported+for _var in [v for v in os.environ if v.startswith("MOSS_")]:+ del os.environ[_var]++from dimos.core.coordination.blueprints import BlueprintAtom # noqa: E402+from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped # noqa: E402+from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo # noqa: E402+from dimos.msgs.sensor_msgs.Image import Image # noqa: E402+from dimos.msgs.sensor_msgs.Imu import Imu # noqa: E402+from dimos.msgs.sensor_msgs.JointState import JointState # noqa: E402+from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 # noqa: E402+from dimos.msgs.tf2_msgs.TFMessage import TFMessage # noqa: E402++from moss_dimos import costmap2d # noqa: E402+from moss_dimos import lidar_odometry as LO # noqa: E402++OK = 0+KO = 0+++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+++# --- a fake kiss-icp: the pose it reports is the bench's to command ----------++class Threshold:+ def __init__(self, v=0.2):+ self.v = v+ self.deviations = 0++ def get_threshold(self):+ return self.v++ def update_model_deviation(self, corr):+ self.deviations += 1+++class Preproc:+ def preprocess(self, pts, ts, prior):+ return pts+++class LocalMap:+ def __init__(self):+ self.updates = 0++ def update(self, frame, pose):+ self.updates += 1+++class Registration:+ def align_points_to_map(self, points, voxel_map, initial_guess, max_correspondance_distance, kernel):+ # the registration "finds" exactly the prediction: the published pose+ # is then the prior the bench commanded+ return np.asarray(initial_guess)+++class Kiss:+ """Starts at the identity; its constant-velocity model (last_delta) is+ the motion the bench commands for the next revolution."""++ def __init__(self, dx=0.0, dy=0.0, dyaw=0.0, cfg=None):+ self.cfg = cfg+ self.adaptive_threshold = Threshold()+ self.preprocessor = Preproc()+ self.local_map = LocalMap()+ self.registration = Registration()+ self.last_pose = np.eye(4)+ self.last_delta = LO._se2(dx, dy, dyaw)++ def voxelize(self, frame):+ return frame, frame+++_cfg_mod = types.ModuleType("kiss_icp.config")+_cfg_mod.KISSConfig = lambda: types.SimpleNamespace(+ data=types.SimpleNamespace(), mapping=types.SimpleNamespace(),+ adaptive_threshold=types.SimpleNamespace())+_kiss_mod = types.ModuleType("kiss_icp.kiss_icp")+_kiss_mod.KissICP = lambda cfg: Kiss(cfg=cfg)+sys.modules.setdefault("kiss_icp", types.ModuleType("kiss_icp"))+sys.modules["kiss_icp.config"] = _cfg_mod+sys.modules["kiss_icp.kiss_icp"] = _kiss_mod+++class Sink:+ """An Out stream that keeps what was published."""++ def __init__(self):+ self.msgs = []++ def publish(self, msg):+ self.msgs.append(msg)+++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+++LOOP = asyncio.new_event_loop()+REAL = [] # modules built through dimOS's __init__, for the record+++def odometry(kiss, use_gyro_prior=False):+ """The real __init__ (the fields handle_pointcloud needs come from it),+ the fake kiss-icp, and Sinks in place of the Outs."""+ o = LO.LidarOdometry(use_gyro_prior=use_gyro_prior)+ REAL.append(o)+ o._kiss, o._lock = kiss, threading.Lock()+ o.log_every_s = 1e9 # this bench reads publications, not the log+ o.odom, o.lidar, o.tf, o.camera_floor = Sink(), Sink(), Sink(), Sink()+ return o+++def drive(o, n_revs: int, pts):+ msg = PointCloud2.from_numpy(pts.astype(np.float32), frame_id="lidar_link", timestamp=time.time())+ for _ in range(n_revs):+ LOOP.run_until_complete(o.handle_pointcloud(msg))+++def yaw_of(q) -> float:+ """Heading of a quaternion, in degrees."""+ return math.degrees(2.0 * math.atan2(q.z, q.w))+++def cloud_xyz(msg) -> np.ndarray:+ out = msg.as_numpy()+ return np.asarray(out[0] if isinstance(out, tuple) else out)+++# A flat revolution in lidar_link: 360 returns on a 2 m circle, plus one+# marker 2.0 m dead ahead (index 0: angle 0).+SCAN_FLAT = np.stack([np.cos(np.linspace(0, 2 * math.pi, 360, endpoint=False)) * 2.0,+ np.sin(np.linspace(0, 2 * math.pi, 360, endpoint=False)) * 2.0,+ np.zeros(360)], axis=1)+++# --- A. the module as built ---------------------------------------------------+print("A. streams, rpcs, defaults, start()")+streams = {(s.name, s.type): s.direction for s in BlueprintAtom.create(LO.LidarOdometry, {}).streams}+want = {("pointcloud", PointCloud2): "in", ("imu", Imu): "in", ("coordinator_joint_state", JointState): "in",+ ("depth_image", Image): "in", ("camera_info", CameraInfo): "in",+ ("odom", PoseStamped): "out", ("lidar", PointCloud2): "out", ("camera_floor", PointCloud2): "out",+ ("tf", TFMessage): "out"}+check("the stream table: 5 Ins, 4 Outs, names and types as wired by the MOSS blueprints",+ streams == want, f"extra {set(streams) - set(want)}, missing {set(want) - set(streams)}")+check("no reloc_frame, bump or bump_rear stream",+ not any(n in ("reloc_frame", "bump", "bump_rear") for n, _ in streams))+check("LidarOdometry.rpcs carries start and stop", {"start", "stop"} <= set(LO.LidarOdometry.rpcs))+check("dimOS instance name 'lidarodometry'", LO.LidarOdometry.name == "lidarodometry")++o = LO.LidarOdometry(use_gyro_prior=False)+REAL.append(o)+check("defaults: range 0.35-12 m, voxel 0.05 m, initial threshold 0.3 m, gyro axis -y, frames world/base_link/lidar_link",+ (o.min_range_m, o.max_range_m, o.voxel_size_m, o.initial_threshold_m, o.gyro_axis,+ o.world_frame, o.base_frame, o.lidar_frame)+ == (0.35, 12.0, 0.05, 0.3, "-y", "world", "base_link", "lidar_link"))+check("nothing native before start(): no kiss-icp, no lock (dimOS pickles the module)",+ o._kiss is None and o._lock is None)++spy, real_logger = LogSpy(), LO.logger+LO.logger = spy+real_module_start = LO.Module.start+LO.Module.start = lambda self: None # the dimOS runtime is not under test here+try:+ o.start()+finally:+ LO.Module.start = real_module_start+ LO.logger = real_logger+cfg = getattr(o._kiss, "cfg", None)+check("start() builds kiss-icp: max 12 m, min 0.35 m, no deskew, voxel 0.05 m, 30 points/voxel, threshold 0.3 m, min motion 0.02 m",+ cfg is not None and (cfg.data.max_range, cfg.data.min_range, cfg.data.deskew, cfg.mapping.voxel_size,+ cfg.mapping.max_points_per_voxel, cfg.adaptive_threshold.initial_threshold,+ cfg.adaptive_threshold.min_motion_th) == (12.0, 0.35, False, 0.05, 30, 0.3, 0.02))+up = [line for line in spy.lines if line.startswith("lidar odometry up")]+check("start() logs 'lidar odometry up (kiss-icp, ...' once (the launcher greps this prefix)",+ len(up) == 1 and up[0].startswith("lidar odometry up (kiss-icp,") and up[0].endswith("-> world"),+ up[0] if up else "no line")+++# --- B. roundtrip 73 deg --------------------------------------------------------+print("B. roundtrip: a revolution registered at (1.20, -0.70, +73.0 deg)")+TRUE = (1.20, -0.70, math.radians(73.0))+o = odometry(Kiss(*TRUE))+drive(o, 1, SCAN_FLAT)+od = o.odom.msgs[-1]+check("odom position (1.20, -0.70) m, z = 0.00 (the floor plane, never the lidar height)",+ abs(od.position.x - 1.20) < 1e-9 and abs(od.position.y + 0.70) < 1e-9 and od.position.z == 0.0,+ f"({od.position.x:.4f}, {od.position.y:.4f}, {od.position.z:.4f})")+check("odom heading 73.0 deg", abs(yaw_of(od.orientation) - 73.0) < 0.1, f"{yaw_of(od.orientation):.3f} deg")+check("odom frame 'world'", od.frame_id == "world")+xyz = cloud_xyz(o.lidar.msgs[-1])+c, s = math.cos(TRUE[2]), math.sin(TRUE[2])+ahead = (1.20 + 2.0 * c, -0.70 + 2.0 * s)+check(f"the return 2.0 m dead ahead lands at ({ahead[0]:.4f}, {ahead[1]:.4f}, 0.20) m in world",+ abs(xyz[0, 0] - ahead[0]) < 1e-5 and abs(xyz[0, 1] - ahead[1]) < 1e-5 and abs(xyz[0, 2] - 0.20) < 1e-6,+ f"({xyz[0, 0]:.4f}, {xyz[0, 1]:.4f}, {xyz[0, 2]:.4f})")+check("every lidar point sits at z = 0.20 m (MOSS lidar_height_m), to the bit of a float32 on the wire",+ np.all(xyz[:, 2] == np.float64(np.float32(0.20))), f"z in [{xyz[:, 2].min()}, {xyz[:, 2].max()}]")+check("the cloud is in 'world', 360 points", o.lidar.msgs[-1].frame_id == "world" and len(xyz) == 360)+tf = o.tf.msgs[-1].transforms+check("tf world->base_link at (1.20, -0.70, 0.00), heading 73.0 deg",+ tf[0].frame_id == "world" and tf[0].child_frame_id == "base_link"+ and abs(tf[0].translation.x - 1.20) < 1e-9 and abs(tf[0].translation.y + 0.70) < 1e-9+ and tf[0].translation.z == 0.0 and abs(yaw_of(tf[0].rotation) - 73.0) < 0.1)+check("tf base_link->lidar_link at z = 0.20 m",+ tf[1].frame_id == "base_link" and tf[1].child_frame_id == "lidar_link"+ and abs(tf[1].translation.z - 0.20) < 1e-12 and tf[1].translation.x == 0.0)+check("kiss-icp integrated the scan: local map updated, adaptive threshold fed",+ o._kiss.local_map.updates == 1 and o._kiss.adaptive_threshold.deviations == 1)+p = odometry(Kiss())+drive(p, 25, SCAN_FLAT)+check("a parked rover: 25 revolutions -> 25 odom, 25 clouds, 25 tf, all at (0, 0)",+ len(p.odom.msgs) == len(p.lidar.msgs) == len(p.tf.msgs) == 25+ and all(m.position.x == 0.0 and m.position.y == 0.0 for m in p.odom.msgs))+short = odometry(Kiss())+drive(short, 3, SCAN_FLAT[:29])+check("a revolution under 30 points is skipped (nothing published)", not short.odom.msgs and not short.lidar.msgs)+++# --- C. the lidar height contract with costmap2d --------------------------------+print("C. lidar_height_m: the lift here, the tag in costmap2d")+os.environ["MOSS_LIDAR_HEIGHT_M"] = "0.215"+try:+ o = odometry(Kiss(0.5, 0.0, 0.0))+ drive(o, 1, SCAN_FLAT)+ xyz = cloud_xyz(o.lidar.msgs[-1])+ check("MOSS_LIDAR_HEIGHT_M=0.215 (read at call time): every point at z = 0.215 m",+ np.all(xyz[:, 2] == np.float64(np.float32(0.215))))+ check("... and the base_link->lidar_link tf at z = 0.215 m",+ abs(o.tf.msgs[-1].transforms[1].translation.z - 0.215) < 1e-12)+ check("... and odom stays on the floor, z = 0.00", o.odom.msgs[-1].position.z == 0.0)+ tagged = costmap2d.lidar_returns(xyz.astype(np.float64))+ check("costmap2d reads the same height: all 360 points tagged as lidar returns", bool(tagged.all()))+finally:+ del os.environ["MOSS_LIDAR_HEIGHT_M"]+check("a cloud lifted to 0.215 m is NOT tagged when the costmap reads 0.20 m (one value, both ends)",+ not costmap2d.lidar_returns(xyz.astype(np.float64)).any())+++# --- D. the gyro prior ----------------------------------------------------------+print("D. the rotation prior: gyro when on and fed, constant velocity otherwise")+++def imu(gy, ts):+ return types.SimpleNamespace(angular_velocity=types.SimpleNamespace(x=0.0, y=gy, z=0.0), ts=ts)+++o = odometry(Kiss(0.10, 0.0, math.radians(2.0)), use_gyro_prior=True)+t0 = 1000.0+for i in range(11): # 0.2 s at 50 Hz, yaw rate 0.5 rad/s+ LOOP.run_until_complete(o.handle_imu(imu(-0.5, t0 + 0.02 * i)))+drive(o, 1, SCAN_FLAT)+yaw = yaw_of(o.odom.msgs[-1].orientation)+check("0.5 rad/s for 0.2 s on the -y axis -> the revolution turns 5.73 deg (not the 2.0 deg the motion model says)",+ abs(yaw - math.degrees(0.1)) < 0.01, f"{yaw:.3f} deg, prior {o._prior_used}")+check("the translation still comes from the motion model: x = 0.10 m",+ abs(o.odom.msgs[-1].position.x - 0.10) < 1e-9)+check("the prior is consumed: the next revolution has no gyro sample", o._gyro_seen is False and o._gyro_acc == 0.0)+q = odometry(Kiss(0.10, 0.0, math.radians(2.0)), use_gyro_prior=False)+for i in range(11):+ LOOP.run_until_complete(q.handle_imu(imu(-0.5, t0 + 0.02 * i)))+drive(q, 1, SCAN_FLAT)+check("use_gyro_prior=False (the MOSS blueprint): the same gyro is ignored, 2.0 deg from the motion model",+ abs(yaw_of(q.odom.msgs[-1].orientation) - 2.0) < 0.01 and q._prior_used == "cv")+js = types.SimpleNamespace(name=["base/vx", "base/vy", "base/wz"], position=[0.5, 0.0, 0.1])+LOOP.run_until_complete(q.handle_coordinator_joint_state(js))+drive(q, 1, SCAN_FLAT)+step = math.radians(2.0) # second revolution: (0.10, 0) then 0.10 m along 2 deg+check("wheel odometry is read for the log only: the pose follows the motion model to (0.1999, 0.0035) m, 4.0 deg",+ q._wheel == (0.5, 0.0, 0.1)+ and abs(q.odom.msgs[-1].position.x - (0.10 + 0.10 * math.cos(step))) < 1e-9+ and abs(q.odom.msgs[-1].position.y - 0.10 * math.sin(step)) < 1e-9+ and abs(yaw_of(q.odom.msgs[-1].orientation) - 4.0) < 0.01,+ f"({q.odom.msgs[-1].position.x:.4f}, {q.odom.msgs[-1].position.y:.4f})")+++# --- E. the optional depth path -------------------------------------------------+print("E. depth camera (idle on MOSS): one pixel through the profile's mount")+o = odometry(Kiss())+o._K = (600.0, 600.0, 0.0, 0.0)+o._depth_n = LO.DEPTH_EVERY - 1 # the next frame is the one processed+depth = types.SimpleNamespace(data=np.array([[1000]], dtype=np.uint16), ts=0.0)+LOOP.run_until_complete(o.handle_depth_image(depth))+cam_pt = None if o._pending_cam_pts is None else [float(v) for v in o._pending_cam_pts[0][0]]+check("1.000 m at the principal point, rover at the origin -> (1.156, 0, 0.075) m (MOSS camera 0.156/0/0.075, pitch 0)",+ cam_pt is not None and all(abs(a - b) < 1e-5 for a, b in zip(cam_pt, (1.156, 0.0, 0.075))), f"{cam_pt}")+drive(o, 1, SCAN_FLAT)+xyz = cloud_xyz(o.lidar.msgs[-1]).astype(np.float64)+tag = costmap2d.lidar_returns(xyz)+check("the next lidar cloud carries 360 lidar points + the camera point",+ len(xyz) == 361 and abs(xyz[-1, 2] - 0.075) < 1e-6)+check("costmap2d tags the 360 lidar points and not the camera point", int(tag.sum()) == 360 and not tag[-1])++# Open3D (behind PointCloud2) reports every cloud still alive at interpreter+# exit as a leak and turns the exit status to 1: drop the published clouds first.+for m in REAL:+ for name in ("odom", "lidar", "tf", "camera_floor"):+ sink = getattr(m, name)+ if isinstance(sink, Sink):+ sink.msgs.clear()+gc.collect()++print()+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/benches/bench_memory_rotation_cold.py b/benches/bench_memory_rotation_cold.pynew file mode 100644index 0000000..3e679b9--- /dev/null+++ b/benches/bench_memory_rotation_cold.py@@ -0,0 +1,332 @@+"""Cold bench for the recording rotation: a SIGKILLed run keeps its WAL.++A recording is a TRIO - moss.db, moss.db-wal, moss.db-shm - and a run killed+with SIGKILL (`dimos stop` escalates to it) leaves its last commits in the+-wal alone. dimOS's backup_file() renames the .db by itself, so the backup a+later reader opens has an empty schema and the next run's fresh db writes over+the -wal that held everything: the crashed run, the one worth keeping, is the+one destroyed. Sections, all in physical units (metres of recorded pose x,+bytes on disk):++ A. what a kill leaves - the writer dies by SIGKILL with 500 rows committed;+ the .db alone reads as "no such table: obs"+ B. healthy rotation - the trio is renamed together and the rotated+ recording gives back 500 rows and x = 124.75 m+ through a read-only URI; the next run's fresh db+ leaves it alone+ C. torn trio - -wal/-shm with no db: dated quarantine, byte for+ byte, never rm; the new db is fresh, the quarantined+ -wal survives it, and re-paired with its own db it+ still returns the 500 rows / 124.75 m+ D. no overwrite - two rotations in the same second keep both trios+ whole (500 rows and 300 rows, each with its own -wal)+ and the prune retires whole trios, never a widowed+ -wal that would shadow a later recording+ E. the wiring - RotatingRecorder / MossRecorder really rotate on+ start(), and only where dimOS would have called+ backup_file (BACKUP, not replay, not APPEND); the+ MOSS recording path follows MOSS_STATE_DIR++Run: PYTHONDONTWRITEBYTECODE=1 PYTHONPATH=. .venv/bin/python benches/bench_memory_rotation_cold.py+"""++import hashlib+import re+import shutil+import sqlite3+import subprocess+import sys+import tempfile+import types+from pathlib import Path++sys.path.insert(0, str(Path(__file__).resolve().parent.parent))++# dimos.memory.module reaches dimos.models.embedding -> torch, absent from a+# bench venv. Only annotations need it here; nothing in this bench calls torch.+if "torch" not in sys.modules:+ class _Dtype:+ pass++ class _TorchStub(types.ModuleType):+ dtype = _Dtype+ Tensor = type("Tensor", (), {})+ device = str+ cuda = types.SimpleNamespace(is_available=lambda: False)+ backends = types.SimpleNamespace(mps=types.SimpleNamespace(is_available=lambda: False))++ def __getattr__(self, name):+ return _Dtype()++ sys.modules["torch"] = _TorchStub("torch")++from dimos.memory.module import Recorder # noqa: E402++from moss_dimos import memory as M # noqa: E402++OK = 0+KO = 0+++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+++# A recorded pose every 0.25 m: row i is at x = i * 0.25 m, so 500 rows end at+# 124.75 m - a known input read back as a known output.+STEP_M = 0.25+WRITER = """+import os, signal, sqlite3, sys+db, rows = sys.argv[1], int(sys.argv[2])+c = sqlite3.connect(db)+c.execute("PRAGMA journal_mode=WAL")+c.execute("CREATE TABLE obs (id INTEGER PRIMARY KEY, x REAL)")+c.executemany("INSERT INTO obs VALUES (?, ?)", [(i, i * %r) for i in range(rows)])+c.commit()+os.kill(os.getpid(), signal.SIGKILL) # what `dimos stop` escalates to+""" % STEP_M+++def killed_recording(d: Path, rows: int = 500) -> Path:+ """A run that committed *rows* poses and was then SIGKILLed: db + -wal + -shm."""+ db = d / "moss.db"+ proc = subprocess.run([sys.executable, "-c", WRITER, str(db), str(rows)],+ capture_output=True, timeout=120)+ assert proc.returncode == -9, f"writer should die by SIGKILL, rc={proc.returncode}"+ for suffix in ("", "-wal", "-shm"):+ assert db.with_name(db.name + suffix).exists(), f"missing moss.db{suffix}"+ return db+++def read_rows(db: Path):+ """(row count, x of the last pose), read the way an offline reader opens+ a recording: read-only URI."""+ conn = sqlite3.connect(f"file:{db}?mode=ro", uri=True)+ try:+ n = conn.execute("SELECT count(*) FROM obs").fetchone()[0]+ x = conn.execute("SELECT x FROM obs ORDER BY id DESC LIMIT 1").fetchone()[0]+ return n, x+ except sqlite3.OperationalError as e:+ # a recording whose -wal was left behind: empty schema, nothing to read+ print(f" sqlite: {e}")+ return 0, float("nan")+ finally:+ conn.close()+++def digest(p: Path) -> str:+ return hashlib.sha256(p.read_bytes()).hexdigest()+++def names(d: Path):+ return sorted(p.name for p in d.iterdir())+++ROTATED = re.compile(r"^moss\.\d{14}(-\d+)?\.db$")+root = Path(tempfile.mkdtemp(prefix="moss-rotation-"))++# --- A. what a SIGKILL leaves behind --------------------------------------+print("A. a killed run's rows live in the -wal, not in the .db")+a = root / "a"+a.mkdir()+db = killed_recording(a, rows=500)+check("the writer died by SIGKILL and left the trio",+ names(a) == ["moss.db", "moss.db-shm", "moss.db-wal"], str(names(a)))+check("the -wal is where the bytes are",+ db.with_name("moss.db-wal").stat().st_size > 20_000,+ f"{db.with_name('moss.db-wal').stat().st_size} B of -wal "+ f"vs {db.stat().st_size} B of .db")++alone = root / "a_db_alone"+alone.mkdir()+shutil.copy2(db, alone / "moss.db") # the .db moved by itself, as backup_file does+n, _ = read_rows(alone / "moss.db")+check("the .db by itself has an empty schema (the pre-fix loss)", n == 0, f"{n} rows")++# --- B. the rotation moves the trio ---------------------------------------+print("B. rotation renames db + -wal + -shm together")+rotated = M.rotate_recording(db, keep_last=10)+check("rotate_recording returns the rotated db", rotated is not None and rotated.exists(),+ str(rotated))+check("named <stem>.<stamp>.db (moss.<stamp>.db), the rotated-recording pattern",+ bool(ROTATED.match(rotated.name)), rotated.name)+check("nothing of the previous run is left at the live path",+ not any(db.with_name(db.name + s).exists() for s in ("", "-wal", "-shm")),+ str(names(a)))+check("the whole trio moved",+ all(rotated.with_name(rotated.name + s).exists() for s in ("-wal", "-shm")),+ str(names(a)))+n, x = read_rows(rotated)+check("the rotated recording reads back its 500 poses", n == 500, f"{n} rows")+check("... and the last one is at 124.75 m", abs(x - 124.75) < 1e-9, f"x = {x} m")++fresh = sqlite3.connect(str(db)) # the next run opens its own moss.db+fresh.execute("PRAGMA journal_mode=WAL")+fresh.execute("CREATE TABLE obs (id INTEGER PRIMARY KEY, x REAL)")+fresh.execute("INSERT INTO obs VALUES (0, 0.5)")+fresh.commit()+fresh.close()+n, x = read_rows(rotated)+check("the new run does not touch it: still 500 poses at 124.75 m",+ n == 500 and abs(x - 124.75) < 1e-9, f"{n} rows, x = {x} m")++# --- C. the torn trio: -wal/-shm with no db --------------------------------+print("C. orphaned -wal/-shm -> dated quarantine, intact")+c = root / "c"+c.mkdir()+db = killed_recording(c, rows=500)+widowed = c / "moss.20260101000000.db"+db.rename(widowed) # the pre-fix rotation: the .db alone+before = {p.name: (p.stat().st_size, digest(p)) for p in+ (c / "moss.db-wal", c / "moss.db-shm")}+check("the state a pre-fix rotation leaves: sidecars with no db",+ names(c) == ["moss.20260101000000.db", "moss.db-shm", "moss.db-wal"],+ str(names(c)))++check("rotate_recording has no recording to rotate", M.rotate_recording(db) is None)+quarantine = c / "quarantine"+dated = sorted(quarantine.iterdir()) if quarantine.is_dir() else []+check("a dated quarantine directory was created",+ len(dated) == 1 and re.match(r"^\d{14}(-\d+)?$", dated[0].name),+ str([p.name for p in dated]))+if dated:+ after = {p.name: (p.stat().st_size, digest(p)) for p in dated[0].iterdir()}+ check("both sidecars are in it, byte for byte", after == before,+ f"{sorted(after)} vs {sorted(before)}")+check("and none is left beside the live db",+ not (c / "moss.db-wal").exists() and not (c / "moss.db-shm").exists(),+ str(names(c)))++fresh = sqlite3.connect(str(db)) # the next run, on a clean slate+fresh.execute("PRAGMA journal_mode=WAL")+fresh.execute("CREATE TABLE obs (id INTEGER PRIMARY KEY, x REAL)")+fresh.execute("INSERT INTO obs VALUES (0, 0.5)")+fresh.commit()+fresh.close()+n, x = read_rows(db)+check("the new recording is fresh (1 pose at 0.5 m)", n == 1 and abs(x - 0.5) < 1e-9,+ f"{n} rows, x = {x} m")+if dated:+ still = {p.name: (p.stat().st_size, digest(p)) for p in dated[0].iterdir()}+ check("the quarantined -wal survived the new run", still == before)+ # nothing was lost: paired back with its own db, the killed run reads out+ for p in dated[0].iterdir():+ shutil.copy2(p, c / p.name.replace("moss.db", widowed.name))+ n, x = read_rows(widowed)+ check("re-paired with its db it still holds 500 poses, last at 124.75 m",+ n == 500 and abs(x - 124.75) < 1e-9, f"{n} rows, x = {x} m")++# --- D. no overwrite, and the prune retires whole trios ---------------------+print("D. two rotations in the same second, then the prune")+d = root / "d"+d.mkdir()+db = d / "moss.db"+killed_recording(d, rows=500)+first = M.rotate_recording(db, keep_last=10)+killed_recording(d, rows=300)+second = M.rotate_recording(db, keep_last=10)+check("the second rotation took a free name", first != second, f"{first.name} / {second.name}")+n1, x1 = read_rows(first)+n2, x2 = read_rows(second)+check("the first recording still holds its own 500 poses (124.75 m)",+ n1 == 500 and abs(x1 - 124.75) < 1e-9, f"{n1} rows, x = {x1} m")+check("the second holds its own 300 poses (74.75 m)",+ n2 == 300 and abs(x2 - 74.75) < 1e-9, f"{n2} rows, x = {x2} m")++killed_recording(d, rows=200)+third = M.rotate_recording(db, keep_last=2)+kept = [p.name for p in d.iterdir() if ROTATED.match(p.name)]+check("keep_last=2 retires the oldest recording", sorted(kept) == sorted(+ [second.name, third.name]), str(sorted(kept)))+check("... the whole trio of it, no widowed -wal left to shadow a later run",+ all((p.parent / p.name[:-len("-wal")]).exists()+ for p in d.iterdir() if p.name.endswith("-wal")),+ str(names(d)))+n3, x3 = read_rows(third)+check("the newest recording is the one that reads back (200 poses, 49.75 m)",+ n3 == 200 and abs(x3 - 49.75) < 1e-9, f"{n3} rows, x = {x3} m")++# --- E. the module rotates on start ----------------------------------------+print("E. the recorders' start() is what rotates")+check("the recorders still ask dimOS for BACKUP (the rotation we replace)",+ M.RotatingRecorderConfig().on_existing is M.OnExisting.BACKUP+ and M.MossMemoryConfig().on_existing is M.OnExisting.BACKUP)+check("start() is ours, on both recorders, and still an rpc",+ M.RotatingRecorder.start is not Recorder.start+ and M.MossRecorder.start is M.RotatingRecorder.start+ and getattr(M.RotatingRecorder.start, "__rpc__", False) is True)+check("MossRecorder sits on RotatingRecorder (odom + global_costmap inherited)",+ M.MossRecorder.__mro__[1] is M.RotatingRecorder+ and {"odom", "global_costmap"} <= set(M.RotatingRecorder.__annotations__))+import os # noqa: E402+_saved_state = os.environ.pop("MOSS_STATE_DIR", None)+try:+ os.environ["MOSS_STATE_DIR"] = str(root / "state")+ _db = Path(M.MossMemoryConfig().db_path)+ check("MOSS_STATE_DIR=<tmp>/state -> MossMemoryConfig().db_path = <tmp>/state/recordings/moss.db (read at build time)",+ _db == root / "state" / "recordings" / "moss.db", str(_db))+ os.environ.pop("MOSS_STATE_DIR", None)+ _db = Path(M.RotatingRecorderConfig().db_path)+ check("default db_path = ~/.local/state/moss/recordings/moss.db, absolute",+ _db.is_absolute() and _db == Path.home() / ".local/state/moss/recordings/moss.db", str(_db))+finally:+ os.environ.pop("MOSS_STATE_DIR", None)+ if _saved_state is not None:+ os.environ["MOSS_STATE_DIR"] = _saved_state+check("MossMemoryConfig: the command and telemetry streams are poseless, tf not recorded",+ M.MossMemoryConfig().poseless_streams == ["global_costmap", "cmd_vel", "base_telemetry", "episode_marker"]+ and M.MossMemoryConfig().record_tf is False)++saved_start, Recorder.start = Recorder.start, lambda self: None # no store, no bus+try:+ e = root / "e"+ e.mkdir()+ db = killed_recording(e, rows=500)+ M.RotatingRecorder(db_path=str(db)).start()+ live = [p.name for p in e.iterdir() if ROTATED.match(p.name)]+ check("RotatingRecorder.start() rotated the trio", len(live) == 1 and not db.exists(), str(names(e)))+ if live:+ n, x = read_rows(e / live[0])+ check("... and the killed run reads back: 500 poses, last at 124.75 m",+ n == 500 and abs(x - 124.75) < 1e-9, f"{n} rows, x = {x} m")++ e2 = root / "e_append"+ e2.mkdir()+ db2 = killed_recording(e2, rows=500)+ M.RotatingRecorder(db_path=str(db2), on_existing="append").start()+ check("on_existing=append rotates nothing (upstream's business)",+ names(e2) == ["moss.db", "moss.db-shm", "moss.db-wal"], str(names(e2)))++ e3 = root / "e_replay"+ e3.mkdir()+ db3 = killed_recording(e3, rows=500)+ m3 = M.MossRecorder(db_path=str(db3))+ m3.config.g.replay = True+ try:+ m3.start()+ finally:+ m3.config.g.replay = False+ check("a replay leaves the recording untouched (MossRecorder)",+ names(e3) == ["moss.db", "moss.db-shm", "moss.db-wal"], str(names(e3)))+ e4 = root / "e_moss"+ e4.mkdir()+ db4 = killed_recording(e4, rows=300)+ M.MossRecorder(db_path=str(db4)).start()+ live4 = [p.name for p in e4.iterdir() if ROTATED.match(p.name)]+ check("MossRecorder.start() rotates the trio too", len(live4) == 1 and not db4.exists(), str(names(e4)))+ if live4:+ n, x = read_rows(e4 / live4[0])+ check("... and the killed run reads back: 300 poses, last at 74.75 m",+ n == 300 and abs(x - 74.75) < 1e-9, f"{n} rows, x = {x} m")+finally:+ Recorder.start = saved_start+ shutil.rmtree(root, ignore_errors=True)++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/benches/bench_moss_adapter_cold.py b/benches/bench_moss_adapter_cold.pynew file mode 100644index 0000000..99495eb--- /dev/null+++ b/benches/bench_moss_adapter_cold.py@@ -0,0 +1,418 @@+"""Cold bench: MossBaseAdapter on the fake MOSS firmware. Known in -> known out.++No hardware. dimOS only for section 0's gamepad clamp (skipped without it).+The fake runs the firmware's own state machine (Control.h, qtpy-mdd3a-v3:+arm / drive / stop / ramp, 300 ms drive deadman, 10 s arm expiry, +-100 %),+so what is proven here is the line protocol the Jetson speaks to the QT Py+over USB CDC, and the adapter's deadman on top of it.++Known input -> known output in percent, metres, radians, seconds.++Section 0's gamepad half needs dimOS (moss_dimos.gamepad imports it) and is+SKIPPED with a line where dimOS is not installed; any other import failure of+moss_dimos.gamepad is a KO.++Run: PYTHONDONTWRITEBYTECODE=1 PYTHONPATH=. python3 benches/bench_moss_adapter_cold.py+"""+import importlib.util+import math+import os+import re+import sys+import threading+import time+from pathlib import Path++sys.path.insert(0, str(Path(__file__).resolve().parent.parent))++from moss_dimos.moss_adapter import (FIRMWARE_MAX_PCT, KEEPALIVE_S, STALE_S,+ MossBaseAdapter, MossGeometry,+ teleop_envelope, teleop_speeds, tracks_to_twist,+ twist_to_tracks)+from moss_dimos.moss_mock import DRIVE, OFF, READY, FakeMossFirmware++OK = KO = 0+++def check(cond, label):+ global OK, KO+ if cond:+ OK += 1+ else:+ KO += 1+ print((" OK " if cond else " KO ") + label)+++def settle(s=0.06):+ time.sleep(s)+++# ── 0. gamepad scale: full stick = the base's limit, not the generic 0.6 m/s ──+print("0. teleop_speeds / teleop_envelope (track 0.30 m, 0.45 m/s at 100 %, turn gain 2)")+os.environ.pop("MOSS_TURN_GAIN", None)+G100 = MossGeometry(track_m=0.30, mps_at_limit=0.45, max_pct=100)+_lin, _ang = teleop_speeds(G100)+check(abs(_lin - 0.45) < 1e-9, "full stick forward -> 0.45 m/s (= 100/100)")+check(abs(_ang - 6.0) < 1e-9, "full stick sideways -> 6.0 rad/s (gain 2 x the 3.0 rad/s spin)")+check(twist_to_tracks(_lin, 0.0, G100) == (100, 100), "that twist IS 100/100")+check(twist_to_tracks(0.0, _ang / 2, G100) == (-100, 100), "half turn stick alone = full spin -100/+100")+check(teleop_speeds(MossGeometry(track_m=0.0, mps_at_limit=0.45, max_pct=100)) == (0.45, 0.0), "track 0 -> no spin, no crash")+env = teleop_envelope(G100)+check(all(abs(env[k] - v) < 1e-9 for k, v in {"linear": 0.45, "angular": 6.0, "lever": 0.15, "rim": 1.35}.items()), f"envelope: {env}")+HAVE_DIMOS = False+SKIP_GAMEPAD = None+try:+ from moss_dimos.gamepad import TwistEnvelope, clamp_twist # gamepad.py imports dimOS+ HAVE_DIMOS = True+except ImportError as exc:+ if importlib.util.find_spec("dimos") is None:+ SKIP_GAMEPAD = f"no dimOS here ({exc}): run this on the robot"+ else:+ check(False, f"moss_dimos.gamepad imports (TwistEnvelope, clamp_twist): {type(exc).__name__}: {exc}")+if HAVE_DIMOS:+ E = TwistEnvelope(**env)+ def tracks(vx, wz):+ c = clamp_twist(vx, 0.0, wz, E); return twist_to_tracks(c[0], c[2], G100)+ check(tracks(0.45, 0.0) == (100, 100), "full forward through the gamepad clamp -> 100/100")+ check(tracks(0.0, 6.0) == (-100, 100), "full turn alone -> full spin -100/+100")+ check(tracks(0.45, 3.0) == (0, 100), "full forward + HALF turn -> inner track stops (0/100)")+ check(tracks(0.45, 6.0) == (-100, 100), "full forward + FULL turn -> inner track full reverse (-100/100): the tank pivot")+ check(tracks(0.45, 1.92) == (36, 100), f"full forward + turn just past the deadzone -> inner slows (36/100): {tracks(0.45, 1.92)}")+ os.environ["MOSS_TURN_GAIN"] = "1"+ _l1, _a1 = teleop_speeds(G100); E1 = TwistEnvelope(**teleop_envelope(G100))+ _c = clamp_twist(0.45, 0.0, _a1, E1)+ check(abs(_a1 - 3.0) < 1e-9 and twist_to_tracks(_c[0], _c[2], G100) == (0, 100), "MOSS_TURN_GAIN=1 -> full forward + full turn only stops the inner track (0/100)")+ os.environ.pop("MOSS_TURN_GAIN", None)+ check(clamp_twist(9.0, 0.0, 0.0) == (0.45, 0.0, 0.0), "no envelope given -> the gamepad's conservative default ceiling (0.45 m/s)")+elif SKIP_GAMEPAD:+ print(f" -- SKIP the gamepad clamp half: {SKIP_GAMEPAD}")+++# ── 1. pure mapping: twist -> track percent ─────────────────────────────+G = MossGeometry(track_m=0.30, mps_at_limit=0.15, max_pct=25, ticks_per_m=None)+print("1. twist -> tracks (track 0.30 m, 0.15 m/s at 25 %)")+check(twist_to_tracks(0.15, 0.0, G) == (25, 25), "vx=0.15 -> L=25 R=25 (the limit)")+check(twist_to_tracks(0.06, 0.0, G) == (10, 10), "vx=0.06 -> L=10 R=10")+check(twist_to_tracks(-0.06, 0.0, G) == (-10, -10), "vx=-0.06 -> -10/-10 (reverse)")+check(twist_to_tracks(0.0, 1.0, G) == (-25, 25), "wz=+1 rad/s -> L=-25 R=+25 (spin left)")+check(twist_to_tracks(0.30, 0.0, G) == (25, 25), "vx=0.30 (2x the limit) -> clamped 25/25")+check(twist_to_tracks(0.001, 0.0, G) == (0, 0), "vx=0.001 rounds to 0 -> a stop")+from moss_dimos.moss_adapter import CONNECT_PROBE_S, REPLY_TIMEOUT_S as _RT # noqa: E402+check(CONNECT_PROBE_S >= 1.0 and _RT < CONNECT_PROBE_S,+ f"connect probe waits >= 1 s for the first status (reply tick {_RT} s): the first status can take longer than one reply tick")+++# ... and connect() really uses that window, not the reply tick (a cold reader+# thread on the robot's computer answers the first 'status' late).+class _MuteFirmware(FakeMossFirmware):+ """Takes every line, never answers (a board with no firmware, or the wrong port)."""++ def write(self, data):+ return len(data)+++class _LateStatus(FakeMossFirmware):+ """Answers the first 'status' only after `delay` s, like a cold reader thread."""++ def __init__(self, delay):+ super().__init__()+ self.delay, self._first = delay, True++ def write(self, data):+ if self._first and data.strip() == b"status":+ self._first = False+ threading.Timer(self.delay, FakeMossFirmware.write, (self, data)).start()+ return len(data)+ return super().write(data)+++def _factory(fw_):+ f = lambda: fw_ # noqa: E731+ f.is_mock = True+ return f+++_a = MossBaseAdapter(dof=3, port_factory=_factory(_MuteFirmware()), geometry=G)+_t0 = time.monotonic()+_ok = _a.connect()+_dt = time.monotonic() - _t0+check(_ok is False and 0.9 * CONNECT_PROBE_S <= _dt < CONNECT_PROBE_S + 0.5,+ f"mute firmware: connect() waits the {CONNECT_PROBE_S} s probe window, then refuses "+ f"(took {_dt:.2f} s, connect() is {_ok}; the reply tick alone would be {_RT} s)")+_a.disconnect()+_a = MossBaseAdapter(dof=3, port_factory=_factory(_LateStatus(0.5)), geometry=G)+_ok = _a.connect()+check(_ok is True, f"first status 0.5 s late (over the {_RT} s reply tick, inside the "+ f"{CONNECT_PROBE_S} s window) -> connect() is True ({_ok})")+_a.disconnect()+GF = MossGeometry(track_m=0.30, mps_at_limit=0.15, max_pct=25, ticks_per_m=None, min_pct=12)+check(twist_to_tracks(0.06, 0.0, GF) == (12, 12), "floor 12: vx=0.06 (10 %) -> raised to L=12 R=12")+check(twist_to_tracks(-0.06, 0.0, GF) == (-12, -12), "floor 12: vx=-0.06 -> -12/-12 (sign kept)")+check(twist_to_tracks(0.0, 0.3, GF) == (-12, 12), "floor 12: tiny spin (-5/+5) -> -12/+12")+check(twist_to_tracks(0.001, 0.0, GF) == (0, 0), "floor 12: a twist that rounds to 0 is STILL a stop")+check(twist_to_tracks(0.15, 0.0, GF) == (25, 25), "floor 12: the limit is untouched")+check(twist_to_tracks(0.09, 0.0, GF) == (15, 15), "floor 12: 15 % above the floor passes as is")+check(twist_to_tracks(0.06, 0.0, G) == (10, 10), "floor 0 (default): L=10 R=10 unchanged")+check(twist_to_tracks(0.06, 0.0, MossGeometry(track_m=0.30, mps_at_limit=0.15, max_pct=25, min_pct=99)) == (25, 25),+ "a floor above the limit is clamped to the limit")+check(twist_to_tracks(0.15, 0.0, MossGeometry(mps_at_limit=0.15, max_pct=10)) == (10, 10),+ "max_pct=10 -> the same full-scale twist lands on L=10 R=10")+vx, vy, wz = tracks_to_twist(-0.15, 0.15, G)+check(abs(vx) < 1e-9 and abs(wz - 1.0) < 1e-9 and vy == 0.0,+ "tracks (-0.15, +0.15) m/s -> vx=0 wz=+1.0 (inverse of the above)")++# ── 2. the adapter drives the fake firmware ────────────────────────────+print("2. adapter on the fake firmware: connect, arm, drive, stop")+fw = FakeMossFirmware()+factory = lambda: fw+factory.is_mock = True+a = MossBaseAdapter(dof=3, port_factory=factory, geometry=G)+check(a.connect() is True, "connect() probes with 'status' and gets telemetry")+check(fw.commands[:1] == ["status"], f"first line on the wire is 'status': {fw.commands[:1]}")+check(a.get_dof() == 3, "get_dof() == 3 (vy ignored at the tracks)")+check(a.read_odometry() is None, "no ticks_per_m -> read_odometry() is None (honest)")+check(a.write_velocities([0.15, 0.0, 0.0]) is False and "drive 25 25" not in fw.commands,+ "drive before enable is refused and never reaches the wire")+check(a.write_enable(True) and a.read_enabled() and fw.mode == READY,+ "write_enable(True) = 'arm' -> firmware READY")+check(a.write_velocities([0.15, 0.4, 0.0]) is True,+ "write_velocities(vx=0.15, vy=0.4 ignored) accepted")+check(fw.commands[-2:] == ["arm", "drive 25 25"] and fw.mode == DRIVE and (fw.left, fw.right) == (25, 25),+ f"-> 'arm' then 'drive 25 25' on the wire, nothing else, firmware DRIVE 25/25: {fw.commands[-2:]}")+check(a.read_velocities() == [0.15, 0.0, 0.0],+ "read_velocities() = the commanded twist (open loop, no encoder calibration)")+n_before = fw.commands.count("drive 25 25")+settle(KEEPALIVE_S * 3.5)+n_after = fw.commands.count("drive 25 25")+check(n_after - n_before >= 2,+ f"keepalive re-sends the drive every {KEEPALIVE_S*1000:.0f} ms: +{n_after - n_before} in 350 ms")+check(a.write_stop() is True and fw.commands[-1] == "stop" and fw.mode == OFF,+ "write_stop() = 'stop' -> firmware OFF")+check(a.read_velocities() == [0.0, 0.0, 0.0], "after stop, commanded velocity is zero")++# ── 3. the adapter deadman: a commander that goes quiet ────────────────+print("3. deadman: no fresh twist -> one 'stop', tracks held")+check(a.write_velocities([0.06, 0.0, 0.0]) and fw.mode == DRIVE, "drive L=10 R=10 again")+settle(STALE_S + 0.25)+tail = fw.commands[-3:]+check(fw.commands[-1] == "stop" and fw.mode == OFF,+ f"after {STALE_S}s without a write: 'stop' sent, firmware OFF (tail {tail})")+n_stop = fw.commands.count("stop")+settle(KEEPALIVE_S * 3)+check(fw.commands.count("stop") == n_stop, "the stop is sent ONCE, not hammered")+check(a.target_pct == (0, 0), "target zeroed")++# ── 4. arm expiry in the firmware, seen through telemetry ──────────────+print("4. the firmware disarms itself (10 s idle) -> 'ERR not_armed' -> re-arm, retry")+check(a.write_enable(True) and fw.mode == READY, "armed again")+fw.advance(10_001) # firmware: READY for 10 s -> OFF 'arm_expired', telemetry queued+settle(0.1)+check(fw.mode == OFF and fw.reason == "arm_expired", f"firmware OFF ({fw.reason})")+check(a.firmware_mode == OFF, "adapter read mode OFF from telemetry")+before = len(fw.commands)+check(a.write_velocities([0.06, 0.0, 0.0]) is True, "a new drive is accepted")+check(fw.commands[before:] == ["drive 10 10", "arm", "drive 10 10"] and fw.mode == DRIVE,+ f"-> refused, 'arm', resent (never an 'arm' on a guess): {fw.commands[before:]}")++# ── 5. 'ERR not_armed' without telemetry -> re-arm and retry once ───────+print("5. firmware stopped silently (no telemetry) -> the same path, no guessing")+fw._stop("timeout") # no telemetry line emitted: the adapter cannot know+a.write_stop() # adapter-side reset to a clean known state+check(a.write_enable(True) and fw.mode == READY, "armed")+fw._stop("timeout")+before = len(fw.commands)+check(a.write_velocities([0.06, 0.0, 0.0]) is True, "drive accepted after the retry")+check(fw.commands[before:] == ["drive 10 10", "arm", "drive 10 10"] and fw.mode == DRIVE,+ f"-> refused, 'arm', resent: {fw.commands[before:]}")+a.write_stop()++# ── 6. odometry from ticks when calibrated ─────────────────────────────+print("6. ticks_per_m set -> velocity and pose from encoder ticks")+G2 = MossGeometry(track_m=0.30, mps_at_limit=0.15, max_pct=25, ticks_per_m=1000.0)+fw2 = FakeMossFirmware()+f2 = lambda: fw2+f2.is_mock = True+b = MossBaseAdapter(dof=3, port_factory=f2, geometry=G2)+check(b.connect(), "connect (calibrated geometry)")+settle()+fw2.ticks = [100, 100]+fw2.emit_telemetry()+settle()+x, y, th = b.read_odometry()+check(abs(x - 0.10) < 1e-6 and abs(y) < 1e-6 and abs(th) < 1e-6,+ f"+100 ticks both sides (1000/m) -> x=+0.100 m, y=0, th=0: {x:.3f} {y:.3f} {th:.3f}")+fw2.ticks = [50, 150]+fw2.emit_telemetry()+settle()+x, y, th = b.read_odometry()+check(abs(x - 0.10) < 1e-6 and abs(th - (0.10 / 0.30)) < 1e-6,+ f"then -50/+50 ticks -> x unchanged, th=+0.333 rad (0.1 m over 0.30 m track): th={th:.3f}")+vx, vy, wz = b.read_velocities()+check(abs(vx) < 1e-9 and wz > 0, f"read_velocities() from ticks: vx=0, wz>0 ({wz:+.2f})")+b.disconnect()+check(fw2.commands[-1] == "stop" and not b.is_connected(), "disconnect() sends 'stop'")++# ── 7. geometry from env: bounds and mistyped values ───────────────────+print("7. MossGeometry.from_env")+import os+os.environ.update({"MOSS_MAX_PCT": "140", "MOSS_TICKS_PER_M": "abc", "MOSS_TRACK_M": "0.28"})+g = MossGeometry.from_env()+check(g.max_pct == FIRMWARE_MAX_PCT, f"MOSS_MAX_PCT=140 clamped to the firmware's {FIRMWARE_MAX_PCT}")+check(g.ticks_per_m is None, "MOSS_TICKS_PER_M=abc -> None (no odometry, not a random scale)")+check(g.track_m == 0.28, "MOSS_TRACK_M=0.28 taken")+for k in ("MOSS_MAX_PCT", "MOSS_TICKS_PER_M", "MOSS_TRACK_M"):+ os.environ.pop(k, None)+os.environ["MOSS_MIN_PCT"] = "40"+check(MossGeometry.from_env().min_pct == 40, "MOSS_MIN_PCT=40 -> min_pct 40")+os.environ["MOSS_MIN_PCT"] = "150"+check(MossGeometry.from_env().min_pct == MossGeometry.from_env().max_pct, "MOSS_MIN_PCT above the ceiling -> clamped to max_pct")+os.environ["MOSS_MIN_PCT"] = "-5"+check(MossGeometry.from_env().min_pct == 0, "MOSS_MIN_PCT negative -> 0")+os.environ.pop("MOSS_MIN_PCT", None)+check(MossGeometry.from_env().min_pct == 0, "MOSS_MIN_PCT unset -> 0 (floor off, unchanged behaviour)")++# ── 8. the fake is the firmware: its own refusals ──────────────────────+print("8. fake firmware fidelity (Control.h)")+fw3 = FakeMossFirmware()+fw3.write(b"drive 10 10\n")+check(fw3.sent[-1] == "ERR not_armed" and fw3.mode == OFF, "drive before arm -> ERR not_armed")+fw3.write(b"arm\n")+fw3.write(b"drive 26 0\n")+check(fw3.sent[-1] == "OK" and fw3.mode == DRIVE and (fw3.left, fw3.right) == (26, 0),+ "26 % is valid since v3 (limit 100) -> OK, DRIVE 26/0")+fw3.write(b"drive 101 0\n")+check(fw3.sent[-1] == "ERR invalid_command" and fw3.mode == OFF and fw3.reason == "invalid",+ "101 % -> ERR invalid_command, OFF 'invalid'")+fw3.write(b"arm\ndrive -100 100\n")+check(fw3.sent[-1] == "OK" and (fw3.left, fw3.right) == (-100, 100), "arm + drive -100 100 (the full pivot) -> OK")+fw3.write(b"stop\n") # 'arm' while DRIVE is arm_while_moving (a stop), so stop first+fw3.write(b"arm\ndrive 5 -5\n")+check(fw3.mode == DRIVE and (fw3.left, fw3.right) == (5, -5), "arm + drive 5 -5 -> DRIVE 5/-5")+fw3.advance(301)+check(fw3.mode == OFF and fw3.reason == "timeout", "301 ms without a drive -> OFF 'timeout'")+a.disconnect()++# ── 9. v3 firmware surface: ramp command and telemetry keys ────────────+print("9. fake firmware v3: 'ramp UP DOWN' and the telemetry keys")+import json+fw4 = FakeMossFirmware()+check("limit 100%" in fw4.sent[0] and "qtpy-mdd3a-v3" in fw4.sent[0], f"banner: {fw4.sent[0]!r}")+fw4.write(b"status\n")+tel = json.loads(fw4.sent[-1])+check(tel.get("fw") == "qtpy-mdd3a-v3" and tel.get("ramp_ms") == [30, 15],+ f"boot telemetry: fw qtpy-mdd3a-v3, ramp 30 / 15 ms per % ({tel.get('fw')}, {tel.get('ramp_ms')})")+check(tel.get("ina_ok") is True and tel.get("bus_mV") == 11600 and tel.get("shunt_uV") == 0+ and tel.get("current_mA") is None, "battery: ina_ok, 11600 mV (11.6 V), shunt 0, current null")+check("driver" not in tel, "no 'driver' key (only the MDD10A build sends it)")+fw4.write(b"ramp 5 5\n")+check(fw4.sent[-1] == "OK" and fw4.ramp_ms == [5, 5], "ramp 5 5 -> OK, 5/5 ms per %")+fw4.write(b"arm\ndrive 40 40\nramp 10 500\n")+check(fw4.sent[-1] == "OK" and fw4.mode == DRIVE and fw4.ramp_ms == [10, 500],+ "ramp 10 500 while driving -> OK, still DRIVE (a ramp change never stops)")+fw4.emit_telemetry()+tel = json.loads(fw4.sent[-1])+check(tel["ramp_ms"] == [10, 500] and tel["target_pct"] == [40, 40] and tel["output_pct"] == [40, 40],+ "telemetry reports ramp 10/500; output_pct == target_pct (ramp not simulated)")+for bad in ("ramp 0 5", "ramp 5 501", "ramp x 5"):+ fw4.write(b"arm\n")+ fw4.write((bad + "\n").encode())+ check(fw4.sent[-1] == "ERR invalid_command" and fw4.mode == OFF and fw4.ramp_ms == [10, 500],+ f"'{bad}' -> ERR invalid_command, OFF, ramp unchanged")++# ── 10. adapter: telemetry age, default port, geometry default ────────+print("10. adapter telemetry_age_s, default_port (QT Py by-id, never ttyUSB0), track 0.266")+import tempfile+from moss_dimos import robot_profile as rp+from moss_dimos.moss_adapter import default_port+fw5 = FakeMossFirmware()+f5 = lambda: fw5+f5.is_mock = True+c = MossBaseAdapter(dof=3, port_factory=f5, geometry=G)+check(c.telemetry_age_s is None, "before connect: telemetry_age_s is None")+check(c.connect(), "connect")+age0 = c.telemetry_age_s+settle(0.25)+age1 = c.telemetry_age_s+check(age0 is not None and 0.0 <= age0 < 0.2 and 0.2 <= age1 - age0 < 0.5,+ f"age right after connect {age0:.3f} s, +0.25 s later {age1:.3f} s")+fw5.emit_telemetry()+settle()+check(c.telemetry_age_s < 0.2, f"a new line resets the age ({c.telemetry_age_s:.3f} s)")+check(c.telemetry.get("bus_mV") == 11600 and c.telemetry is not c.telemetry, "telemetry is a copy of the last line")+c.disconnect()+saved_root, saved_port = rp.BYID_ROOT, os.environ.pop("MOSS_PORT", None)+with tempfile.TemporaryDirectory() as d:+ rp.BYID_ROOT = d+ check(default_port() == "/dev/serial/by-id/MISSING-QT-PY",+ f"no QT Py, MOSS_PORT unset -> the sentinel: {default_port()}")+ lidar = os.path.join(d, "usb-Silicon_Labs_CP2102N_USB_to_UART_Bridge_Controller_x-if00-port0")+ open(lidar, "w").close()+ check(default_port() == "/dev/serial/by-id/MISSING-QT-PY", "a CP2102N alone is never taken for the base")+ qt = os.path.join(d, "usb-Adafruit_QT_Py_ESP32-S3_No_PSRAM_00:00:00:00:00:00-if00")+ open(qt, "w").close()+ check(default_port() == qt, f"QT Py present -> its by-id path: {os.path.basename(default_port())}")+ check(MossBaseAdapter(port_factory=f5)._port == qt, "an adapter built without address takes it")+ os.environ["MOSS_PORT"] = "/dev/ttyACM9"+ check(default_port() == "/dev/ttyACM9", "MOSS_PORT wins")+ os.environ.pop("MOSS_PORT", None)+rp.BYID_ROOT = saved_root+if saved_port is not None:+ os.environ["MOSS_PORT"] = saved_port+check("ttyUSB0" not in default_port(), "the default is never /dev/ttyUSB0 (the lidar on MOSS)")+os.environ.pop("MOSS_TRACK_M", None)+check(MossGeometry().track_m == 0.266 and MossGeometry.from_env().track_m == 0.266,+ "MossGeometry track default 0.266 m (robot_profile.MOSS, CAD placeholder)")++# ── 11. the mock bus on the wall clock (MOSS_MOCK_BUS runs) ──────────────+print("11. MOSS_MOCK_BUS: live 200 ms telemetry and a firmware deadman that fires")+fw6 = FakeMossFirmware(auto_time=True)+fw6.write(b"arm\ndrive 10 10\n")+check(fw6.mode == DRIVE, "auto-time fake: arm + drive 10 10 -> DRIVE")+t_end = time.monotonic() + 0.45+while time.monotonic() < t_end:+ fw6.readline()+check(fw6.mode == OFF and fw6.reason == "timeout", f"no refresh for 0.45 s -> OFF '{fw6.reason}' (300 ms deadman)")+n_tel = sum(1 for line in fw6.sent if line.startswith("{"))+check(n_tel >= 2, f"telemetry every 200 ms while polled: {n_tel} lines in 0.45 s")+os.environ["MOSS_MOCK_BUS"] = "1"+m = MossBaseAdapter(dof=3, geometry=G)+check(m.connect() and m.mock_bus, "MOSS_MOCK_BUS=1 -> the adapter connects to the fake, mock_bus True")+settle(0.6)+check(m.telemetry_age_s is not None and m.telemetry_age_s < 0.35,+ f"telemetry stays fresh on the mock bus: age {m.telemetry_age_s:.3f} s after 0.6 s")+check(m.write_enable(True) and m.write_velocities([0.06, 0.0, 0.0]), "arm + drive through the mock bus")+settle(0.35)+check(m.telemetry.get("mode") == DRIVE and m.telemetry.get("target_pct") == [10, 10],+ f"keepalive holds DRIVE L=10 R=10 past the 300 ms firmware deadman: {m.telemetry.get('mode')} {m.telemetry.get('target_pct')}")+m.disconnect()+# the switch is parsed by robot_profile.env_on, like MOSS_CAMERA / MOSS_RECORD+os.environ["MOSS_MOCK_BUS"] = " Y "+m = MossBaseAdapter(dof=3, geometry=G)+check(m.connect() and m.mock_bus, "MOSS_MOCK_BUS=' Y ' -> the fake too (any value but 0/false/no/off)")+m.disconnect()+_saved_port = os.environ.get("MOSS_PORT")+os.environ["MOSS_PORT"] = "/dev/serial/by-id/MISSING-QT-PY"+for _off in ("0", "OFF", " false ", "No"):+ os.environ["MOSS_MOCK_BUS"] = _off+ m = MossBaseAdapter(dof=3, geometry=G)+ check(m.connect() is False and not m.mock_bus,+ f"MOSS_MOCK_BUS={_off!r} -> no fake: the real port is opened (and fails here, no board)")+if _saved_port is None:+ os.environ.pop("MOSS_PORT", None)+else:+ os.environ["MOSS_PORT"] = _saved_port+os.environ.pop("MOSS_MOCK_BUS", None)++# ── 12. public-repo hygiene: the adapter and the fake carry no journal ──+print("12. moss_adapter.py / moss_mock.py: comments explain the code, no dated journal")+for _name in ("moss_adapter.py", "moss_mock.py"):+ _src = (Path(__file__).resolve().parent.parent / "moss_dimos" / _name).read_text()+ _hits = (re.findall(r"\b[0-3][0-9]/[01][0-9]\b", _src) # day/month dates+ + re.findall(r"\b[0-9]{1,2}h[0-5][0-9]\b", _src) # clock stamps+ + re.findall(r"\b20[0-9]{2}-[01][0-9]-[0-3][0-9]\b", _src)) # ISO dates+ check(_hits == [], f"{_name}: no dated journal comment (day/month, clock stamp, ISO date): hits {_hits}")++print()+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/benches/bench_moss_blueprints_cold.py b/benches/bench_moss_blueprints_cold.pynew file mode 100644index 0000000..97623ee--- /dev/null+++ b/benches/bench_moss_blueprints_cold.py@@ -0,0 +1,632 @@+"""Cold bench for moss_dimos.blueprints: devices, the import, the four+blueprints, the coordinator in a fresh worker.++Known input -> known output in physical units. Groups:++ A. devices - moss_ports() on a fake /dev/serial/by-id: the QT Py and the+ C1 resolved by glob, the sentinel when one is missing (never+ another device, never /dev/ttyUSB0), MOSS_PORT /+ MOSS_LIDAR_PORT win+ B. the import - importing the module in a clean interpreter builds no+ blueprint, loads no camera / lidar / pad module, sets no+ environment variable, registers 'moss' with the exact+ factory path and installs the LCM queue clamp+ C. state dir - recordings at ~/.local/state/moss/recordings/moss.db,+ MOSS_STATE_DIR moves it, always absolute+ D. telemetry - telemetry_payload(adapter_like, now) -> the known document+ (driver None when the build does not send it, the age in+ seconds, the mock flag), None for an adapter without+ telemetry; the real adapter on the fake firmware+ -> 11.6 V, fw qtpy-mdd3a-v3, JSON round trip+ E. the camera - the base_link -> camera_link mount from the profile+ (pitch -> quaternion)+ H. wrist index - wrist_camera_index() on a fake /dev/v4l/by-id+ F. entry points - pyproject: distribution moss-dimos, the four entry points+ on the four lazy builders; then each one resolved through+ dimOS's own `dimos run` lookup (get_by_name) from a+ temporary dist-info built from pyproject.toml+ G. blueprints - moss-base, moss-gamepad, moss-nav and moss-explore build:+ module names, remaps, the pad's MOSS envelope, JPEG camera+ transport, recorder path, explorer clearances from the MOSS+ profile, no gamepad -> cmd_vel remap in explore; switches+ 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+ 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+ MossBaseAdapter from the factory string, the LCM clamp is+ on, no other moss_dimos module came along. WristCamera+ pickles by reference both ways. A conflicting 'moss'+ registration is reported with both paths, not swallowed.++Needs dimOS (the module imports it). No hardware.++Run: PYTHONDONTWRITEBYTECODE=1 PYTHONPATH=. .venv/bin/python benches/bench_moss_blueprints_cold.py+"""+import importlib+import json+import math+import os+import pickle+import re+import shutil+import subprocess+import sys+import tempfile+import textwrap+import time+from pathlib import Path++ROOT = Path(__file__).resolve().parent.parent+sys.path.insert(0, str(ROOT))++OK = 0+KO = 0+++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+++def close(a, b, tol=1e-9):+ return abs(a - b) <= tol+++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)+++CLEARED = [k for k in os.environ if k.startswith("MOSS_")] + ["ROBOT_PROFILE", "PERSISTENT_MAP"]+for var in CLEARED:+ os.environ.pop(var, None)++try:+ import dimos.core.coordination.blueprints # noqa: F401+except Exception as exc: # noqa: BLE001+ check("dimOS importable (moss_dimos.blueprints needs it: run this in the robot's venv)", False,+ f"{type(exc).__name__}: {exc}")+ finish()++from moss_dimos import robot_profile as rp # noqa: E402+from moss_dimos import blueprints as mb # noqa: E402++# Device names as /dev/serial/by-id spells them, with zeroed identifiers.+QTPY = "usb-Adafruit_QT_Py_ESP32-S3_No_PSRAM_00:00:00:00:00:00-if00"+C1 = "usb-Silicon_Labs_CP2102N_USB_to_UART_Bridge_Controller_" + "0" * 32 + "-if00-port0"+FACTORY = "moss_dimos.moss_adapter:MossBaseAdapter"+++def child_env(**extra):+ """A clean environment for a fresh interpreter: no MOSS_* / ROBOT_PROFILE,+ this checkout first on PYTHONPATH (the rest kept: dimOS may come from it)."""+ env = {k: v for k, v in os.environ.items() if k not in CLEARED and not k.startswith("MOSS_")}+ env["PYTHONPATH"] = str(ROOT) + os.pathsep + os.environ.get("PYTHONPATH", "")+ env["PYTHONDONTWRITEBYTECODE"] = "1"+ env.update(extra)+ return env+++def run_child(code, stdin=None, **extra):+ return subprocess.run([sys.executable, "-c", textwrap.dedent(code)], input=stdin,+ env=child_env(**extra), capture_output=True, timeout=180)+++def last_line(proc):+ lines = proc.stdout.decode(errors="replace").strip().splitlines()+ return lines[-1] if lines else ""+++# ── A. devices ────────────────────────────────────────────────────────────+print("A. moss_ports() on a fake /dev/serial/by-id")+saved_root = rp.BYID_ROOT+byid = tempfile.mkdtemp(prefix="byid_")+rp.BYID_ROOT = byid+ports = mb.moss_ports()+check("empty by-id -> both sentinels",+ ports == {"base": "/dev/serial/by-id/MISSING-QT-PY", "lidar": "/dev/serial/by-id/MISSING-C1"}, f"{ports}")+open(os.path.join(byid, C1), "w").close()+ports = mb.moss_ports()+check("C1 only -> lidar resolved, base still the sentinel (the CP2102N is never the base)",+ ports["lidar"] == os.path.join(byid, C1) and ports["base"] == rp.MOSS_QTPY_MISSING, f"{ports}")+open(os.path.join(byid, QTPY), "w").close()+ports = mb.moss_ports()+check("both plugged -> both by-id paths",+ ports == {"base": os.path.join(byid, QTPY), "lidar": os.path.join(byid, C1)},+ f"base {os.path.basename(ports['base'])}, lidar {os.path.basename(ports['lidar'])}")+os.environ["MOSS_PORT"] = "/dev/ttyACM7"+os.environ["MOSS_LIDAR_PORT"] = "/dev/ttyUSB7"+check("MOSS_PORT / MOSS_LIDAR_PORT win over the glob",+ mb.moss_ports() == {"base": "/dev/ttyACM7", "lidar": "/dev/ttyUSB7"})+os.environ.pop("MOSS_PORT")+os.environ.pop("MOSS_LIDAR_PORT")+check("explicit env dict is honoured (no os.environ read)",+ mb.moss_ports({"MOSS_PORT": "/x", "MOSS_LIDAR_PORT": "/y"}) == {"base": "/x", "lidar": "/y"})+check("no resolution ever lands on /dev/ttyUSB0",+ all("ttyUSB0" not in v for v in mb.moss_ports().values()))+++def refused(env):+ try:+ mb.moss_ports(env)+ except ValueError as exc:+ return str(exc)+ return None+++c1_path = os.path.join(byid, C1)+msg = refused({"MOSS_PORT": c1_path})+check("MOSS_PORT = the C1's by-id path (lidar from the glob) -> ValueError naming the device",+ msg is not None and "same device" in msg and C1 in msg, f"{msg}")+alias = os.path.join(byid, "alias-to-c1")+os.symlink(c1_path, alias)+check("MOSS_PORT = a symlink to the C1, MOSS_LIDAR_PORT = the C1 -> refused (realpath compare)",+ refused({"MOSS_PORT": alias, "MOSS_LIDAR_PORT": c1_path}) is not None)+check("the plugged QT Py + C1 -> no refusal", refused({}) is None)++# ── B. the import ────────────────────────────────────────────────────────+print("B. importing moss_dimos.blueprints in a clean interpreter")+probe = run_child("""+ import json, os, sys+ from dimos.hardware.drive_trains.registry import twist_base_adapter_registry as r+ before = r._factory_paths.get("moss")+ import moss_dimos.blueprints as b+ from dimos.protocol.pubsub.impl.lcmpubsub import LCM+ heavy = ["moss_dimos.gamepad", "moss_dimos.costmap2d", "moss_dimos.rplidar_c1",+ "moss_dimos.lidar_odometry", "moss_dimos.explorer2", "moss_dimos.recovering_planner",+ "moss_dimos.memory", "moss_dimos.c1_serial", "dimos.hardware.sensors.camera.module",+ "pygame", "kiss_icp"]+ print(json.dumps({+ "before": before, "after": r._factory_paths.get("moss"),+ "built": [k for k in ("moss_base_blueprint", "moss_gamepad_blueprint",+ "moss_nav_blueprint", "moss_explore_blueprint") if k in vars(b)],+ "heavy": [m for m in heavy if m in sys.modules],+ "env": {k: os.environ.get(k) for k in ("ROBOT_PROFILE", "PERSISTENT_MAP")},+ "clamp": hasattr(LCM.subscribe, "__wrapped__"),+ }))+""")+try:+ got = json.loads(last_line(probe))+except ValueError:+ got = None+ print(probe.stderr.decode(errors="replace")[-600:])+check("the import runs in a clean interpreter", got is not None, f"rc {probe.returncode}")+got = got or {}+check("'moss' unknown to the registry before the import", got.get("before") is None, f"{got.get('before')}")+check(f"after it: 'moss' -> {FACTORY} (the factory path is a string)", got.get("after") == FACTORY, f"{got.get('after')}")+check("no blueprint built at import (all four are lazy)", got.get("built") == [], f"{got.get('built')}")+check("no pad, lidar, map, planner, explorer, recorder or camera module imported with it",+ got.get("heavy") == [], f"{got.get('heavy')}")+check("no environment variable set by the import (ROBOT_PROFILE, PERSISTENT_MAP untouched)",+ got.get("env") == {"ROBOT_PROFILE": None, "PERSISTENT_MAP": None}, f"{got.get('env')}")+check("the LCM queue clamp is installed by the import (LCM.subscribe wrapped)", got.get("clamp") is True)+other = run_child("""+ import moss_dimos.robot_profile as rp+ print(rp.current().name)+""", ROBOT_PROFILE="other")+check("ROBOT_PROFILE=other is ignored: the profile is still MOSS, with a warning",+ last_line(other) == "moss" and b"ROBOT_PROFILE is ignored" in other.stderr + other.stdout,+ (other.stdout + other.stderr).decode(errors="replace").strip()[-200:])+try:+ mb.no_such_blueprint+ check("unknown attribute raises AttributeError", False)+except AttributeError:+ check("unknown attribute raises AttributeError", True)+try:+ mb._coordinator_blueprint()+ check("_coordinator_blueprint() without a base -> TypeError (no default base)", False)+except TypeError:+ check("_coordinator_blueprint() without a base -> TypeError (no default base)", True)++# ── C. state dir ─────────────────────────────────────────────────────────+print("C. the recording path")+home_db = Path.home() / ".local/state/moss/recordings/moss.db"+check("default ~/.local/state/moss/recordings/moss.db", rp.moss_recording_path() == home_db, str(rp.moss_recording_path()))+os.environ["MOSS_STATE_DIR"] = "/data/moss_state"+check("MOSS_STATE_DIR=/data/moss_state -> /data/moss_state/recordings/moss.db",+ rp.moss_recording_path() == Path("/data/moss_state/recordings/moss.db"))+os.environ["MOSS_STATE_DIR"] = "~/elsewhere"+check("MOSS_STATE_DIR=~/elsewhere -> expanded", rp.moss_state_dir() == Path.home() / "elsewhere")+os.environ["MOSS_STATE_DIR"] = "relative/state"+check("a relative MOSS_STATE_DIR is made absolute (never site-packages)",+ rp.moss_recording_path().is_absolute(), str(rp.moss_recording_path()))+os.environ.pop("MOSS_STATE_DIR")++# ── D. telemetry payload ─────────────────────────────────────────────────+print("D. telemetry_payload(adapter_like, now)")+from moss_dimos.moss_adapter import (MossBaseAdapter, MossGeometry, telemetry_payload, # noqa: E402+ teleop_envelope, teleop_speeds)+from moss_dimos.moss_mock import FakeMossFirmware # noqa: E402+++class FakeAdapter:+ def __init__(self, telemetry, age, mock=False):+ self.telemetry, self.telemetry_age_s, self.mock_bus = telemetry, age, mock+++v3_line = {"ms": 1234, "fw": "qtpy-mdd3a-v3", "mode": 2, "reason": "drive",+ "target_pct": [40, -40], "output_pct": [35, -35], "ramp_ms": [5, 5],+ "ticks": [160585, 158263], "invalid_edges": [0, 0], "ina_ok": True,+ "bus_mV": 11208, "shunt_uV": 0, "current_mA": None}+doc = telemetry_payload(FakeAdapter(v3_line, 0.3), 1760000000.0)+expected = {"ts": 1760000000.0, "fw": "qtpy-mdd3a-v3", "driver": None, "mode": 2, "reason": "drive",+ "bus_mV": 11208, "ina_ok": True, "ticks": [160585, 158263], "target_pct": [40, -40],+ "output_pct": [35, -35], "ramp_ms": [5, 5], "firmware_age_s": 0.3, "mock": False}+check("v3 line, age 0.3 s, now 1760000000 -> the known document", doc == expected,+ "" if doc == expected else f"{doc}")+check("exactly the cockpit's keys, in order", list(doc) == ["ts", "fw", "driver", "mode", "reason", "bus_mV", "ina_ok",+ "ticks", "target_pct", "output_pct", "ramp_ms",+ "firmware_age_s", "mock"])+mdd10a = dict(v3_line, driver="mdd10a")+check("an MDD10A build's driver key is carried", telemetry_payload(FakeAdapter(mdd10a, 0.0), 0.0)["driver"] == "mdd10a")+check("no telemetry yet -> None (nothing published)", telemetry_payload(FakeAdapter(None, None), 0.0) is None)+check("an adapter without the attribute -> None", telemetry_payload(object(), 0.0) is None)+check("no adapter at all -> None", telemetry_payload(None, 0.0) is None)+check("age unknown -> firmware_age_s None", telemetry_payload(FakeAdapter(v3_line, None), 0.0)["firmware_age_s"] is None)++fw = FakeMossFirmware()+factory = lambda: fw # noqa: E731+factory.is_mock = True+a = MossBaseAdapter(port_factory=factory, geometry=MossGeometry())+check("real adapter on the fake firmware connects", a.connect())+time.sleep(0.12)+t0 = time.time()+doc = telemetry_payload(a, t0)+check("-> fw qtpy-mdd3a-v3, 11600 mV (11.6 V), ina_ok, mode 0, mock True",+ doc is not None and doc["fw"] == "qtpy-mdd3a-v3" and doc["bus_mV"] == 11600 and doc["ina_ok"] is True+ and doc["mode"] == 0 and doc["mock"] is True and doc["driver"] is None, f"{doc}")+check("firmware_age_s ~0.12 s after the connect probe", doc is not None and 0.05 <= doc["firmware_age_s"] < 0.5,+ f"{doc and doc['firmware_age_s']}")+check("JSON round trip keeps every value", json.loads(json.dumps(doc)) == doc)+a.disconnect()++# ── E. camera mount ──────────────────────────────────────────────────────+print("E. camera_mount_xyzq(profile)")+m = mb.camera_mount_xyzq(rp.MOSS)+check("MOSS: (0.156, 0, 0.075) m, identity rotation (pitch 0)", m == (0.156, 0.0, 0.075, 0.0, 0.0, 0.0, 1.0), f"{m}")+from dataclasses import replace # noqa: E402+m = mb.camera_mount_xyzq(replace(rp.MOSS, camera_pitch_deg=1.1))+check("1.1 deg nose down -> qy 0.00959916, qw 0.99995393 (sin / cos of 0.55 deg)",+ close(m[4], 0.00959916, 1e-8) and close(m[6], 0.99995393, 1e-8), f"qy {m[4]:.8f} qw {m[6]:.8f}")+check("unit quaternion", close(math.hypot(m[4], m[6]), 1.0, 1e-12))++# ── H. wrist camera index ────────────────────────────────────────────────+print("H. wrist_camera_index(env, by_id_dir)")+v4l = tempfile.mkdtemp(prefix="v4l_")+check("empty by-id dir, no MOSS_WRIST_DEV -> None (no wrist camera)", mb.wrist_camera_index({}, v4l) is None)+os.symlink("/dev/video6", os.path.join(v4l, "usb-Sonix_Technology_Co.__Ltd._KD-USB_Cameras_KD-USB_Cameras-video-index0"))+os.symlink("/dev/video7", os.path.join(v4l, "usb-Sonix_Technology_Co.__Ltd._KD-USB_Cameras_KD-USB_Cameras-video-index1"))+check("KD-USB by-id video-index0 -> /dev/video6 -> 6 (index1 ignored)", mb.wrist_camera_index({}, v4l) == 6)+check("MOSS_WRIST_DEV=3 -> 3", mb.wrist_camera_index({"MOSS_WRIST_DEV": "3"}, v4l) == 3)+check("MOSS_WRIST_DEV=/dev/video9 -> 9", mb.wrist_camera_index({"MOSS_WRIST_DEV": "/dev/video9"}, v4l) == 9)+check("MOSS_WRIST_DEV=/dev/ttyUSB0 (not a video device) -> None", mb.wrist_camera_index({"MOSS_WRIST_DEV": "/dev/ttyUSB0"}, v4l) is None)+mb.V4L_BY_ID = v4l # F and G build the blueprints against this fake by-id+print()++# ── F. entry points ──────────────────────────────────────────────────────+print("F. pyproject entry points")+try:+ import tomllib+ meta = tomllib.loads((ROOT / "pyproject.toml").read_text())+except Exception as exc: # noqa: BLE001+ meta = {}+ check("pyproject.toml parses", False, f"{type(exc).__name__}: {exc}")+project = meta.get("project", {})+eps = project.get("entry-points", {}).get("dimos.blueprints", {})+TARGETS = {"moss-base": "moss_dimos.blueprints:moss_base_blueprint",+ "moss-gamepad": "moss_dimos.blueprints:moss_gamepad_blueprint",+ "moss-nav": "moss_dimos.blueprints:moss_nav_blueprint",+ "moss-explore": "moss_dimos.blueprints:moss_explore_blueprint"}+check("distribution name moss-dimos (the dimOS namespace)", project.get("name") == "moss-dimos", f"{project.get('name')}")+check("exactly the four MOSS entry points, on moss_dimos.blueprints", eps == TARGETS, f"{eps}")+check("every target has a lazy builder", {t.split(":")[1] for t in eps.values()} == set(mb._BUILDERS),+ f"{sorted(mb._BUILDERS)}")+check("names match dimOS's ^[a-z0-9]+(-[a-z0-9]+)*$", bool(eps) and all(re.match(r"^[a-z0-9]+(-[a-z0-9]+)*$", k) for k in eps))+check("packages moss_dimos and moss_pi", meta.get("tool", {}).get("setuptools", {}).get("packages") == ["moss_dimos", "moss_pi"])+check("pytest collects tests/ only (the benches are scripts)",+ meta.get("tool", {}).get("pytest", {}).get("ini_options", {}).get("testpaths") == ["tests"])++# dimOS reads entry points from installed metadata. A dist-info built from+# pyproject.toml on sys.path is that metadata, without installing anything.+dist_root = tempfile.mkdtemp(prefix="moss_dist_")+dist = Path(dist_root) / f"moss_dimos-{project.get('version', '0')}.dist-info"+dist.mkdir()+(dist / "METADATA").write_text(f"Metadata-Version: 2.1\nName: {project.get('name')}\nVersion: {project.get('version')}\n")+(dist / "entry_points.txt").write_text("[dimos.blueprints]\n" + "".join(f"{k} = {v}\n" for k, v in eps.items()))+sys.path.append(dist_root)+importlib.invalidate_caches()+from dimos.core.coordination.blueprints import Blueprint # noqa: E402+from dimos.robot.external_blueprints import list_external_blueprint_names # noqa: E402+from dimos.robot.get_all_blueprints import get_by_name # noqa: E402++listed = [n for n in list_external_blueprint_names() if n.startswith("moss-dimos.")]+check("dimOS lists moss-dimos.moss-base / moss-explore / moss-gamepad / moss-nav",+ listed == ["moss-dimos.moss-base", "moss-dimos.moss-explore", "moss-dimos.moss-gamepad", "moss-dimos.moss-nav"],+ f"{listed}")+resolved = {}+for local in TARGETS:+ name = f"moss-dimos.{local}"+ try:+ resolved[local] = get_by_name(name)+ n = len(resolved[local].blueprints)+ check(f"get_by_name({name!r}) -> a Blueprint ({n} module(s))", isinstance(resolved[local], Blueprint))+ except Exception as exc: # noqa: BLE001+ check(f"get_by_name({name!r})", False, f"{type(exc).__name__}: {exc}")+check("resolution caches the built blueprint on the module (built once per process)",+ resolved.get("moss-nav") is not None and resolved.get("moss-nav") is mb.moss_nav_blueprint)+sys.path.remove(dist_root)+shutil.rmtree(dist_root, ignore_errors=True)+importlib.invalidate_caches()++# ── G. blueprints (dimOS) ────────────────────────────────────────────────+print("G. moss-base / moss-gamepad / moss-nav / moss-explore blueprints")+from dimos.core.coordination.blueprints import TransportSpec # noqa: E402+from dimos.core.transport import JpegLcmTransport # noqa: E402+from dimos.msgs.sensor_msgs.Image import Image # noqa: E402+from moss_dimos.blueprints import MossControlCoordinator # noqa: E402+from moss_dimos.gamepad import GamepadTeleop # noqa: E402+from moss_dimos.memory import MossMemoryConfig, MossRecorder # noqa: E402+++def names(bp):+ return {b.module.__name__ for b in bp.active_blueprints}+++def atom(bp, cls_name):+ return next(b for b in bp.active_blueprints if b.module.__name__ == cls_name)+++base = mb.build_moss_base()+check("moss-base: the coordinator alone, served as 'ControlCoordinator'",+ [x.name for x in base.blueprints] == ["ControlCoordinator"], f"{[x.name for x in base.blueprints]}")+coord_b = base._instance_key(MossControlCoordinator)+check("moss-base: twist_command -> cmd_vel", base.remapping_map.get((coord_b, "twist_command")) == "cmd_vel")+hw = atom(base, "MossControlCoordinator").kwargs["hardware"][0]+check("moss-base: adapter 'moss', port left to the adapter (MOSS_PORT, by-id, sentinel)",+ hw.adapter_type == "moss" and hw.address is None, f"{hw.adapter_type} {hw.address}")+task = atom(base, "MossControlCoordinator").kwargs["tasks"][0]+check("moss-base: task vel_base, velocity, priority 10, on the base joints",+ (task.name, task.type, task.priority) == ("vel_base", "velocity", 10) and list(task.joint_names) == list(mb._base_joints))++pad_bp = mb.build_moss_gamepad()+check("moss-gamepad: coordinator + gamepad", names(pad_bp) == {"MossControlCoordinator", "GamepadTeleop"}, f"{sorted(names(pad_bp))}")+coord_g = pad_bp._instance_key(MossControlCoordinator)+pad_g = pad_bp._instance_key(GamepadTeleop)+check("moss-gamepad: coordinator instance ControlCoordinator", coord_g == "ControlCoordinator")+check("moss-gamepad: twist_command -> cmd_vel and tele_cmd_vel -> cmd_vel (direct drive)",+ pad_bp.remapping_map.get((coord_g, "twist_command")) == "cmd_vel"+ and pad_bp.remapping_map.get((pad_g, "tele_cmd_vel")) == "cmd_vel")+gk = atom(pad_bp, "GamepadTeleop").kwargs+g = MossGeometry.from_env()+lin, ang = teleop_speeds(g)+check("moss-gamepad: stick scale = the base's: 0.45 m/s, 6.7669 rad/s (default geometry)",+ close(gk.get("linear_speed"), 0.45) and close(gk.get("angular_speed"), 2.0 * 2.0 * 0.45 / 0.266)+ and (gk.get("linear_speed"), gk.get("angular_speed")) == (lin, ang),+ f"{gk.get('linear_speed')} m/s, {gk.get('angular_speed')} rad/s")+env_d = gk.get("envelope")+check("moss-gamepad: the envelope rides as a plain dict: linear 0.45, lever 0.133, rim 1.35",+ isinstance(env_d, dict) and env_d == teleop_envelope(g)+ and close(env_d["lever"], 0.133) and close(env_d["rim"], 1.35), f"{env_d}")+os.environ["MOSS_MPS_AT_LIMIT"] = "0.30"+slow = atom(mb.build_moss_gamepad(), "GamepadTeleop").kwargs+os.environ.pop("MOSS_MPS_AT_LIMIT")+check("MOSS_MPS_AT_LIMIT=0.30 read when the blueprint is built -> 0.30 m/s, rim 0.90",+ close(slow.get("linear_speed"), 0.30) and close(slow["envelope"]["rim"], 0.30 + 0.30 * 4.0 / 0.266 * 0.133),+ f"{slow.get('linear_speed')} {slow['envelope']}")++nav = mb.build_moss_nav()+n = names(nav)+want = {"MossControlCoordinator", "GamepadTeleop", "RPLidarC1", "LidarOdometry",+ "MossCostMap", "RealSenseCamera", "MossRecorder"}+check("moss-nav modules", want <= n, f"{sorted(n)}")+check("moss-nav has no planner, no explorer, no MovementManager, no viewer",+ not ({"RecoveringPlanner", "Explorer2", "MovementManager", "WebsocketVisModule"} & n))+coord = nav._instance_key(MossControlCoordinator)+pad = nav._instance_key(GamepadTeleop)+check("coordinator instance name ControlCoordinator", coord == "ControlCoordinator")+check("remap twist_command -> cmd_vel", nav.remapping_map.get((coord, "twist_command")) == "cmd_vel")+check("remap pad tele_cmd_vel -> cmd_vel (direct drive)", nav.remapping_map.get((pad, "tele_cmd_vel")) == "cmd_vel")+spec = nav.transport_map.get(("color_image", Image))+check("color_image crosses the bus as JPEG on /color_image",+ isinstance(spec, TransportSpec) and spec.cls is JpegLcmTransport and spec.args[0] == "/color_image")+lid = atom(nav, "RPLidarC1").kwargs+check("lidar on the CP2102N by-id at 460800, frame lidar_link",+ lid.get("port") == os.path.join(byid, C1) and lid.get("baudrate") == 460800 and lid.get("frame_id") == "lidar_link")+hw = atom(nav, "MossControlCoordinator").kwargs["hardware"][0]+check("base: adapter 'moss' on the QT Py by-id", hw.adapter_type == "moss" and hw.address == os.path.join(byid, QTPY))+check("pad: the same MOSS envelope as moss-gamepad", atom(nav, "GamepadTeleop").kwargs.get("envelope") == teleop_envelope(g))+check("odometry without the gyro prior", atom(nav, "LidarOdometry").kwargs.get("use_gyro_prior") is False)+cam = atom(nav, "RealSenseCamera").kwargs+check("RealSense 640x480 @ 5 fps, colour only",+ (cam.get("width"), cam.get("height"), cam.get("fps"), cam.get("enable_depth"),+ cam.get("enable_pointcloud"), cam.get("enable_imu")) == (640, 480, 5, False, False, False))+t = cam["base_transform"]+check("camera mount from the profile: (0.156, 0, 0.075) m",+ close(t.translation.x, 0.156) and close(t.translation.z, 0.075))+check("recorder at the MOSS state dir", atom(nav, "MossRecorder").kwargs.get("db_path") == str(home_db))+check("wrist camera module WristCamera in moss-nav (fake by-id -> /dev/video6)", "WristCamera" in n, f"{sorted(n)}")+wc = atom(nav, "WristCamera").kwargs+check("wrist: Webcam hardware factory on index 6, 640x480 @ 5 fps, frame prefix 'wrist', no TF, 5 Hz barrier",+ getattr(wc.get("hardware"), "keywords", {}).get("camera_index") == 6+ and getattr(wc.get("hardware"), "keywords", {}).get("width") == 640+ and getattr(wc.get("hardware"), "keywords", {}).get("fps") == 5.0+ and getattr(wc.get("hardware"), "keywords", {}).get("frame_id_prefix") == "wrist"+ and wc.get("transform") is None and wc.get("frequency") == 5.0, f"{wc}")+wk = mb.wrist_instance(nav)+check("wrist instance name found (dimOS lowercases it)", wk == "wristcamera", f"{wk}")+check("wrist outputs remapped: color_image -> wrist_image, camera_info -> wrist_camera_info, tf -> wrist_tf",+ nav.remapping_map.get((wk, "color_image")) == "wrist_image"+ and nav.remapping_map.get((wk, "camera_info")) == "wrist_camera_info"+ and nav.remapping_map.get((wk, "tf")) == "wrist_tf", f"{dict(nav.remapping_map)}")+wspec = nav.transport_map.get(("wrist_image", Image))+check("wrist_image crosses the bus as JPEG on /wrist_image",+ isinstance(wspec, TransportSpec) and wspec.cls is JpegLcmTransport and wspec.args[0] == "/wrist_image")+check("the RealSense keeps its own names (no remap on RealSenseCamera)",+ not any(k[0] == "RealSenseCamera" for k in nav.remapping_map))++exp = mb.build_moss_explore()+e = names(exp)+check("moss-explore adds RecoveringPlanner, MovementManager, Explorer2",+ {"RecoveringPlanner", "MovementManager", "Explorer2"} <= e and want <= e, f"{sorted(e)}")+pad_e = exp._instance_key(GamepadTeleop)+check("moss-explore: NO pad -> cmd_vel remap (MovementManager arbitrates)",+ (pad_e, "tele_cmd_vel") not in exp.remapping_map)+check("moss-explore: twist_command -> cmd_vel", exp.remapping_map.get(("ControlCoordinator", "twist_command")) == "cmd_vel")+ex = atom(exp, "Explorer2").kwargs+check("explorer clearances = derived(MOSS): 0.22 / 0.25 m",+ close(ex.get("lethal_clearance_m"), 0.22) and close(ex.get("pivot_clearance_m"), 0.25), f"{ex}")+check("explorer frontier floor 0.3 m, goal timeout 45 s",+ ex.get("min_frontier_perimeter") == 0.3 and ex.get("goal_timeout") == 45.0)+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)++os.environ["MOSS_CAMERA"] = "0"+os.environ["MOSS_RECORD"] = "0"+bare = mb.build_moss_nav()+check("MOSS_CAMERA=0 MOSS_RECORD=0 -> no RealSense, no recorder, no JPEG transport",+ not ({"RealSenseCamera", "MossRecorder"} & names(bare)) and ("color_image", Image) not in bare.transport_map)+os.environ["MOSS_WRIST"] = "0"+bare2 = mb.build_moss_nav()+check("MOSS_WRIST=0 -> no WristCamera, no wrist remap, no wrist transport",+ "WristCamera" not in names(bare2) and mb.wrist_instance(bare2) is None+ and ("wrist_image", Image) not in bare2.transport_map)+os.environ.pop("MOSS_WRIST")+os.environ.update(MOSS_CAMERA="OFF", MOSS_RECORD=" no ", MOSS_WRIST="False")+spelled = mb.build_moss_nav()+check("MOSS_CAMERA=OFF, MOSS_RECORD=' no ', MOSS_WRIST=False -> the same bare build "+ "(robot_profile.env_on, the one switch parser)",+ not ({"RealSenseCamera", "MossRecorder", "WristCamera"} & names(spelled))+ and ("color_image", Image) not in spelled.transport_map and ("wrist_image", Image) not in spelled.transport_map)+check("the blueprint has no switch parser of its own", not hasattr(mb, "_env_on"))+os.environ.update(MOSS_CAMERA="0", MOSS_RECORD="0")+os.environ.pop("MOSS_WRIST")+mb.V4L_BY_ID = tempfile.mkdtemp(prefix="v4l_empty_")+nowrist = mb.build_moss_nav()+check("wrist camera absent from by-id -> module skipped, blueprint still builds",+ "WristCamera" not in names(nowrist) and ("wrist_image", Image) not in nowrist.transport_map)+mb.V4L_BY_ID = v4l+os.environ["MOSS_RERUN"] = "1"+check("MOSS_RERUN=1 -> the stock viewers ride along", "WebsocketVisModule" in names(mb.build_moss_nav()))+for k in ("MOSS_CAMERA", "MOSS_RECORD", "MOSS_RERUN"):+ os.environ.pop(k, None)+empty = tempfile.mkdtemp(prefix="byid_empty_")+rp.BYID_ROOT = empty+missing = mb.build_moss_nav()+shutil.rmtree(empty, ignore_errors=True)+check("devices absent -> the sentinels reach the modules (fail loudly, no fallback)",+ atom(missing, "RPLidarC1").kwargs["port"] == rp.MOSS_LIDAR_MISSING+ and atom(missing, "MossControlCoordinator").kwargs["hardware"][0].address == rp.MOSS_QTPY_MISSING)++cfg = MossMemoryConfig()+check("MossMemoryConfig default db_path ~/.local/state/moss/recordings/moss.db", Path(cfg.db_path) == home_db, str(cfg.db_path))+check("poseless: global_costmap, cmd_vel, base_telemetry, episode_marker; record_tf False",+ cfg.poseless_streams == ["global_costmap", "cmd_vel", "base_telemetry", "episode_marker"] and cfg.record_tf is False)+os.environ["MOSS_STATE_DIR"] = "/data/moss_state"+check("MOSS_STATE_DIR read when the config is built", Path(MossMemoryConfig().db_path) == Path("/data/moss_state/recordings/moss.db"))+os.environ.pop("MOSS_STATE_DIR")+ins = set(MossRecorder.__annotations__) | set(MossRecorder.__mro__[1].__annotations__)+check("MossRecorder records odom, global_costmap, lidar, color_image, wrist_image, cmd_vel, base_telemetry, episode_marker",+ {"odom", "global_costmap", "lidar", "color_image", "wrist_image", "cmd_vel", "base_telemetry", "episode_marker"} <= ins)+check("MossControlCoordinator has the base_telemetry Out, and no other stream of its own",+ set(MossControlCoordinator.__annotations__) == {"base_telemetry"}, f"{sorted(MossControlCoordinator.__annotations__)}")+check("its start/stop overrides keep @rpc (else dimOS pickles the module to call them)",+ {"start", "stop"} <= set(MossControlCoordinator.rpcs))+check("it runs in its own worker (dedicated_worker)", MossControlCoordinator.dedicated_worker is True)++# ── I. the coordinator and the wrist camera in a fresh worker ───────────+print("I. what a dimOS worker does: unpickle in a clean interpreter")+# dimOS deploys modules into forkserver workers: a fresh interpreter that+# imports dimos but not this package. It only imports what it needs to+# UNPICKLE the module class it was told to deploy. So the pickled coordinator+# has to drag moss_dimos.blueprints in with it - otherwise _setup_hardware+# dies with KeyError: "Unknown twist base adapter: moss".+payload = pickle.dumps(MossControlCoordinator)+check("the coordinator pickles by reference to moss_dimos.blueprints",+ b"moss_dimos.blueprints" in payload and b"MossControlCoordinator" in payload)+worker = run_child("""+ import json, pickle, sys+ from dimos.hardware.drive_trains.registry import twist_base_adapter_registry as r+ from dimos.protocol.pubsub.impl.lcmpubsub import LCM+ before = "moss" in r.available()+ cls = pickle.loads(sys.stdin.buffer.read())+ after = "moss" in r.available()+ adapter = r.create("moss", dof=3, address="/dev/serial/by-id/MISSING-QT-PY", hardware_id="base")+ print(json.dumps({+ "before": before, "after": after, "cls": cls.__name__,+ "path": r._factory_paths.get("moss"),+ "adapter": f"{type(adapter).__module__}.{type(adapter).__name__}",+ "port": adapter._port, "clamp": hasattr(LCM.subscribe, "__wrapped__"),+ "moss_modules": sorted(m for m in sys.modules if m.startswith("moss_dimos")),+ }))+""", stdin=payload)+try:+ w = json.loads(last_line(worker))+except ValueError:+ w = {}+ print(worker.stderr.decode(errors="replace")[-600:])+check("the worker unpickles MossControlCoordinator", w.get("cls") == "MossControlCoordinator", f"rc {worker.returncode}")+check("a clean interpreter does not know 'moss' before unpickling", w.get("before") is False)+check("'moss' registered after unpickling, path " + FACTORY, w.get("after") is True and w.get("path") == FACTORY)+check("the registry builds the adapter from that string (as _setup_hardware does): MossBaseAdapter",+ w.get("adapter") == "moss_dimos.moss_adapter.MossBaseAdapter" and w.get("port") == "/dev/serial/by-id/MISSING-QT-PY",+ f"{w.get('adapter')} on {w.get('port')}")+check("the LCM queue clamp is on in the worker", w.get("clamp") is True)+check("the worker loads only the package, robot_profile, the adapter, the clamp and the wrapper",+ w.get("moss_modules") == ["moss_dimos", "moss_dimos.blueprints", "moss_dimos.latest_only", "moss_dimos.lcm_latest",+ "moss_dimos.moss_adapter", "moss_dimos.robot_profile"],+ f"{w.get('moss_modules')}")++WristCamera = mb.WristCamera+wpayload = pickle.dumps(WristCamera)+check("WristCamera: module moss_dimos.blueprints, qualname WristCamera",+ (WristCamera.__module__, WristCamera.__qualname__) == ("moss_dimos.blueprints", "WristCamera"))+check("pickle.dumps(WristCamera) round-trips to the same class (served by the module __getattr__)",+ pickle.loads(wpayload) is WristCamera and mb.WristCamera is WristCamera)+wchild = run_child("""+ import json, pickle, sys+ from dimos.hardware.sensors.camera.module import CameraModule+ cls = pickle.loads(sys.stdin.buffer.read())+ import moss_dimos.blueprints as b+ print(json.dumps({"name": cls.__name__, "module": cls.__module__, "camera": issubclass(cls, CameraModule),+ "same": b.WristCamera is cls}))+""", stdin=wpayload)+try:+ wc_got = json.loads(last_line(wchild))+except ValueError:+ wc_got = {}+ print(wchild.stderr.decode(errors="replace")[-600:])+check("... and unpickles in a clean interpreter to a CameraModule named WristCamera",+ wc_got == {"name": "WristCamera", "module": "moss_dimos.blueprints", "camera": True, "same": True}, f"{wc_got}")++from dimos.hardware.drive_trains.registry import twist_base_adapter_registry # noqa: E402+try:+ twist_base_adapter_registry.register_path("moss", mb.MOSS_ADAPTER_PATH)+ check("registering the same path again is a no-op (re-import is safe)", True)+except ValueError as exc:+ check("registering the same path again is a no-op (re-import is safe)", False, str(exc))+clash = run_child("""+ from dimos.hardware.drive_trains.registry import twist_base_adapter_registry as r+ r.register_path("moss", "other_pkg.adapter:OtherAdapter")+ import moss_dimos.blueprints+ print("REGISTRY", r._factory_paths.get("moss"))+""")+out = clash.stdout.decode(errors="replace") + clash.stderr.decode(errors="replace")+warn = [ln for ln in out.splitlines() if "already registered" in ln]+check("a conflicting 'moss' registration: the import still succeeds, the first path stays",+ clash.returncode == 0 and last_line(clash) == "REGISTRY other_pkg.adapter:OtherAdapter", out.strip()[-300:])+check("... and a warning names both paths (not swallowed)",+ bool(warn) and "other_pkg.adapter:OtherAdapter" in warn[0] and FACTORY in warn[0], f"{warn[:1]}")++rp.BYID_ROOT = saved_root+shutil.rmtree(byid, ignore_errors=True)+shutil.rmtree(v4l, ignore_errors=True)+finish()diff --git a/benches/bench_moss_cockpit_cold.py b/benches/bench_moss_cockpit_cold.pynew file mode 100644index 0000000..6845034--- /dev/null+++ b/benches/bench_moss_cockpit_cold.py@@ -0,0 +1,1144 @@+"""Cold bench for tools/moss_cockpit.py: a real server on a free port, known in -> known out.++Every check feeds a value we know and reads back a value in physical units+(metres, radians, volts, %, pixel colours, bytes on a named channel). Groups:++ A. pure helpers - battery gauge (3S: 10.2 V -> 0 %, 11.4 V -> 50 %, 12.6 V -> 100 %),+ quaternion -> yaw, grid -> PNG pixel colours, the map_meta+ pixel <-> metre formulas, even downsampling+ B. demo server - /, /static, /state.json schema, /map.png (demo size, wall =+ WHITE, floor = free colour), /map_meta roundtrip, /scan.json+ (<= 1500 points, every point on a wall), /camera.mjpg framing+ and its 5 fps cap, POST /cmd/goal|stop|episode|stack, path+ traversal refused, bad bodies refused+ C. decoding - messages built with dimos_lcm and fed to a live cockpit's bus+ cache: OccupancyGrid -> exact PNG pixels, PointCloud2 -> scan+ points (and the /pointcloud fallback), PoseStamped yaw 90 deg,+ telemetry String -> battery %, Path, explore/goal state, a JPEG+ image passed through byte for byte+ D. commands - POST /cmd/* in live mode publish the known bytes on the known+ channels (fake LCM handle); explore_ctl's generic publisher and+ its start/stop; a run that is not on LCM is refused (503), the+ bus read in dimOS's order (DIMOS_TRANSPORT, then TRANSPORT) from+ the running stack's own environment, else from the cockpit's+ E. stack + files - run registry (dead pid skipped), main.jsonl tail, preflight.json,+ the launcher run non-blocking on a stub script, refusal when a+ stack runs+ G. robustness - launcher env without EXPLORE/MOCK/MOSS_MOCK_BUS, Stop cancels a+ launch in its gates (SIGTERM, SIGKILL after the grace), an orphan+ episode closed when the stack stops, a new run_id empties the bus+ cache, a camera frame after a run change, a viewer that stops+ reading frees its camera slot+ H. the real page - tools/moss_cockpit/ served as shipped: index.html, every asset it+ and tokens.css reference, the run caption's namespace strip+ 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+dimOS; skipped without it). Nothing here needs dimOS, lcm or hardware.++Run: PYTHONDONTWRITEBYTECODE=1 PYTHONPATH=. .venv/bin/python benches/bench_moss_cockpit_cold.py+"""++import atexit+import http.client+import json+import math+import os+import re+import struct+import subprocess+import sys+import tempfile+import threading+import time+import types+import datetime as _dt+import signal+import socket+import urllib.error+import urllib.request+import zlib+from pathlib import Path++ROOT = Path(__file__).resolve().parent.parent+sys.path.insert(0, str(ROOT))+sys.path.insert(0, str(ROOT / "tools"))++import numpy as np # noqa: E402++import moss_cockpit as mc # noqa: E402++try:+ import dimos_lcm # noqa: F401+ from dimos_lcm.geometry_msgs import Point, Pose, PoseStamped, Quaternion, Twist, Vector3+ from dimos_lcm.nav_msgs import MapMetaData, OccupancyGrid, Path as LcmPath+ from dimos_lcm.sensor_msgs import Image, PointCloud2, PointField+ from dimos_lcm.std_msgs import Bool, Header, String, Time+ HAVE_LCM_MSGS = True++ # Fresh nested objects every time: the generated constructors share their+ # default instances, so a default-built message mutated here would leak into+ # every later one (the cockpit's own zero twist included).+ def hdr(frame="", sec=0, nsec=0):+ return Header(seq=0, stamp=Time(sec=sec, nsec=nsec), frame_id=frame)++ def pose_of(x, y, qz=0.0, qw=1.0):+ return Pose(position=Point(x=x, y=y, z=0.0), orientation=Quaternion(x=0.0, y=0.0, z=qz, w=qw))+except ImportError:+ HAVE_LCM_MSGS = False++OK = 0+KO = 0+_TMPDIR = tempfile.TemporaryDirectory(prefix="moss_cockpit_bench_") # state dir, run registry, page, logs+TMP = _TMPDIR.name+os.environ["MOSS_STATE_DIR"] = os.path.join(TMP, "state")+os.environ["XDG_STATE_HOME"] = os.path.join(TMP, "xdg") # an empty dimOS run registry+os.environ["DIMOS_TRANSPORT"] = "lcm" # the cockpit's own bus, known under both names dimOS reads+os.environ["TRANSPORT"] = "lcm"++WHITE = (0xEA, 0xF0, 0xEF)+FREE = (0x2E, 0x45, 0x56)+UNKNOWN = (0x3D, 0x57, 0x6A)+COST_MAX = (0x8F, 0xA6, 0xB5)+++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+++def cost_colour(v):+ """The free -> COST_MAX ramp for cost v in 1..99, computed independently of the LUT."""+ return tuple(int(round(f + (t - f) * v / 99.0)) for f, t in zip(FREE, COST_MAX))+++def png_decode(data: bytes) -> np.ndarray:+ """(h, w, 3) uint8 from an 8-bit RGB PNG whose rows all use filter 0 (what we write)."""+ assert data[:8] == b"\x89PNG\r\n\x1a\n", "PNG signature"+ pos, idat = 8, b""+ w = h = None+ while pos < len(data):+ n = struct.unpack(">I", data[pos:pos + 4])[0]+ tag, body = data[pos + 4:pos + 8], data[pos + 8:pos + 8 + n]+ crc = struct.unpack(">I", data[pos + 8 + n:pos + 12 + n])[0]+ assert crc == zlib.crc32(tag + body) & 0xFFFFFFFF, f"CRC of {tag!r}"+ if tag == b"IHDR":+ w, h, depth, ctype = struct.unpack(">IIBB", body[:10])+ assert (depth, ctype) == (8, 2), "8-bit RGB"+ elif tag == b"IDAT":+ idat += body+ pos += 12 + n+ rows = np.frombuffer(zlib.decompress(idat), np.uint8).reshape(h, w * 3 + 1)+ assert (rows[:, 0] == 0).all(), "filter byte 0 on every row"+ return rows[:, 1:].reshape(h, w, 3)+++class Client:+ def __init__(self, port):+ self.port = port+ self.base = f"http://127.0.0.1:{port}"++ def get(self, path):+ try:+ with urllib.request.urlopen(self.base + path, timeout=10) as r:+ return r.status, r.headers, r.read()+ except urllib.error.HTTPError as e:+ return e.code, e.headers, e.read()++ def json(self, path):+ code, _, body = self.get(path)+ return code, json.loads(body)++ def post(self, path, payload, raw=None, header=True):+ data = raw if raw is not None else json.dumps(payload).encode()+ hdrs = {"Content-Type": "application/json"}+ if header:+ hdrs["X-Requested-With"] = "moss-cockpit" # what the page's post() sends+ req = urllib.request.Request(self.base + path, data=data, method="POST", headers=hdrs)+ try:+ with urllib.request.urlopen(req, timeout=10) as r:+ return r.status, json.loads(r.read())+ except urllib.error.HTTPError as e:+ return e.code, json.loads(e.read())++ def raw_get(self, path):+ """No client-side path normalisation (urllib keeps '..' too, this is explicit)."""+ conn = http.client.HTTPConnection("127.0.0.1", self.port, timeout=10)+ conn.putrequest("GET", path, skip_accept_encoding=True)+ conn.endheaders()+ r = conn.getresponse()+ body = r.read()+ conn.close()+ return r.status, body++ def mjpeg_frames(self, n, timeout=8.0):+ """Read n multipart parts; returns (content_type, [(t_arrival, jpeg)])."""+ conn = http.client.HTTPConnection("127.0.0.1", self.port, timeout=timeout)+ conn.request("GET", "/camera.mjpg")+ r = conn.getresponse()+ ctype = r.getheader("Content-Type")+ if r.status != 200:+ body = r.read()+ conn.close()+ return r.status, ctype, body+ frames = []+ fp = r.fp+ while len(frames) < n:+ line = fp.readline()+ if not line:+ break+ if line.strip() != b"--frame":+ continue+ headers = {}+ while True:+ h = fp.readline().strip()+ if not h:+ break+ k, v = h.split(b":", 1)+ headers[k.strip().lower()] = v.strip()+ jpeg = fp.read(int(headers[b"content-length"]))+ frames.append((time.monotonic(), jpeg))+ conn.close()+ return 200, ctype, frames+++def serve(**kw):+ server = mc.make_server("127.0.0.1", 0, listen=False, **kw)+ threading.Thread(target=server.serve_forever, daemon=True).start()+ return server, Client(server.server_address[1])+++# ---------------------------------------------------------------------------+print("A. pure helpers")+check("11.4 V -> 50 %", mc.battery_pct(11.4) == 50, str(mc.battery_pct(11.4)))+check("10.2 V -> 0 %, 12.6 V -> 100 %", (mc.battery_pct(10.2), mc.battery_pct(12.6)) == (0, 100))+check("clamped: 9.0 V -> 0 %, 13.1 V -> 100 %", (mc.battery_pct(9.0), mc.battery_pct(13.1)) == (0, 100))+s45 = math.sin(math.pi / 4)+check("quaternion (0,0,sin45,cos45) -> yaw 1.5708 rad",+ abs(mc.yaw_of(0, 0, s45, s45) - math.pi / 2) < 1e-9, f"{mc.yaw_of(0, 0, s45, s45):.6f}")+check("quaternion (0,0,1,0) -> yaw +-pi", abs(abs(mc.yaw_of(0, 0, 1, 0)) - math.pi) < 1e-9)+cells = np.array([[-1, 0, 100], [50, 0, -1]], dtype=np.int8) # row 0 = min y+png, meta = mc.render_grid(cells, 0.5, 1.0, 2.0, stamp=7.5, frame="world")+img = png_decode(png)+check("3x2 grid -> 3x2 PNG", img.shape == (2, 3, 3), str(img.shape))+expect = [[cost_colour(50), FREE, UNKNOWN], [UNKNOWN, FREE, WHITE]] # PNG row 0 = max y+got = [[tuple(int(c) for c in img[r, k]) for k in range(3)] for r in range(2)]+check("pixels flipped (row 0 = max y): cost 50, free, unknown / unknown, free, WHITE",+ got == expect, str(got))+check("cost 99 -> #8FA6B5, cost 1 ~ free",+ tuple(mc.MAP_LUT[99]) == COST_MAX and tuple(mc.MAP_LUT[1]) == cost_colour(1))+check("meta: res 0.5, origin (1, 2), extent x 1..2.5 y 2..3, stamp, frame",+ (meta["res"], meta["ox"], meta["oy"], meta["x_max"], meta["y_max"], meta["stamp"], meta["frame"])+ == (0.5, 1.0, 2.0, 2.5, 3.0, 7.5, "world"), json.dumps(meta)[:120])+x_of_px = lambda px, m: m["ox"] + (px + 0.5) * m["res"] # noqa: E731+y_of_py = lambda py, m: m["oy"] + (m["height"] - py - 0.5) * m["res"] # noqa: E731+px_of_x = lambda x, m: (x - m["ox"]) / m["res"] # noqa: E731+py_of_y = lambda y, m: m["height"] - (y - m["oy"]) / m["res"] # noqa: E731+check("formulas: lethal cell (col 2, grid row 0) centre = (2.25 m, 2.25 m) -> PNG pixel (2, 1)",+ (x_of_px(2, meta), y_of_py(1, meta)) == (2.25, 2.25)+ and tuple(int(c) for c in img[1, 2]) == WHITE)+pts = np.column_stack((np.arange(4000, dtype=np.float32), np.zeros(4000, np.float32)))+ds = mc.downsample(pts)+check("4000 points -> 1500, first and last kept, evenly spaced",+ len(ds) == 1500 and ds[0, 0] == 0 and ds[-1, 0] == 3999+ and np.ptp(np.diff(ds[:, 0])) <= 1.0, f"{len(ds)} pts")+base = mc.base_view({"fw": "qtpy-mdd3a-v3", "mode": 2, "bus_mV": 11400, "ina_ok": True,+ "ticks": [10, 12], "target_pct": [40, 40], "output_pct": [38, 39]}, 0.25)+check("telemetry 11400 mV, mode 2 -> 11.4 V, 50 %, 'drive', driver None",+ (base["battery_v"], base["battery_pct"], base["mode_name"], base["driver"]) == (11.4, 50, "drive", None),+ json.dumps(base)[:120])+check("ina_ok False -> no battery % (the voltage is not measured)",+ mc.base_view({"bus_mV": 0, "ina_ok": False}, 0.1)["battery_pct"] is None)+check("NaN -> null in JSON (a cancelled dimOS goal is NaN)",+ json.dumps(mc.json_safe({"x": float("nan"), "y": [np.float32(1.5)]})) == '{"x": null, "y": [1.5]}')++# ---------------------------------------------------------------------------+print("B. demo server")+page_dir = os.path.join(TMP, "page")+os.makedirs(os.path.join(page_dir, "fonts"))+Path(page_dir, "index.html").write_text("<!doctype html><title>MOSS cockpit</title>bench page")+Path(page_dir, "tokens.css").write_text(":root{--moss-mint:#6CF8C7}")+Path(page_dir, "fonts", "x.ttf").write_bytes(b"\x00\x01\x00\x00")+demo, cli = serve(demo=True, page_dir=page_dir)+code, headers, body = cli.get("/")+check("GET / -> the page's index.html", code == 200 and b"bench page" in body+ and headers["Content-Type"].startswith("text/html") and headers["Cache-Control"] == "no-store")+code, headers, body = cli.get("/static/tokens.css")+check("GET /static/tokens.css -> text/css", code == 200 and headers["Content-Type"].startswith("text/css")+ and b"6CF8C7" in body)+code, headers, _ = cli.get("/static/fonts/x.ttf")+check("GET /static/fonts/x.ttf -> font/ttf", code == 200 and headers["Content-Type"] == "font/ttf")+for bad in ("/static/../moss_cockpit.py", "/static/%2e%2e/moss_cockpit.py", "/static//etc/passwd",+ "/static/..%2f..%2fREADME.md"):+ code, body = cli.raw_get(bad)+ check(f"path traversal {bad} -> 404", code == 404 and b"import" not in body, str(code))+code, _, _ = cli.get("/nothing")+check("unknown GET -> 404", code == 404)++code, st = cli.json("/state.json")+need = {"ts", "mode", "stack", "pose", "topics", "base", "nav", "recording", "system"}+check("state.json top-level keys", code == 200 and need <= set(st), str(sorted(set(st))))+check("mode demo, stack running moss-explore", st["mode"] == "demo" and st["stack"]["running"]+ and st["stack"]["blueprint"] == "moss-explore")+check("topics carry age_s + hz for odom/global_costmap/lidar/path/cmd_vel/base_telemetry/color_image",+ all(k in st["topics"] and {"age_s", "hz"} <= set(st["topics"][k])+ for k in ("odom", "global_costmap", "lidar", "path", "cmd_vel", "base_telemetry", "color_image")))+b = st["base"]+check("base: battery_v / battery_pct consistent with the 3S gauge",+ b and abs(b["battery_pct"] - mc.battery_pct(b["battery_v"])) <= 1 and 10.9 <= b["battery_v"] <= 11.6,+ f"{b and b['battery_v']} V {b and b['battery_pct']} %")+check("nav keys", {"exploring", "explore_done", "goal", "goal_reached", "last_cmd_vel"} <= set(st["nav"]))+check("recording keys", {"db", "exists", "size_mb", "episode"} <= set(st["recording"]))+sysm = st["system"]+check("system: real cpu/ram/disks/load/uptime", all(k in sysm for k in (+ "cpu_percent", "cpu_per_core", "ram_used_gb", "ram_total_gb", "ram_percent", "temps", "disks",+ "load_1m", "uptime_s")) and sysm["ram_total_gb"] > 0 and isinstance(sysm["disks"], list),+ f"ram {sysm.get('ram_total_gb')} GB, {len(sysm.get('disks', []))} disks")+p0 = st["pose"]+check("demo pose starts on the loop's first corner (-4.25, -4.25)",+ abs(p0["y"] + 4.25) < 0.05 and -4.3 < p0["x"] < -3.5, f"{p0}")++code, headers, png = cli.get("/map.png")+img = png_decode(png)+check("/map.png: valid PNG, demo size 240x240", code == 200 and img.shape == (240, 240, 3)+ and headers["Content-Type"] == "image/png", str(img.shape))+code, meta = cli.json("/map_meta")+check("/map_meta: res 0.05, origin (-6, -6), 240x240, stamp + frame",+ (meta["res"], meta["ox"], meta["oy"], meta["width"], meta["height"], meta["frame"])+ == (0.05, -6.0, -6.0, 240, 240, "world") and meta["stamp"] > 0)+rt = px_of_x(x_of_px(10, meta), meta) # x_of_px = the pixel's centre; px_of_x is continuous+check("px_of_x(x_of_px(10)) == 10 (pixel index = floor)", math.floor(rt) == 10, f"{rt}")+check("py_of_y(y_of_py(10)) == 10 (pixel index = floor)", math.floor(py_of_y(y_of_py(10, meta), meta)) == 10)+++def pixel_at(x, y):+ return tuple(int(c) for c in img[int(py_of_y(y, meta)), int(px_of_x(x, meta))])+++check("wall at (-5.5, -4.25) -> WHITE", pixel_at(-5.5, -4.25) == WHITE, str(pixel_at(-5.5, -4.25)))+check("floor at (-4.25, -4.25) (1.15 m from any wall) -> free colour", pixel_at(-4.25, -4.25) == FREE,+ str(pixel_at(-4.25, -4.25)))+check("outside the walls (-5.9, 5.9) -> unknown", pixel_at(-5.9, 5.9) == UNKNOWN)+check("next to the wall (-5.35, -4.25) -> a cost colour between free and #8FA6B5",+ pixel_at(-5.35, -4.25) not in (FREE, WHITE, UNKNOWN))++code, scan = cli.json("/scan.json")+pts = scan["points"]+truth = demo.cockpit.world.truth+++def on_wall(x, y):+ c = int(math.floor((x + 6.0) / 0.05))+ r = int(math.floor((y + 6.0) / 0.05))+ return 0 <= c < 240 and 0 <= r < 240 and truth[r, c] == 100+++check("/scan.json: world frame, 1..1500 points", scan["frame"] == "world" and 0 < len(pts) <= 1500,+ f"{len(pts)} pts")+check("every scan point lies on a wall cell of the demo map", all(on_wall(x, y) for x, y in pts))+wall_below = [p for p in pts if abs(p[0] - p0["x"]) < 0.2 and p[1] < p0["y"]]+check("the ray straight down from the pose hits the outer wall at y ~ -5.4",+ bool(wall_below) and abs(max(p[1] for p in wall_below) + 5.4) < 0.06,+ f"{max(p[1] for p in wall_below) if wall_below else None}")++code, ctype, frames = cli.mjpeg_frames(4)+check("/camera.mjpg: multipart/x-mixed-replace; boundary=frame",+ code == 200 and ctype == "multipart/x-mixed-replace; boundary=frame", str(ctype))+check("first part is a JPEG (SOI ff d8 ... EOI ff d9)",+ bool(frames) and frames[0][1][:2] == b"\xff\xd8" and frames[0][1][-2:] == b"\xff\xd9")+if len(frames) == 4:+ span = frames[-1][0] - frames[0][0]+ check("4 frames take >= 0.55 s (5 fps cap)", span >= 0.55, f"{span:.2f} s for 3 intervals")+else:+ check("4 frames received", False, f"{len(frames)}")+++def clients_drop_to_zero(cockpit, within_s=2.0):+ deadline = time.monotonic() + within_s+ while cockpit.camera_clients and time.monotonic() < deadline:+ time.sleep(0.05)+ return cockpit.camera_clients == 0+++check("viewer gone -> camera client count back to 0", clients_drop_to_zero(demo.cockpit),+ str(demo.cockpit.camera_clients))++code, ans = cli.post("/cmd/goal", {"x": 1.0, "y": -4.25}, header=False)+_, st = cli.json("/state.json")+check("POST /cmd/goal without 'X-Requested-With: moss-cockpit' -> 403, demo goal untouched",+ code == 403 and ans["ok"] is False and "X-Requested-With" in ans["error"] and st["nav"]["goal"] is None+ and st["nav"]["exploring"] is True, f"{code} {json.dumps(ans)[:100]}")+code, ans = cli.post("/cmd/goal", {"x": 1.0, "y": -4.25})+check("POST /cmd/goal (1.0, -4.25) -> ok, sent goal_request",+ code == 200 and ans["ok"] and ans["sent"] == "/goal_request#geometry_msgs.PoseStamped", json.dumps(ans))+_, st = cli.json("/state.json")+check("state.nav.goal = (1.0, -4.25), goal_reached null, exploring stopped",+ st["nav"]["goal"] == {"x": 1.0, "y": -4.25} and st["nav"]["goal_reached"] is None+ and st["nav"]["exploring"] is False, json.dumps(st["nav"]))+x_before = st["pose"]["x"]+time.sleep(1.0)+_, st = cli.json("/state.json")+dx = st["pose"]["x"] - x_before+check("the demo pose drives toward the goal at ~0.2 m/s", 0.1 < dx < 0.35, f"dx {dx:+.3f} m in ~1 s")+code, path = cli.json("/path.json")+check("/path.json: pose -> goal", code == 200 and path["points"] and path["points"][-1] == [1.0, -4.25],+ json.dumps(path)[:100])+for body, why in (({"x": "a", "y": 0}, "x not a number"), ({"x": 1e6, "y": 0}, "beyond 500 m"),+ ({"y": 0}, "x missing")):+ code, ans = cli.post("/cmd/goal", body)+ check(f"goal refused ({why}) -> 400", code == 400 and ans["ok"] is False, ans.get("error", ""))+code, ans = cli.post("/cmd/goal", None, raw=b"{not json")+check("unreadable JSON body -> 400", code == 400 and ans["ok"] is False)+code, ans = cli.post("/cmd/fly", {})+check("unknown command -> 404", code == 404 and ans["ok"] is False)+code, ans = cli.post("/cmd/stop", {})+check("POST /cmd/stop -> ok, three topics named", code == 200 and ans["ok"]+ and len([ans["sent"]] + ans["also"]) == 3, json.dumps(ans))+_, st = cli.json("/state.json")+time.sleep(0.4)+_, st2 = cli.json("/state.json")+check("after the soft stop the demo pose holds still",+ abs(st2["pose"]["x"] - st["pose"]["x"]) < 1e-6 and st2["nav"]["goal"] is not None)+code, ans = cli.post("/cmd/episode", {"action": "start", "name": "ep3"})+_, st = cli.json("/state.json")+check("episode start ep3 -> state.recording.episode.name", code == 200 and ans["ok"]+ and st["recording"]["episode"]["name"] == "ep3", json.dumps(st["recording"]["episode"]))+code, ans = cli.post("/cmd/episode", {"action": "start", "name": "../x"})+check("episode name with a slash -> 400", code == 400)+cli.post("/cmd/episode", {"action": "stop"})+_, st = cli.json("/state.json")+check("episode stop -> null", st["recording"]["episode"] is None)+code, ans = cli.post("/cmd/stack", {"action": "map"})+check("demo: stack map while running -> refused", code == 200 and ans["ok"] is False, ans.get("error", ""))+cli.post("/cmd/stack", {"action": "stop"})+_, st = cli.json("/state.json")+check("demo: stack stop -> running false, pose null", st["stack"] == {"running": False} and st["pose"] is None)+code, ans = cli.post("/cmd/stack", {"action": "map"})+_, st = cli.json("/state.json")+check("demo: stack map -> moss-nav running", ans["ok"] and st["stack"]["blueprint"] == "moss-nav")+code, lg = cli.json("/log.json?n=2")+check("/log.json?n=2 -> 2 records {ts, level, event, logger}", code == 200 and len(lg["lines"]) == 2+ and set(lg["lines"][0]) == {"ts", "level", "event", "logger"})+code, pf = cli.json("/preflight.json")+check("/preflight.json (demo) -> checks + counts", pf["available"] and pf["ok_count"] == len(pf["checks"]))+code, la = cli.json("/launch.json")+check("/launch.json -> running flag + lines", code == 200 and "running" in la and isinstance(la["lines"], list))+demo.shutdown()++page_none = os.path.join(TMP, "no_page")+os.makedirs(page_none)+srv, c2 = serve(demo=True, page_dir=page_none)+code, _, body = c2.get("/")+check("no index.html -> one-line placeholder page, 200", code == 200 and b"not installed" in body)+srv.shutdown()++# ---------------------------------------------------------------------------+print("C. decoding (dimos_lcm messages fed to a live cockpit)")+live = None+if not HAVE_LCM_MSGS or not mc.HAVE_MSGS:+ print(" -- skipped, no dimos_lcm here (it comes with dimOS: run in the robot's venv)")+else:+ RUNS = Path(os.environ["XDG_STATE_HOME"], "dimos", "runs")++ def stand_in(**bus):+ """A live process started with a known bus environment: a registered stack's pid,+ whose /proc/<pid>/environ the cockpit reads to learn the bus that stack chose."""+ env = {k: v for k, v in os.environ.items() if k not in ("DIMOS_TRANSPORT", "TRANSPORT")}+ env.update(bus)+ proc = subprocess.Popen(["sleep", "600"], env=env, stdin=subprocess.DEVNULL)+ atexit.register(proc.kill)+ return proc++ STACK_LCM = stand_in() # neither variable: lcm+ STACK_ZENOH = stand_in(DIMOS_TRANSPORT="zenoh", TRANSPORT="lcm") # dimOS runs this one on zenoh++ def register(run_id, blueprint, pid=None):+ """A live dimOS registry entry (default: the stand-in started on lcm), or none with run_id None."""+ RUNS.mkdir(parents=True, exist_ok=True)+ f = RUNS / "bench.json"+ if run_id is None:+ f.unlink(missing_ok=True)+ return+ f.write_text(json.dumps({"run_id": run_id, "pid": pid or STACK_LCM.pid, "blueprint": blueprint,+ "started_at": _dt.datetime.now(_dt.timezone.utc).isoformat(),+ "log_dir": "/nowhere"}))++ real_run_pid = mc._dimos_run_pid+ mc._dimos_run_pid = lambda: None # this machine's own dimos processes never count as a stack+ register("run-a", "moss-dimos.moss-explore")+ live, lcli = serve(demo=False, page_dir=page_dir)+ bus = live.cockpit.bus+ code, st = lcli.json("/state.json")+ check("live, empty bus: pose null, base null, topics never seen", code == 200 and st["mode"] == "live"+ and st["pose"] is None and st["base"] is None and st["topics"]["odom"]["age_s"] is None)+ code, _, _ = lcli.get("/map.png")+ check("no costmap yet -> /map.png 404", code == 404)++ g = OccupancyGrid(data_length=6, header=hdr("world", 1760000000, 500000000),+ info=MapMetaData(map_load_time=Time(), resolution=0.5, width=3, height=2,+ origin=pose_of(1.0, 2.0)),+ data=[-1, 0, 100, 50, 0, -1])+ bus.ingest(mc.CHANNELS["global_costmap"], g.lcm_encode())+ code, _, png = lcli.get("/map.png")+ img = png_decode(png)+ got = [[tuple(int(c) for c in img[r, k]) for k in range(3)] for r in range(2)]+ check("OccupancyGrid 3x2 res 0.5 origin (1,2) -> PNG 3x2", img.shape == (2, 3, 3))+ check("exact pixels: [cost50, free, unknown] / [unknown, free, WHITE]", got == expect, str(got))+ code, meta = lcli.json("/map_meta")+ check("map_meta from the message: res 0.5, ox 1, oy 2, stamp 1760000000.5, frame world",+ (meta["res"], meta["ox"], meta["oy"], meta["stamp"], meta["frame"])+ == (0.5, 1.0, 2.0, 1760000000.5, "world"), json.dumps(meta)[:120])++ def cloud(points, frame):+ m = PointCloud2(header=hdr(frame))+ m.height, m.width = 1, len(points)+ fields = []+ for i, name in enumerate(("x", "y", "z", "intensity")):+ f = PointField()+ f.name, f.offset, f.datatype, f.count = name, 4 * i, 7, 1+ fields.append(f)+ m.fields, m.fields_length = fields, 4+ m.point_step, m.row_step, m.is_bigendian, m.is_dense = 16, 16 * len(points), False, True+ m.data = np.array([[x, y, z, 0.0] for x, y, z in points], dtype="<f4").tobytes()+ m.data_length = len(m.data)+ return m.lcm_encode()++ bus.ingest(mc.CHANNELS["pointcloud"], cloud([(0.5, 0.25, 0.0)], "lidar_link"))+ code, scan = lcli.json("/scan.json")+ check("/lidar never seen -> /pointcloud fallback, lidar frame, (0.5, 0.25)",+ scan["frame"] == "lidar_link" and scan["source"] == "pointcloud" and scan["points"] == [[0.5, 0.25]],+ json.dumps(scan)[:120])+ bus.ingest(mc.CHANNELS["lidar"], cloud([(1.0, 2.0, 0.37), (3.0, -4.0, 0.37)], "world"))+ time.sleep(mc.SCAN_MIN_INTERVAL_S + 0.05)+ code, scan = lcli.json("/scan.json")+ check("PointCloud2 with 2 points on /lidar -> [[1, 2], [3, -4]] world",+ scan["frame"] == "world" and scan["points"] == [[1.0, 2.0], [3.0, -4.0]], json.dumps(scan)[:120])+ big = [(math.cos(a), math.sin(a), 0.0) for a in np.linspace(0, 2 * math.pi, 3000)]+ bus.ingest(mc.CHANNELS["lidar"], cloud(big, "world"))+ time.sleep(mc.SCAN_MIN_INTERVAL_S + 0.05)+ code, scan = lcli.json("/scan.json")+ radii = [math.hypot(x, y) for x, y in scan["points"]]+ check("3000-point ring of radius 1 m -> 1500 points, all at 1 m",+ len(scan["points"]) == 1500 and max(abs(r - 1.0) for r in radii) < 2e-3, f"{len(scan['points'])}")++ pose = PoseStamped(header=hdr("world"), pose=pose_of(1.25, -0.5, qz=s45, qw=s45))+ for _ in range(3):+ bus.ingest(mc.CHANNELS["odom"], pose.lcm_encode())+ time.sleep(0.1)+ tele = {"ts": time.time(), "fw": "qtpy-mdd3a-v3", "driver": None, "mode": 0, "reason": "boot",+ "bus_mV": 11400, "ina_ok": True, "ticks": [5, 6], "target_pct": [0, 0], "output_pct": [0, 0],+ "ramp_ms": [5, 5], "firmware_age_s": 0.2, "mock": False}+ bus.ingest(mc.CHANNELS["base_telemetry"], String(data=json.dumps(tele)).lcm_encode())+ code, st = lcli.json("/state.json")+ check("PoseStamped (1.25, -0.5), quaternion yaw 90 deg -> pose yaw 1.5708",+ st["pose"]["x"] == 1.25 and st["pose"]["y"] == -0.5 and abs(st["pose"]["yaw"] - 1.5708) < 1e-4,+ json.dumps(st["pose"]))+ check("odom 3 messages 0.1 s apart -> ~10 Hz", 7.0 <= st["topics"]["odom"]["hz"] <= 11.0,+ f"{st['topics']['odom']['hz']} Hz")+ b = st["base"]+ check("String telemetry 11400 mV -> battery 11.4 V, 50 %, mode off, fw, driver null",+ (b["battery_v"], b["battery_pct"], b["mode_name"], b["fw"], b["driver"])+ == (11.4, 50, "off", "qtpy-mdd3a-v3", None), json.dumps(b)[:160])++ poses = [PoseStamped(header=hdr("world"), pose=pose_of(x, y))+ for x, y in ((0.0, 0.0), (0.5, 0.25), (1.0, 1.0))]+ path = LcmPath(poses_length=3, header=hdr("world"), poses=poses)+ bus.ingest(mc.CHANNELS["path"], path.lcm_encode())+ code, pj = lcli.json("/path.json")+ check("Path of 3 poses -> [[0,0],[0.5,0.25],[1,1]]", pj["points"] == [[0.0, 0.0], [0.5, 0.25], [1.0, 1.0]],+ json.dumps(pj))++ t0 = 100.0+ bus.ingest(mc.CHANNELS["explore_cmd"], Bool(data=True).lcm_encode(), mono=t0)+ code, st = lcli.json("/state.json")+ check("explore_cmd True -> exploring", st["nav"]["exploring"] is True)+ bus.ingest(mc.CHANNELS["stop_explore_cmd"], Bool(data=True).lcm_encode(), mono=t0 + 1)+ code, st = lcli.json("/state.json")+ check("then stop_explore_cmd -> not exploring", st["nav"]["exploring"] is False)+ bus.ingest(mc.CHANNELS["explore_cmd"], Bool(data=True).lcm_encode(), mono=t0 + 2)+ bus.ingest(mc.CHANNELS["explore_done"], Bool(data=True).lcm_encode(), mono=t0 + 3)+ code, st = lcli.json("/state.json")+ check("start again, then explore_done -> not exploring, explore_done true",+ st["nav"]["exploring"] is False and st["nav"]["explore_done"] is True, json.dumps(st["nav"]))+ goal = PoseStamped(header=hdr("world"), pose=pose_of(2.0, 3.5))+ bus.ingest(mc.CHANNELS["goal_request"], goal.lcm_encode(), mono=t0 + 4)+ bus.ingest(mc.CHANNELS["goal_reached"], Bool(data=True).lcm_encode(), mono=t0 + 3.5)+ code, st = lcli.json("/state.json")+ check("goal_request (2, 3.5) -> nav.goal; an older goal_reached does not count",+ st["nav"]["goal"] == {"x": 2.0, "y": 3.5} and st["nav"]["goal_reached"] is None)+ bus.ingest(mc.CHANNELS["goal_reached"], Bool(data=True).lcm_encode(), mono=t0 + 5)+ code, st = lcli.json("/state.json")+ check("goal_reached after it -> true", st["nav"]["goal_reached"] is True)+ nan_goal = PoseStamped(header=hdr("world"), pose=pose_of(float("nan"), float("nan")))+ bus.ingest(mc.CHANNELS["goal_request"], nan_goal.lcm_encode(), mono=t0 + 6)+ code, st = lcli.json("/state.json")+ check("a NaN goal (dimOS cancel) -> goal null, still valid JSON", code == 200 and st["nav"]["goal"] is None)+ bus.ingest(mc.CHANNELS["explore_cmd"], Bool(data=True).lcm_encode(), mono=t0 + 7)+ exp_a = lcli.json("/state.json")[1]["nav"]["exploring"]+ bus.ingest(mc.CHANNELS["stop_movement"], Bool(data=True).lcm_encode(), mono=t0 + 8)+ exp_b = lcli.json("/state.json")[1]["nav"]["exploring"]+ bus.ingest(mc.CHANNELS["explore_cmd"], Bool(data=True).lcm_encode(), mono=t0 + 9)+ exp_c = lcli.json("/state.json")[1]["nav"]["exploring"]+ check("explore_cmd -> exploring; a newer stop_movement (pad touch) -> not; a newer explore_cmd -> again",+ (exp_a, exp_b, exp_c) == (True, False, True), str((exp_a, exp_b, exp_c)))+ tw = Twist(linear=Vector3(x=0.15, y=0.0, z=0.0), angular=Vector3(x=0.0, y=0.0, z=-0.3))+ bus.ingest(mc.CHANNELS["cmd_vel"], tw.lcm_encode())+ code, st = lcli.json("/state.json")+ check("cmd_vel (0.15 m/s, -0.3 rad/s) -> last_cmd_vel", (st["nav"]["last_cmd_vel"]["vx"],+ st["nav"]["last_cmd_vel"]["wz"]) == (0.15, -0.3))+ marker = {"action": "start", "name": "bus-ep", "ts": 1760000001.0}+ bus.ingest(mc.CHANNELS["episode_marker"], String(data=json.dumps(marker)).lcm_encode())+ code, st = lcli.json("/state.json")+ check("an episode_marker from anyone -> state.recording.episode",+ st["recording"]["episode"] == {"name": "bus-ep", "started_at": 1760000001.0})++ def feed_image(enc, data, w, h):+ im = Image(header=hdr("camera"))+ im.width, im.height, im.encoding = w, h, enc+ im.step = 0 if enc == "jpeg" else w * 3+ im.data, im.data_length = data, len(data)++ # The first frame is on the bus before any reader connects (a new viewer gets the+ # latest frame at once); the pump returned is joined before the next one starts,+ # so two pumps never write the same slot.+ bus.ingest(mc.IMAGE_CHANNEL, im.lcm_encode())++ def pump():+ for _ in range(29):+ time.sleep(0.05)+ bus.ingest(mc.IMAGE_CHANNEL, im.lcm_encode())+ t = threading.Thread(target=pump, daemon=True)+ t.start()+ return t++ pumps = [feed_image("jpeg", mc.DEMO_JPEG, 320, 240)]+ code, ctype, frames = lcli.mjpeg_frames(2)+ check("JPEG on the bus (JpegLcmTransport) -> MJPEG part byte for byte",+ code == 200 and frames and frames[0][1] == mc.DEMO_JPEG, f"{len(frames)} frames")+ pumps[-1].join(5.0) # the JPEG pump is over before the raw frame goes out+ try:+ import cv2 # noqa: F401+ have_enc = True+ except ImportError:+ try:+ import turbojpeg # noqa: F401+ have_enc = True+ except ImportError:+ have_enc = False+ if have_enc:+ raw = np.zeros((8, 8, 3), np.uint8)+ raw[:, :, 0] = 200+ pumps.append(feed_image("rgb8", raw.tobytes(), 8, 8))+ code, ctype, frames = lcli.mjpeg_frames(1)+ ok_jpeg = code == 200 and frames and frames[0][1][:2] == b"\xff\xd8"+ detail = ""+ if ok_jpeg:+ import cv2+ dec = cv2.imdecode(np.frombuffer(frames[0][1], np.uint8), cv2.IMREAD_COLOR)+ detail = f"decoded BGR {dec[4, 4].tolist()}"+ ok_jpeg = abs(int(dec[4, 4, 2]) - 200) <= 8 and int(dec[4, 4, 0]) <= 8 # red stays red+ check("raw rgb8 8x8 red frame -> encoded JPEG, still red", ok_jpeg, detail)+ else:+ print(" -- skipped, no cv2/turbojpeg here: raw-frame encoding")+ for pump in pumps:+ pump.join(5.0) # every pump is over: the camera is silent, nothing left to write+ check("viewers gone while the camera is silent -> 0 clients (camera unsubscribed)",+ clients_drop_to_zero(live.cockpit), str(live.cockpit.camera_clients))++# ---------------------------------------------------------------------------+print("D. commands on the bus (fake LCM handle)")+TRUE_BYTES = b"\x1e\xbe\xf0jP\x95\x0e>\x01" # std_msgs.Bool fingerprint + data=True+if live is None:+ print(" -- skipped, no dimos_lcm here (it comes with dimOS: run in the robot's venv)")+else:+ sent = []++ class FakeLCM:+ def publish(self, channel, payload):+ sent.append((channel, bytes(payload)))++ fake_mod = types.SimpleNamespace(LCM=FakeLCM)+ real_lcm = mc.lcm+ real_sys_lcm = sys.modules.get("lcm")+ mc.lcm = fake_mod+ sys.modules["lcm"] = fake_mod # explore_ctl imports it when no handle is passed+ live.cockpit._pub_lcm = None+ try:+ import explore_ctl+ check("explore_ctl.topic unchanged: start/stop",+ (explore_ctl.topic("start"), explore_ctl.topic("stop"))+ == ("/explore_cmd#std_msgs.Bool", "/stop_explore_cmd#std_msgs.Bool"))+ used = explore_ctl.publish("stop")+ check("explore_ctl.publish('stop') -> Bool(True) bytes on /stop_explore_cmd",+ used == "/stop_explore_cmd#std_msgs.Bool" and sent == [(used, TRUE_BYTES)], str(sent))+ check("generic channel -> /goal_request#geometry_msgs.PoseStamped",+ explore_ctl.channel("goal_request", "geometry_msgs.PoseStamped")+ == "/goal_request#geometry_msgs.PoseStamped")+ sent.clear()+ # the running stack's own environment decides the bus: dimOS read it when the stack started+ register("run-a", "moss-dimos.moss-explore", pid=STACK_ZENOH.pid)+ code_e, ans_e = lcli.post("/cmd/explore", {"action": "start"})+ code_g, ans_g = lcli.post("/cmd/goal", {"x": 1.0, "y": 1.0})+ code_s, ans_s = lcli.post("/cmd/stop", {})+ _, st_z = lcli.json("/state.json")+ register("run-a", "moss-dimos.moss-explore")+ errs = [a.get("error", "") for a in (ans_e, ans_g, ans_s)]+ check("a MOSS stack started with DIMOS_TRANSPORT=zenoh TRANSPORT=lcm, the cockpit on lcm -> explore, "+ "goal and STOP refused 503 (LCM only) naming DIMOS_TRANSPORT=zenoh and the stack's pid, nothing "+ "published, state.bus.transport zenoh",+ (code_e, code_g, code_s) == (503, 503, 503)+ and all("here: DIMOS_TRANSPORT=zenoh:" in e and "LCM only" in e+ and f"the running stack's environment, pid {STACK_ZENOH.pid}" in e for e in errs)+ and sent == [] and st_z["bus"]["transport"] == "zenoh",+ f"{code_e} {code_g} {code_s} {errs[0][:160]!r} {sent} {st_z['bus']}")+ sent.clear()+ os.environ["DIMOS_TRANSPORT"] = "zenoh"+ code, ans = lcli.post("/cmd/explore", {"action": "start"})+ _, st_l = lcli.json("/state.json")+ os.environ["DIMOS_TRANSPORT"] = "lcm"+ check("a MOSS stack started with neither variable (lcm), the cockpit's own DIMOS_TRANSPORT=zenoh -> "+ "the stack's decides: explore start published, state.bus.transport lcm",+ code == 200 and ans["ok"] and [c for c, _ in sent] == ["/explore_cmd#std_msgs.Bool"]+ and st_l["bus"]["transport"] == "lcm", f"{code} {ans} {sent} {st_l['bus']}")+ sent.clear()++ code, ans = lcli.post("/cmd/explore", {"action": "start"})+ check("POST explore start -> /explore_cmd Bool(True)", ans["ok"]+ and sent == [("/explore_cmd#std_msgs.Bool", TRUE_BYTES)], str(sent))+ sent.clear()+ code, ans = lcli.post("/cmd/explore", {"action": "start"}, header=False)+ check("POST explore start without the cockpit header -> 403, nothing published",+ code == 403 and ans["ok"] is False and sent == [], f"{code} {sent}")+ queued = []+ real_later = mc._later+ mc._later = lambda delay, fn: queued.append((delay, fn))+ GOAL_CH = "/goal_request#geometry_msgs.PoseStamped"+ STOP_CH = "/stop_explore_cmd#std_msgs.Bool"+ # not exploring (the bench bus says explore_cmd at t0+9, stop_explore_cmd at t0+10)+ bus.ingest(mc.CHANNELS["stop_explore_cmd"], Bool(data=True).lcm_encode(), mono=t0 + 10)+ sent.clear()+ code, ans = lcli.post("/cmd/goal", {"x": 1.5, "y": 0.5})+ g0 = PoseStamped.lcm_decode(sent[0][1]) if sent else None+ check("goal while NOT exploring -> goal_request (1.5, 0.5) at once, no stop_explore_cmd, nothing queued",+ [c for c, _ in sent] == [GOAL_CH] and queued == [] and ans.get("queued_s") is None+ and (g0.pose.position.x, g0.pose.position.y) == (1.5, 0.5), f"{[c for c, _ in sent]} {ans}")+ # exploring: stop now, goal 3.0 s later+ bus.ingest(mc.CHANNELS["explore_cmd"], Bool(data=True).lcm_encode())+ sent.clear()+ code, ans = lcli.post("/cmd/goal", {"x": 2.5, "y": -1.0})+ chans = [c for c, _ in sent]+ check("goal while exploring -> stop_explore_cmd now, answer queued_s 3.0, goal held back",+ code == 200 and ans["ok"] and chans == [STOP_CH] and ans["sent"] == STOP_CH+ and ans["queued_s"] == 3.0 and ans["queued"] == GOAL_CH, f"{chans} {ans}")+ check("the goal is handed to a timer of 3.0 s", [d for d, _ in queued] == [3.0], str(queued))+ sent.clear()+ if queued:+ queued.pop()[1]() # the 3 s are up+ chans = [c for c, _ in sent]+ check("when the timer fires -> goal_request only", chans == [GOAL_CH], str(chans))+ g = PoseStamped.lcm_decode(sent[-1][1]) if sent else None+ check("goal bytes decode to (2.5, -1.0, 0) frame world, orientation (0,0,0,1), stamped now",+ g is not None and (g.pose.position.x, g.pose.position.y, g.pose.position.z) == (2.5, -1.0, 0.0)+ and g.header.frame_id == "world"+ and (g.pose.orientation.x, g.pose.orientation.y, g.pose.orientation.z, g.pose.orientation.w)+ == (0.0, 0.0, 0.0, 1.0) and abs(g.header.stamp.sec - time.time()) < 5)+ sent.clear()+ lcli.post("/cmd/goal", {"x": 4.0, "y": 4.0}) # still exploring: queued again+ lcli.post("/cmd/stop", {}) # the operator stops inside the 3 s+ sent.clear()+ if queued:+ queued.pop()[1]()+ check("a STOP inside the 3 s drops the queued goal: nothing published when the timer fires",+ sent == [], str(sent))+ mc._later = real_later+ register(None, None) # the stop check below runs with no stack at all+ sent.clear()+ polluted = Twist() # what a careless caller does to the shared default+ polluted.linear.x = 9.9+ code, ans = lcli.post("/cmd/stop", {})+ polluted.linear.x = 0.0+ chans = [c for c, _ in sent]+ check("POST stop, no stack -> stop_explore, stop_movement, tele_cmd_vel, cmd_vel",+ chans == ["/stop_explore_cmd#std_msgs.Bool", "/stop_movement#std_msgs.Bool",+ "/tele_cmd_vel#geometry_msgs.Twist", "/cmd_vel#geometry_msgs.Twist"], str(chans))+ zero = Twist.lcm_decode(sent[2][1]) if len(sent) > 2 else None+ check("the twist is all zeros (even with dimos_lcm's shared default Vector3 dirtied)", zero is not None and (zero.linear.x, zero.linear.y, zero.linear.z,+ zero.angular.x, zero.angular.y, zero.angular.z) == (0.0,) * 6)+ check("stop_movement payload = Bool(True)", len(sent) > 1 and sent[1][1] == TRUE_BYTES)+ # no stack: the cockpit's own environment stands in (a stack launched from the page inherits it)+ own = {}+ for dimos_tr, tr in ((None, "zenoh"), ("zenoh", "lcm")):+ if dimos_tr is None:+ os.environ.pop("DIMOS_TRANSPORT", None)+ else:+ os.environ["DIMOS_TRANSPORT"] = dimos_tr+ os.environ["TRANSPORT"] = tr+ sent.clear()+ c_, a_ = lcli.post("/cmd/stop", {})+ _, st_ = lcli.json("/state.json")+ own[(dimos_tr, tr)] = (c_, a_.get("error", ""), list(sent), st_["bus"]["transport"])+ os.environ["DIMOS_TRANSPORT"] = os.environ["TRANSPORT"] = "lcm"+ for (dimos_tr, tr), named in (((None, "zenoh"), "TRANSPORT=zenoh"),+ (("zenoh", "lcm"), "DIMOS_TRANSPORT=zenoh")):+ c_, e_, s_, t_ = own[(dimos_tr, tr)]+ pair = f"DIMOS_TRANSPORT={dimos_tr} TRANSPORT={tr}" if dimos_tr else f"TRANSPORT={tr} alone"+ check(f"no stack, the cockpit's own {pair} -> STOP refused 503 naming {named} and the cockpit's "+ f"environment, nothing published, state.bus.transport zenoh",+ c_ == 503 and f"here: {named}:" in e_ and "the cockpit's own environment" in e_ and s_ == []+ and t_ == "zenoh", f"{c_} {e_[:160]!r} {s_} {t_}")+ sent.clear()+ register("run-a", "moss-dimos.moss-explore")+ sent.clear()+ code, ans = lcli.post("/cmd/episode", {"action": "start", "name": "ep7"})+ m = json.loads(String.lcm_decode(sent[0][1]).data) if sent else {}+ check("POST episode start ep7 -> /episode_marker String {action, name, ts}",+ sent and sent[0][0] == "/episode_marker#std_msgs.String" and m.get("action") == "start"+ and m.get("name") == "ep7" and abs(m.get("ts", 0) - time.time()) < 5, json.dumps(m))+ _, st = lcli.json("/state.json")+ check("state.recording.episode = ep7", (st["recording"]["episode"] or {}).get("name") == "ep7")+ sent.clear()+ code, ans = lcli.post("/cmd/explore", {"action": "go"})+ check("explore with a bad action -> 400, nothing published", code == 400 and sent == [])+ # another robot's explore stack on the same computer and bus: hands off+ register("run-x", "other-dimos.explore")+ sent.clear()+ c1, a1 = lcli.post("/cmd/explore", {"action": "start"})+ c2, a2 = lcli.post("/cmd/goal", {"x": 1.0, "y": 1.0})+ check("a foreign stack (blueprint 'explore') -> explore start and goal refused 409, nothing published",+ (c1, c2) == (409, 409) and not a1["ok"] and not a2["ok"] and sent == [], f"{c1} {c2} {sent}")+ stub_ok = Path(TMP, "stub_never.sh")+ stub_ok.write_text("echo launched > " + str(Path(TMP, "launched.txt")) + "\n")+ live.cockpit.launcher.script = str(stub_ok) # never the real launcher, even on a KO+ c3_, a3 = lcli.post("/cmd/stack", {"action": "explore"})+ time.sleep(0.3)+ check("a foreign stack -> stack launch refused 409 (not MOSS), the launcher never ran",+ c3_ == 409 and not a3["ok"] and "not MOSS" in a3["error"]+ and not Path(TMP, "launched.txt").exists(), f"{c3_} {a3}")+ sent.clear()+ c4, a4 = lcli.post("/cmd/stop", {})+ check("a foreign stack -> the soft STOP is still allowed (published)", c4 == 200 and a4["ok"]+ and "/stop_movement#std_msgs.Bool" in [c for c, _ in sent], str([c for c, _ in sent]))+ register(None, None)+ finally:+ for proc in (STACK_LCM, STACK_ZENOH):+ proc.kill()+ proc.wait()+ mc._dimos_run_pid = real_run_pid+ mc.lcm = real_lcm+ if real_sys_lcm is None:+ sys.modules.pop("lcm", None)+ else:+ sys.modules["lcm"] = real_sys_lcm++# ---------------------------------------------------------------------------+print("E. stack, logs, flight check, launcher")+runs = Path(os.environ["XDG_STATE_HOME"], "dimos", "runs")+runs.mkdir(parents=True, exist_ok=True)+logdir = Path(TMP, "logs", "run1")+logdir.mkdir(parents=True)+dead = subprocess.Popen(["true"])+dead.wait()+started = time.time() - 120.0+iso = _dt.datetime.fromtimestamp(started, _dt.timezone.utc).isoformat()+Path(runs, "old.json").write_text(json.dumps({"run_id": "old", "pid": dead.pid, "blueprint": "moss-dimos.moss-explore",+ "started_at": iso, "log_dir": "/nowhere"}))+check("registry with only a dead pid -> no stack", mc.stack_status() == {"running": False})+Path(runs, "live.json").write_text(json.dumps({"run_id": "20260101-120000-moss-dimos-moss-nav", "pid": os.getpid(),+ "blueprint": "moss-dimos.moss-nav", "started_at": iso,+ "log_dir": str(logdir)}))+s = mc.stack_status()+check("live pid -> running, blueprint moss-nav, uptime 120 s",+ s["running"] and s["blueprint"] == "moss-nav" and s["pid"] == os.getpid()+ and abs(s["uptime_s"] - 120.0) < 2.0, json.dumps(s))+recs = [{"timestamp": f"2026-10-11T12:00:0{i}Z", "level": "info", "event": f"line {i}", "logger": "x.py",+ "func_name": "f"} for i in range(3)]+Path(logdir, "main.jsonl").write_text("\n".join(json.dumps(r) for r in recs) + "\nnot json\n")+check("main.jsonl tail n=2 (after a non-JSON last line) -> the last two records, four keys",+ mc.log_tail(str(logdir), 2) == [{"ts": "2026-10-11T12:00:01Z", "level": "info", "event": "line 1",+ "logger": "x.py"}, {"ts": "2026-10-11T12:00:02Z", "level": "info",+ "event": "line 2", "logger": "x.py"}],+ json.dumps(mc.log_tail(str(logdir), 2))[:160])+check("no preflight.json -> available false", mc.preflight_doc() == {"available": False})+os.makedirs(os.environ["MOSS_STATE_DIR"], exist_ok=True)+Path(os.environ["MOSS_STATE_DIR"], "preflight.json").write_text(json.dumps(+ {"ts": 1.0, "checks": [{"ok": True, "label": "stack", "detail": "none", "section": "stack"}],+ "ok_count": 1, "ko_count": 0, "summary": "1/1 OK - ready to fly"}))+pf = mc.preflight_doc()+check("saved preflight.json -> served with available true", pf["available"] and pf["summary"] == "1/1 OK - ready to fly")++stub = Path(TMP, "fly_stub.sh")+stub.write_text("echo \"stub $1 start\"\nsleep 1\necho \"stub $1 end\"\n")+real_pid = mc._dimos_run_pid+mc._dimos_run_pid = lambda: None+srv, c3 = serve(demo=False, page_dir=page_dir, fly_script=str(stub))+code, st = c3.json("/state.json")+check("state.json stack from the registry", st["stack"]["running"] and st["stack"]["blueprint"] == "moss-nav")+code, lg = c3.json("/log.json?n=1")+check("/log.json?n=1 -> the stack's last record", lg["lines"] == [{"ts": "2026-10-11T12:00:02Z", "level": "info",+ "event": "line 2", "logger": "x.py"}])+code, ans = c3.post("/cmd/stack", {"action": "explore"})+check("stack explore while a stack runs -> refused", ans["ok"] is False and "already running" in ans["error"])+Path(runs, "live.json").unlink()+t = time.monotonic()+code, ans = c3.post("/cmd/stack", {"action": "check"})+elapsed = time.monotonic() - t+check("stack check -> ok at once (non-blocking: the stub sleeps 1 s)", ans["ok"] and elapsed < 0.5,+ f"{elapsed:.2f} s")+code, la = c3.json("/launch.json")+check("/launch.json while it runs -> running true", la["running"] is True and la["action"] == "check")+code, ans = c3.post("/cmd/stack", {"action": "map"})+check("a second launch while the first runs -> refused", ans["ok"] is False)+time.sleep(1.5)+code, la = c3.json("/launch.json")+check("after it ends -> running false, rc 0, output in last_launch.log",+ la["running"] is False and la["returncode"] == 0 and "stub check end" in la["lines"]+ and os.path.isfile(os.path.join(os.environ["MOSS_STATE_DIR"], "last_launch.log")), json.dumps(la)[:200])+code, ans = c3.post("/cmd/stack", {"action": "dance"})+check("stack with an unknown action -> 400", code == 400)+code, pf = c3.json("/preflight.json")+check("/preflight.json (live) -> the saved document", pf.get("ok_count") == 1)+srv.shutdown()+mc._dimos_run_pid = real_pid+if live is not None:+ live.shutdown()++# ---------------------------------------------------------------------------+print("G. robustness: launcher environment and cancel, run change, orphan episode, dead viewers")+for f in runs.glob("*.json"):+ f.unlink()+real_pid = mc._dimos_run_pid+mc._dimos_run_pid = lambda: None++# the per-run switches of a CLI run never reach a page launch+envstub = Path(TMP, "env_stub.sh")+envstub.write_text('echo "env EXPLORE=${EXPLORE--} MOCK=${MOCK--} MOSS_MOCK_BUS=${MOSS_MOCK_BUS--} '+ 'STATE=${MOSS_STATE_DIR--}"\n')+saved = {k: os.environ.get(k) for k in ("EXPLORE", "MOCK", "MOSS_MOCK_BUS")}+os.environ.update(EXPLORE="1", MOCK="1", MOSS_MOCK_BUS="1")+la = mc.Launcher(str(envstub))+ans = la.start("explore")+for k, v in saved.items():+ if v is None:+ os.environ.pop(k, None)+ else:+ os.environ[k] = v+la._procs[-1][2].wait(5)+lines = la.status()["lines"]+want = f"env EXPLORE=- MOCK=- MOSS_MOCK_BUS=- STATE={os.environ['MOSS_STATE_DIR']}"+check("cockpit env has EXPLORE=1 MOCK=1 MOSS_MOCK_BUS=1 -> the launched script sees none of them, "+ "MOSS_STATE_DIR still passed", ans["ok"] and want in lines, str(lines[-2:]))++# Stop during a launch ends the launch first+def fly_stub(trap):+ p = Path(TMP, f"fly_cancel_{int(trap)}.sh")+ ignore_term = "trap '' TERM; " if trap else ""+ p.write_text(f'case "$1" in\n map) {ignore_term}'+ f'sleep 30 & echo $! > {TMP}/sleep_{int(trap)}.pid; wait; echo "map reached the stack";;\n'+ ' stop) echo "stub stop ran";;\nesac\n')+ return p+++def pid_dead(pid, within_s=2.0):+ deadline = time.monotonic() + within_s+ while time.monotonic() < deadline:+ try:+ os.kill(pid, 0)+ except ProcessLookupError:+ return True+ time.sleep(0.05)+ return False+++for trap, sig, grace in ((False, "SIGTERM", None), (True, "SIGKILL", 0.5)):+ real_grace = mc.LAUNCH_KILL_GRACE_S+ if grace:+ mc.LAUNCH_KILL_GRACE_S = grace+ la = mc.Launcher(str(fly_stub(trap)))+ la.start("map")+ pidfile = Path(TMP, f"sleep_{int(trap)}.pid")+ deadline = time.monotonic() + 3+ while not (pidfile.exists() and pidfile.read_text().strip()) and time.monotonic() < deadline:+ time.sleep(0.05)+ map_proc = la._procs[-1][2]+ t = time.monotonic()+ ans = la.start("stop")+ took = time.monotonic() - t+ la._procs[-1][2].wait(5)+ lines = la.status()["lines"]+ rc = map_proc.poll()+ mc.LAUNCH_KILL_GRACE_S = real_grace+ want_rc = -signal.SIGTERM if sig == "SIGTERM" else -signal.SIGKILL+ check(f"Stop while 'map' is in its gates{' (TERM ignored)' if trap else ''} -> map ended by {sig} "+ f"(rc {want_rc}), its child sleep gone, then 'stop' ran",+ ans["ok"] and rc == want_rc and pid_dead(int(pidfile.read_text()))+ and any(f"cancelled by stop: {sig}" in ln for ln in lines) and "stub stop ran" in lines+ and "map reached the stack" not in lines, f"rc {rc}, {took:.2f} s, {lines[-3:]}")++# an episode open when the stack stops is closed by the cockpit+register_g = Path(runs, "g.json")+++def reg(run_id, blueprint="moss-dimos.moss-explore"):+ if run_id is None:+ register_g.unlink(missing_ok=True)+ else:+ register_g.write_text(json.dumps({"run_id": run_id, "pid": os.getpid(), "blueprint": blueprint,+ "started_at": _dt.datetime.now(_dt.timezone.utc).isoformat(),+ "log_dir": "/nowhere"}))+++lc = mc.LiveCockpit(str(envstub))+reg("run-1")+lc._episode({"action": "start", "name": "ep9"})+ep_on = lc.state()["recording"]["episode"]+reg(None)+rec = lc.state()["recording"]+check("episode ep9 open, stack stops -> episode null, recording.closed names ep9 and says why",+ (ep_on or {}).get("name") == "ep9" and rec["episode"] is None and (rec["closed"] or {}).get("name") == "ep9"+ and "stack stopped" in rec["closed"]["reason"], json.dumps(rec)[:200])+lc._episode({"action": "start", "name": "ep10"})+check("the next episode start clears recording.closed", lc.episode_closed is None)++# a new run_id drops the previous run's messages+if HAVE_LCM_MSGS and mc.HAVE_MSGS:+ lc = mc.LiveCockpit(str(envstub))+ reg("run-1")+ lc.state()+ g = OccupancyGrid(data_length=6, header=hdr("world", 1760000000, 0),+ info=MapMetaData(map_load_time=Time(), resolution=0.5, width=3, height=2,+ origin=pose_of(1.0, 2.0)), data=[-1, 0, 100, 50, 0, -1])+ lc.bus.ingest(mc.CHANNELS["global_costmap"], g.lcm_encode())+ lc.bus.ingest(mc.CHANNELS["explore_cmd"], Bool(data=True).lcm_encode())+ lc.bus.ingest(mc.CHANNELS["goal_request"],+ PoseStamped(header=hdr("world"), pose=pose_of(2.0, 3.5)).lcm_encode())+ a = (lc.map() is not None, lc.state()["nav"]["exploring"], lc.state()["nav"]["goal"])+ reg(None)+ b = lc.map() is not None+ reg("run-2")+ nav = lc.state()["nav"]+ c = (lc.map() is None, nav["exploring"], nav["goal"], lc.state()["topics"]["global_costmap"]["age_s"])+ check("run-1: costmap, exploring, goal (2, 3.5); stack stopped: map kept; run-2: no map, not exploring, "+ "no goal, costmap never seen", a == (True, True, {"x": 2.0, "y": 3.5}) and b+ and c == (True, False, None, None), f"{a} {b} {c}")++ # a camera frame after a run change: the per-camera frame cache must survive the reset+ def jpeg_image(data):+ im = Image(header=hdr("camera"))+ im.width, im.height, im.encoding, im.step = 320, 240, "jpeg", 0+ im.data, im.data_length = data, len(data)+ return im.lcm_encode()++ NOSE_CH, WRIST_CH = mc.CAMERAS["nose"][1], mc.CAMERAS["wrist"][1]+ frame1 = mc.DEMO_JPEG+ frame2 = b"\xff\xd8" + b"run-2 frame" + b"\xff\xd9"+ lc = mc.LiveCockpit(str(envstub))+ reg("run-1")+ lc.state()+ lc.bus.ingest(NOSE_CH, jpeg_image(frame1))+ first = lc.camera_frame("nose")+ reg("run-2")+ lc.state() # the cockpit sees the new run_id and resets its caches+ try:+ before = (lc.camera_frame("nose"), lc.camera_frame("wrist"))+ lc.bus.ingest(NOSE_CH, jpeg_image(frame2))+ lc.bus.ingest(WRIST_CH, jpeg_image(frame1))+ after = (lc.camera_frame("nose"), lc.camera_frame("wrist"))+ err = None+ except Exception as exc: # noqa: BLE001 - the failure this check exists for+ before = after = (None, None)+ err = f"{type(exc).__name__}: {exc}"+ check("camera frame after a run change: run-1 nose frame served; run-2 before any frame -> none on "+ "either camera; run-2 frames -> nose and wrist served byte for byte",+ err is None and first is not None and first[1] == frame1 and before == (None, None)+ and after[0] is not None and after[0][1] == frame2 and after[1] is not None and after[1][1] == frame1,+ err or f"first {first and len(first[1])} B, before {before}, after {[x and len(x[1]) for x in after]}")+ cam_srv = mc.CockpitServer(("127.0.0.1", 0), lc, page_dir)+ threading.Thread(target=cam_srv.serve_forever, daemon=True).start()+ code, ctype, frames = Client(cam_srv.server_address[1]).mjpeg_frames(1)+ check("GET /camera.mjpg after the run change -> 200, the run-2 frame byte for byte",+ code == 200 and bool(frames) and frames[0][1] == frame2, f"{code} {frames if code != 200 else len(frames)}")+ cam_srv.shutdown()+else:+ print(" -- skipped the run-change checks, no dimos_lcm here")+reg(None)+mc._dimos_run_pid = real_pid++# a viewer that stops reading frees its camera slot after the write timeout+real_wto = mc.MJPEG_WRITE_TIMEOUT_S+mc.MJPEG_WRITE_TIMEOUT_S = 0.5+srv, c5 = serve(demo=True, page_dir=page_dir)+big = b"\xff\xd8" + bytes(2_000_000) + b"\xff\xd9"+srv.cockpit.camera_frame = lambda cam="nose": (time.monotonic(), big)+sock = socket.socket()+sock.setsockopt(socket.SOL_SOCKET, socket.SO_RCVBUF, 4096)+sock.connect(("127.0.0.1", srv.server_address[1]))+sock.sendall(b"GET /camera.mjpg HTTP/1.1\r\nHost: x\r\n\r\n") # then never reads again+seen = False+deadline = time.monotonic() + 3+while time.monotonic() < deadline and not seen:+ seen = srv.cockpit.camera_clients == 1+ time.sleep(0.02)+t = time.monotonic()+freed = clients_drop_to_zero(srv.cockpit, within_s=10.0)+check("a viewer that stops reading (socket still open) -> slot freed by the write timeout",+ seen and freed, f"seen {seen}, freed {freed} after {time.monotonic() - t:.1f} s")+sock.close()+srv.shutdown()+mc.MJPEG_WRITE_TIMEOUT_S = real_wto++# ---------------------------------------------------------------------------+print("H. the real page, as shipped")+real_page = ROOT / "tools" / "moss_cockpit"+srv, c6 = serve(demo=True) # the default page dir: tools/moss_cockpit+code, headers, html = c6.get("/")+check("GET / -> tools/moss_cockpit/index.html byte for byte",+ code == 200 and html == (real_page / "index.html").read_bytes() and headers["Content-Type"].startswith("text/html"))+refs = [r.decode() for r in re.findall(rb'(?:src|href)="(/static/[^"]+)"', html)]+refs += ["/static/" + u.decode() for u in re.findall(rb'url\("([^"]+)"\)', (real_page / "tokens.css").read_bytes())]+TYPES = {".svg": "image/svg+xml", ".css": "text/css; charset=utf-8", ".js": "text/javascript; charset=utf-8",+ ".ttf": "font/ttf"}+bad = []+for ref in refs:+ code, headers, body = c6.get(ref)+ local = real_page / ref[len("/static/"):]+ if not (code == 200 and local.is_file() and body == local.read_bytes()+ and headers["Content-Type"] == TYPES.get(os.path.splitext(ref)[1])):+ bad.append(f"{ref} {code}")+check(f"every asset index.html and tokens.css reference is served byte for byte, right type ({len(refs)})",+ len(refs) >= 5 and not bad and {"/static/app.js", "/static/tokens.css", "/static/fonts/SpaceGrotesk.ttf",+ "/static/assets/mark-mint.svg", "/static/assets/wordmark-white.svg"} <= set(refs),+ ", ".join(bad) or ", ".join(refs))+ofl = (real_page / "fonts" / "SpaceGrotesk-OFL.txt").read_text()+check("the font licence ships next to the font (SIL Open Font License 1.1)",+ "SIL OPEN FONT LICENSE Version 1.1" in ofl and (real_page / "fonts" / "SpaceGrotesk.ttf").is_file())+srv.shutdown()+js = (real_page / "app.js").read_text()+strip = re.search(r'\(st\.blueprint \|\| "none"\)\.replace\(/(.+?)/, ""\)', js)+pattern = strip.group(1) if strip else None+got = {bp: re.sub(pattern, "", bp, count=1) for bp in ("moss-dimos.moss-explore", "other-dimos.explore", "moss-nav")} \+ if pattern else {}+check("the run caption strips any dimOS namespace: moss-dimos.moss-explore -> moss-explore, "+ "other-dimos.explore -> explore, moss-nav unchanged",+ got == {"moss-dimos.moss-explore": "moss-explore", "other-dimos.explore": "explore", "moss-nav": "moss-nav"},+ f"{pattern} {got}")++# ---------------------------------------------------------------------------+print("F. hygiene")+src = (ROOT / "tools" / "moss_cockpit.py").read_text()+ctl = (ROOT / "tools" / "explore_ctl.py").read_text()+check("no dimOS / torch / open3d import", not re.search(r"^\s*(import|from)\s+(dimos|torch|open3d)\b", src, re.M))+check("no serial port: no pyserial import, no /dev/tty", not re.search(r"^\s*(import|from)\s+serial\b", src, re.M)+ and "/dev/tty" not in src)+lan = re.compile(r"\b(192\.168|10\.\d{1,3}|172\.(1[6-9]|2\d|3[01]))\.\d{1,3}\.\d{1,3}\b")+check("no LAN address literal in moss_cockpit.py / explore_ctl.py", not lan.search(src) and not lan.search(ctl))+check("no non-ASCII text in moss_cockpit.py (English only)", all(ord(ch) < 128 for ch in src))+check("'dimos' never imported by this bench", "dimos" not in sys.modules)++_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/benches/bench_moss_fly_gates_cold.py b/benches/bench_moss_fly_gates_cold.pynew file mode 100644index 0000000..8147ccd--- /dev/null+++ b/benches/bench_moss_fly_gates_cold.py@@ -0,0 +1,727 @@+#!/usr/bin/env python3+"""Cold bench for tools/moss-fly.sh: each subcommand x the gate the operator gets.++The REAL script runs here, gate logic untouched: FLY_TEST=1 makes it source a+stub file (written below) where dimos, the venv python, pgrep, pkill, nohup,+curl, kill, systemctl, sudo, hostname and sleep become recorders.+/dev/serial/by-id, /proc (for the cockpits' working directories) and the dimOS+state dir (run registry, log root) are temp dirs; the fake `dimos run` writes+the run's main.jsonl with whatever proof lines the case asks for. No robot, no+stack, no port.++ A. check - runs ONLY the flight check (full: lidar spin included),+ saved where the cockpit reads it; its exit code is the result+ B. stack up - a registered run is refused before any preflight or launch,+ nothing killed; a zombie (no registered run) is swept, and+ one that survives kill -9 refuses; one still building+ (unregistered, younger than PROOF_S + 30 s) is refused+ B2. launch lock - a second check/map/explore is refused while one runs,+ stop never is; the lock never reaches the daemon or cockpit+ C. QT Py absent - refused before the preflight; MOCK=1 flies without it;+ the lidar is required either way+ D. map - preflight -> dimos run moss-dimos.moss-nav (env: MOSS_PORT+ = the QT Py by-id even if the shell exported the lidar, no+ ROBOT_PROFILE, DIMOS_TRANSPORT = TRANSPORT) -> proof ->+ cockpit; refusals on a missing proof line, a lidar that+ answers but never scans, a stale run dir, a Traceback, a+ dead cockpit (the stack is stopped each time); MOSS_RECORD+ read as the stack reads it+ D3. cockpits - only this checkout's cockpit counts (its /proc cwd): one+ from another checkout is refused before anything starts,+ never killed; this checkout's, older than its file, is+ restarted (killed, or the unit restarted, and said when+ that fails)+ D2. run dir - read from the dimOS registry (log_dir outside the log root,+ as for a dimOS run from a git clone); an older live entry or+ one for another blueprint never proves this launch+ E. explore - without EXPLORE=1 exploration is never published+ F. EXPLORE=1 - start published only after the stack AND cockpit proofs;+ no explorer line in 15 s -> explore stop, then dimos stop;+ EXPLORE=1 with map is a contradiction, refused untouched+ G. stop - explore_ctl stop BEFORE dimos stop; log/cockpit/usage+ H. bash -n on the script and the stubs+ I. real pgrep - the cockpit pattern matches the interpreter, not a viewer+ J. the units - KillMode=process, proven on a transient user unit; no+ NoNewPrivileges (the page's stacks need sudo or the boot+ unit); the template filled by the install sed names the+ user and the checkout, no placeholder left; the install+ line restarts; tools/lcm-loopback.service sets lo+ multicast, the route and rmem once; both files pass+ systemd-analyze verify (when it is here)++Run: PYTHONDONTWRITEBYTECODE=1 PYTHONPATH=. python3 benches/bench_moss_fly_gates_cold.py+"""++import fcntl+import json+import os+import re+import shutil+import subprocess+import sys+import tempfile+import time+from pathlib import Path++ROOT = Path(__file__).resolve().parent.parent+SCRIPT = ROOT / "tools" / "moss-fly.sh"+PROOF_S = int(re.search(r"^PROOF_S=\$\{PROOF_S:-(\d+)\}", SCRIPT.read_text(), re.M).group(1))+OK = 0+KO = 0++STUBS = r"""# Stand-ins sourced by tools/moss-fly.sh under FLY_TEST=1 (benches/bench_moss_fly_gates_cold.py).+: "${FLY_TEST_LOG:?}"+: "${FLY_TEST_DIR:?}"+: > "$FLY_TEST_LOG"+BYID_DIR="$FLY_TEST_DIR/by-id"+RUNS_DIR="$FLY_TEST_DIR/runs"+LOG_ROOT="$FLY_TEST_DIR/logs"+LAUNCH_LOG="$FLY_TEST_DIR/launch.log"+COCKPIT_LOG="$FLY_TEST_DIR/cockpit.log"+FLY_LOCK="$FLY_TEST_DIR/fly.lock"+DIMOS=fake_dimos+PY=fake_py+FLY_TEST_STACK_UP="${FLY_TEST_STACK_UP:-0}" # 1 = a dimos run process exists+FLY_TEST_REGISTERED="${FLY_TEST_REGISTERED:-1}" # 1 = it has a registered run (live flight)+FLY_TEST_SURVIVES="${FLY_TEST_SURVIVES:-0}" # 1 = the zombie survives kill -9+FLY_TEST_STACK_AGE="${FLY_TEST_STACK_AGE:-600}" # seconds since that process started (ps etimes)+FLY_TEST_LAUNCH="${FLY_TEST_LAUNCH:-ok}" # the daemon launcher: ok | error (a module fails after the proofs) | silent+FLY_TEST_PREFLIGHT_RC="${FLY_TEST_PREFLIGHT_RC:-0}"+FLY_TEST_PROOF="${FLY_TEST_PROOF-MOSS base: connected on X|RPLIDAR C1 up on X|RPLIDAR C1 scanning: 412 points in the first turn|lidar odometry up (kiss-icp)}"+FLY_TEST_RUNDIR="${FLY_TEST_RUNDIR:-1}" # 0 = the launch never creates a run dir+FLY_TEST_REGISTRY="${FLY_TEST_REGISTRY:-0}" # 1 = logs outside LOG_ROOT, found through a registry entry+FLY_TEST_COCKPIT_UP="${FLY_TEST_COCKPIT_UP:-0}" # 1 = a cockpit already answers+FLY_TEST_COCKPIT_DEAD="${FLY_TEST_COCKPIT_DEAD:-0}" # 1 = a started cockpit never answers+FLY_TEST_EXPLORE_PROOF="${FLY_TEST_EXPLORE_PROOF:-1}"+FLY_TEST_UNIT_ACTIVE="${FLY_TEST_UNIT_ACTIVE:-0}" # 1 = moss-cockpit.service is active+FLY_TEST_SUDO_OK="${FLY_TEST_SUDO_OK:-1}" # 0 = sudo -n refuses (no passwordless rule)+FLY_TEST_RESTART_OK="${FLY_TEST_RESTART_OK:-1}" # 0 = systemctl restart is refused+PROC_DIR="$FLY_TEST_DIR/proc" # <pid>/cwd -> the cockpit's checkout++_rec() { printf '%s' "$*" | tr '\n' ' ' >> "$FLY_TEST_LOG"; printf '\n' >> "$FLY_TEST_LOG"; }+_fd9() { if [ -e /proc/self/fd/9 ]; then echo open; else echo closed; fi; }++fake_dimos() {+ case " $* " in+ *" run "*)+ _rec "dimos $* | ENV $(env | grep -E '^(ROBOT_PROFILE|MOSS_[A-Z_]+|DIMOS_TRANSPORT|TRANSPORT|GLIBC_TUNABLES)=' | sort | tr '\n' ' ')"+ local bp="" run_id root="$LOG_ROOT"+ for a in "$@"; do case "$a" in moss-dimos.*) bp="${a#moss-dimos.}" ;; esac; done+ run_id="$(date +%Y%m%d-%H%M%S)-moss-dimos-$bp"+ [ "$FLY_TEST_REGISTRY" = "1" ] && root="$FLY_TEST_DIR/clone_logs"+ if [ "$FLY_TEST_RUNDIR" = "1" ]; then+ local d="$root/$run_id"+ mkdir -p "$d"+ printf '%s\n' "$FLY_TEST_PROOF" | tr '|' '\n' | while IFS= read -r ev; do+ [ -n "$ev" ] && printf '{"event": "%s", "level": "info"}\n' "$ev" >> "$d/main.jsonl"+ done+ touch "$d/main.jsonl"+ # as dimOS: the registry entry is written once the build passed, with+ # the daemon's pid (the launcher's own pid here: alive while it runs)+ if [ "$FLY_TEST_REGISTRY" = "1" ]; then+ mkdir -p "$RUNS_DIR"+ printf '{\n "run_id": "%s",\n "pid": %s,\n "blueprint": "moss-dimos.%s",\n "started_at": "%s",\n "log_dir": "%s"\n}\n' \+ "$run_id" "$$" "$bp" "$(date -u +%Y-%m-%dT%H:%M:%S+00:00)" "$d" > "$RUNS_DIR/$run_id.json"+ fi+ fi+ _rec "dimos-run fd9=$(_fd9)"+ # as dimOS: the launcher reports the build's outcome after the modules logged+ case "$FLY_TEST_LAUNCH" in+ ok) command sleep 0.1; echo "✓ DimOS running in background" ;;+ error) command sleep 0.3; echo "Error: RuntimeError: Failed to start camera pipeline" ;;+ esac ;;+ *" status "*)+ _rec "dimos status"+ if [ "$FLY_TEST_REGISTERED" = "1" ]; then echo "Run ID: 20260101-000000-moss-dimos-moss-nav"; else echo "No running DimOS instance"; fi ;;+ *" stop "*) _rec "dimos stop"; echo "stopped" ;;+ *" log "*)+ _rec "dimos $*"+ # a 300-line run log: the two startup lines first, then telemetry chatter;+ # like dimOS, `log` prints its last 50 lines unless -n says otherwise+ local n=50 prev=""+ for a in "$@"; do [ "$prev" = "-n" ] && n="$a"; prev="$a"; done+ { echo "MOSS base: connected on X"; echo "RPLIDAR C1 up on X"+ for i in $(seq 1 298); do echo "MOSS telemetry $i"; done; } | command tail -n "$n" ;;+ *) _rec "dimos $*" ;;+ esac+ return 0+}++fake_py() {+ _rec "py $*"+ case "$1" in+ tools/moss_preflight.py) _rec "preflight fd9=$(_fd9)"; echo "N/M OK (stub)"; return "$FLY_TEST_PREFLIGHT_RC" ;;+ tools/explore_ctl.py)+ echo "published /${2}_cmd on lcm"+ if [ "$2" = "start" ] && [ "$FLY_TEST_EXPLORE_PROOF" = "1" ]; then+ local d; d=$(ls -td "$LOG_ROOT"/*/ 2>/dev/null | head -1)+ [ -n "$d" ] && echo '{"event": "costmap: explore_cmd received - map writing OPEN for the mission"}' >> "$d/main.jsonl"+ fi ;;+ tools/moss_cockpit.py)+ _rec "cockpit-env EXPLORE=${EXPLORE-unset} MOCK=${MOCK-unset} MOSS_MOCK_BUS=${MOSS_MOCK_BUS-unset} fd9=$(_fd9)"+ : > "$FLY_TEST_DIR/cockpit_started" ;;+ esac+ return 0+}++pgrep() {+ _rec "pgrep $*"+ case "$*" in+ *"[b]in/dimos"*)+ [ "$FLY_TEST_STACK_UP" = "1" ] || return 1+ [ -e "$FLY_TEST_DIR/killed" ] && [ "$FLY_TEST_SURVIVES" != "1" ] && return 1+ echo 4242; return 0 ;;+ *"[t]ools/moss_cockpit"*)+ # the cockpits are the entries of the fake /proc (minus the ones killed)+ local d n=0+ for d in "$PROC_DIR"/*/; do+ [ -d "$d" ] || continue+ [ -e "$FLY_TEST_DIR/killed_$(basename "$d")" ] && continue+ basename "$d"; n=$((n + 1))+ done+ [ "$n" -gt 0 ]; return ;;+ esac+ return 1+}+pkill() { _rec "pkill $*"; : > "$FLY_TEST_DIR/killed"; return 0; }+kill() { _rec "kill $*"; local a; for a in "$@"; do : > "$FLY_TEST_DIR/killed_$a"; done; return 0; }+systemctl() {+ _rec "systemctl $*"+ case "$*" in+ *is-active*) [ "$FLY_TEST_UNIT_ACTIVE" = "1" ] ;;+ *restart*) [ "$FLY_TEST_RESTART_OK" = "1" ] ;;+ *) return 0 ;;+ esac+}+sudo() { _rec "sudo $*"; [ "$FLY_TEST_SUDO_OK" = "1" ] || return 1; [ "$1" = "-n" ] && shift; "$@"; }+ps() { _rec "ps $*"; echo " $FLY_TEST_STACK_AGE"; }+nohup() { _rec "nohup $*"; "$@"; }+curl() {+ _rec "curl $*"+ [ "$FLY_TEST_COCKPIT_DEAD" = "1" ] && return 7+ [ "$FLY_TEST_COCKPIT_UP" = "1" ] || [ -e "$FLY_TEST_DIR/cockpit_started" ]+}+hostname() { echo "rover-test-address"; }+sleep() { command sleep 0.02; }+"""++TMP = tempfile.TemporaryDirectory()+TMPD = Path(TMP.name)+STUB_FILE = TMPD / "moss_fly_stubs.sh"+STUB_FILE.write_text(STUBS)+QTPY_NAME = "usb-Adafruit_QT_Py_ESP32-S3_No_PSRAM_00:00:00:00:00:00-if00"+C1_NAME = "usb-Silicon_Labs_CP2102N_USB_to_UART_Bridge_Controller_0000-if00-port0"+++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+++def fly(args=(), ports=("qtpy", "c1"), stale_run=None, locked=False, registry=(), cockpits=(), **env):+ """Run the real script cold. Returns (rc, what the operator saw, recorded commands, the case dir).+ registry: entries written before the launch, each (run_id, pid, blueprint, events, mtime);+ their log dirs live outside the log root, as for a dimOS run from a git clone.+ cockpits: running cockpits, each (pid, working directory, start time as an epoch)."""+ case = TMPD / f"case_{len(list(TMPD.glob('case_*')))}"+ (case / "by-id").mkdir(parents=True)+ (case / "logs").mkdir()+ (case / "runs").mkdir()+ (case / "proc").mkdir()+ for pid, cwd, started in cockpits:+ (case / "proc" / str(pid)).mkdir()+ (case / "proc" / str(pid) / "cwd").symlink_to(cwd)+ os.utime(case / "proc" / str(pid), (started, started))+ for p in ports:+ (case / "by-id" / (QTPY_NAME if p == "qtpy" else C1_NAME)).write_text("")+ if stale_run:+ d = case / "logs" / "20200101-000000-moss-dimos-moss-nav"+ d.mkdir()+ (d / "main.jsonl").write_text("".join(f'{{"event": "{e}"}}\n' for e in stale_run))+ os.utime(d, (1_577_836_800, 1_577_836_800))+ for run_id, pid, blueprint, events, mtime in registry:+ d = case / "clone_logs" / run_id+ d.mkdir(parents=True)+ (d / "main.jsonl").write_text("".join(f'{{"event": "{e}"}}\n' for e in events))+ entry = case / "runs" / f"{run_id}.json"+ entry.write_text(json.dumps({"run_id": run_id, "pid": pid, "blueprint": blueprint,+ "started_at": "2020-01-01T00:00:00+00:00", "log_dir": str(d)}, indent=2))+ os.utime(d, (mtime, mtime))+ os.utime(entry, (mtime, mtime))+ log = case / "rec.log"+ e = {k: v for k, v in os.environ.items()+ if not k.startswith(("MOSS_", "FLY_TEST"))+ and k not in ("EXPLORE", "MOCK", "ROBOT_PROFILE", "TRANSPORT", "DIMOS_TRANSPORT")}+ e.update(FLY_TEST="1", FLY_TEST_STUBS=str(STUB_FILE), FLY_TEST_LOG=str(log), FLY_TEST_DIR=str(case),+ MOSS_STATE_DIR=str(case / "state"))+ e.update({k: str(v) for k, v in env.items()})+ with open(case / "fly.lock", "a") as lk:+ if locked: # another moss-fly.sh holds the launch lock+ fcntl.flock(lk, fcntl.LOCK_EX | fcntl.LOCK_NB)+ p = subprocess.run(["bash", str(SCRIPT), *args], env=e, capture_output=True, text=True, timeout=120)+ if "unbound variable" in p.stderr or "syntax error" in p.stderr:+ SHELL_ERRORS.append(f"{' '.join(args)} {sorted(env)}: {p.stderr.strip().splitlines()[-1][:100]}")+ return p.returncode, p.stdout + p.stderr, (log.read_text() if log.exists() else ""), case+++SHELL_ERRORS = [] # bash errors seen in any case (set -u: an unset variable is a bug, not a branch)+++def ran(log, needle):+ return [ln for ln in log.splitlines() if needle in ln]+++def first(log, needle):+ for i, ln in enumerate(log.splitlines()):+ if needle in ln:+ return i+ return -1+++GOOD_PROOF = "MOSS base: connected on X|RPLIDAR C1 up on X|RPLIDAR C1 scanning: 412 points in the first turn|lidar odometry up (kiss-icp)"++print("A. check = the flight check only")+rc, out, log, case = fly(["check"])+pre = ran(log, "py tools/moss_preflight.py")+check("exit 0 when the check is green", rc == 0, f"rc {rc}")+check("the preflight ran once, FULL (no --quick, no --no-spin)",+ len(pre) == 1 and "--quick" not in pre[0] and "--no-spin" not in pre[0], pre[0] if pre else "none")+check("saved where the cockpit reads it (<state>/preflight.json)",+ bool(pre) and f"--save {case / 'state' / 'preflight.json'}" in pre[0])+check("nothing launched: no dimos run, no nohup, no curl",+ not ran(log, "dimos --rerun-open") and not ran(log, "nohup") and not ran(log, "curl"))+rc, out, log, case = fly(["check"], FLY_TEST_PREFLIGHT_RC=1)+check("a KO check -> exit 1", rc == 1, f"rc {rc}")++print("B. a stack already up")+rc, out, log, case = fly(["map"], FLY_TEST_STACK_UP=1)+check("refused (rc 1) with the fix", rc == 1 and "A DIMOS STACK IS ALREADY RUNNING - tools/moss-fly.sh stop first" in out, f"rc {rc}")+check("no preflight, no launch, no cockpit",+ not ran(log, "moss_preflight") and not ran(log, "--daemon") and not ran(log, "curl"))+check("the live flight is not killed", not ran(log, "pkill") and not ran(log, "kill "))+check("asked dimos status before refusing", bool(ran(log, "dimos status")))+check("gate 0 matches `dimos <options> run`, not `bin/dimos run` only",+ any("[b]in/dimos( [^ ]+)* run( |$)" in ln for ln in ran(log, "pgrep")), ran(log, "pgrep")[0] if ran(log, "pgrep") else "")+rc, out, log, case = fly(["map"], FLY_TEST_STACK_UP=1, FLY_TEST_REGISTERED=0)+check("zombie (no registered run): swept with kill -9, then the flight goes on",+ rc == 0 and "zombie" in out and any("-9" in ln for ln in ran(log, "pkill")) and bool(ran(log, "--daemon")), f"rc {rc}")+check("... the sweep comes before the preflight", first(log, "pkill") < first(log, "moss_preflight"))+check("... the zombie's age was read first (ps etimes)", 0 <= first(log, "ps -o etimes=") < first(log, "pkill"))+rc, out, log, case = fly(["map"], FLY_TEST_STACK_UP=1, FLY_TEST_REGISTERED=0, FLY_TEST_STACK_AGE=12)+check("a stack still building (unregistered, 12 s old) -> refused as running, nothing killed",+ rc == 1 and "A DIMOS STACK IS STILL STARTING (pid 4242, 12 s old" in out and not ran(log, "pkill")+ and not ran(log, "moss_preflight") and not ran(log, "--daemon"), f"rc {rc}")+rc, out, log, case = fly(["map"], FLY_TEST_STACK_UP=1, FLY_TEST_REGISTERED=0, FLY_TEST_STACK_AGE=PROOF_S + 29)+check(f"... {PROOF_S + 29} s old (inside PROOF_S {PROOF_S} + 30 s) -> still starting, refused, nothing killed",+ rc == 1 and f"A DIMOS STACK IS STILL STARTING (pid 4242, {PROOF_S + 29} s old" in out and not ran(log, "pkill"),+ f"rc {rc}")+rc, out, log, case = fly(["map"], FLY_TEST_STACK_UP=1, FLY_TEST_REGISTERED=0, FLY_TEST_STACK_AGE=PROOF_S + 31)+check(f"... {PROOF_S + 31} s old (past PROOF_S {PROOF_S} + 30 s) with no registered run -> a zombie, swept",+ rc == 0 and any("-9" in ln for ln in ran(log, "pkill")), f"rc {rc}")+rc, out, log, case = fly(["map"], FLY_TEST_STACK_UP=1, FLY_TEST_REGISTERED=0, FLY_TEST_SURVIVES=1)+check("a zombie that survives kill -9 -> refused, no launch",+ rc == 1 and "THE ZOMBIE SURVIVED" in out and not ran(log, "--daemon"), f"rc {rc}")++print("B2. one launcher at a time (the launch lock)")+rc, out, log, case = fly(["map"], locked=True, FLY_TEST_STACK_UP=1, FLY_TEST_REGISTERED=0, FLY_TEST_STACK_AGE=600)+check("lock held by another launcher -> map refused with the fix, nothing ran (no sweep)",+ rc == 1 and "ANOTHER moss-fly.sh IS RUNNING" in out and log.strip() == "", f"rc {rc} {log[:60]}")+rc, out, log, case = fly(["explore"], locked=True)+check("... explore refused the same way", rc == 1 and "ANOTHER moss-fly.sh IS RUNNING" in out and log.strip() == "")+rc, out, log, case = fly(["check"], locked=True)+check("... check refused too (it opens the board)", rc == 1 and "ANOTHER moss-fly.sh IS RUNNING" in out+ and not ran(log, "moss_preflight"))+rc, out, log, case = fly(["stop"], locked=True)+check("... stop is never locked out", rc == 0 and bool(ran(log, "dimos stop")))+rc, out, log, case = fly(["map"])+check("the lock is held while the gates run (the preflight sees fd 9)", ran(log, "preflight fd9=") == ["preflight fd9=open"],+ str(ran(log, "preflight fd9=")))+check("... and NOT passed on to the dimOS daemon or the cockpit (they outlive the launcher)",+ ran(log, "dimos-run fd9=") == ["dimos-run fd9=closed"] and ran(log, "fd9=closed") == ["dimos-run fd9=closed",+ "cockpit-env EXPLORE=unset MOCK=unset MOSS_MOCK_BUS=unset fd9=closed"], str(ran(log, "fd9=")))+with open(case / "fly.lock", "a") as lk:+ try:+ fcntl.flock(lk, fcntl.LOCK_EX | fcntl.LOCK_NB)+ check("... released when the launcher exits", True)+ except OSError:+ check("... released when the launcher exits", False)++print("C. QT Py missing")+rc, out, log, case = fly(["map"], ports=("c1",))+check("refused (rc 1) with the fix", rc == 1 and "NO QT PY on USB" in out, f"rc {rc}")+check("before the preflight and the launch", not ran(log, "moss_preflight") and not ran(log, "--daemon"))+rc, out, log, case = fly(["map"], ports=("c1",), MOCK=1)+run_l = ran(log, "| ENV")+check("MOCK=1 flies without the board, MOSS_MOCK_BUS=1 in the stack's env",+ rc == 0 and len(run_l) == 1 and "MOSS_MOCK_BUS=1" in run_l[0], f"rc {rc}")+check("MOCK=1 with no board: MOSS_PORT not exported at all", bool(run_l) and "MOSS_PORT=" not in run_l[0])+rc, out, log, case = fly(["map"], ports=("qtpy",))+check("lidar missing -> refused, no launch", rc == 1 and "NO LIDAR on USB" in out and not ran(log, "--daemon"))+rc, out, log, case = fly(["map"], FLY_TEST_PREFLIGHT_RC=1)+check("preflight KO -> refused, no launch", rc == 1 and "FLIGHT CHECK KO" in out and not ran(log, "--daemon"))++print("D. map: the happy path and its refusals")+rc, out, log, case = fly(["map"], MOSS_PORT=str(TMPD / "by-id-lidar-alias"))+qtpy = str(case / "by-id" / QTPY_NAME)+c1 = str(case / "by-id" / C1_NAME)+run_l = ran(log, "| ENV")+check("rc 0 and READY", rc == 0 and "== READY: moss-dimos.moss-nav ==" in out, f"rc {rc}")+order = [first(log, "py tools/moss_preflight.py"), first(log, " run moss-dimos.moss-nav"), first(log, "curl")]+check("order: preflight -> dimos run -> cockpit curl", -1 not in order and order == sorted(order), str(order))+check("gate 1 runs the preflight with --no-spin (battery + disarmed read) and saves it",+ any("--no-spin" in ln and "--save" in ln for ln in ran(log, "py tools/moss_preflight.py")))+check("one launch: --rerun-open none --nerf-speed 0.5 run moss-dimos.moss-nav --daemon",+ len(run_l) == 1 and "--rerun-open none --nerf-speed 0.5 run moss-dimos.moss-nav --daemon" in run_l[0],+ run_l[0][:140] if run_l else "none")+env_l = run_l[0].split("| ENV ", 1)[1] if run_l and "| ENV " in run_l[0] else ""+for want in (f"MOSS_PORT={qtpy}", f"MOSS_LIDAR_PORT={c1}", "DIMOS_TRANSPORT=lcm",+ "TRANSPORT=lcm", "GLIBC_TUNABLES=glibc.rtld.optional_static_tls=4194304", "MOSS_MAX_PCT=100",+ "MOSS_RAMP_MS=5 5", "MOSS_RECORD=1", "MOSS_CAMERA=1", f"MOSS_STATE_DIR={case / 'state'}"):+ check(f"stack env: {want.split('=')[0]}={want.split('=', 1)[1][-40:]}",+ re.search(r"(^| )" + re.escape(want) + r"( |$)", env_l) is not None, env_l[-120:])+check("the launcher does not export ROBOT_PROFILE (MOSS is the only profile)",+ bool(env_l) and "ROBOT_PROFILE=" not in env_l, env_l[:80])+check("the proof was read in this launch's run dir under the log root",+ "proven: base up, lidar scanning, lidar odometry up, build done, in " in out and "-moss-dimos-moss-nav" in out)+check("MOSS_RECORD=1 -> 'recording into' the state dir", f"recording into {case / 'state'}/recordings/" in out)+check("the shell's MOSS_PORT was overridden by the by-id path, and said so",+ "ignoring MOSS_PORT=" in out and f"MOSS_PORT={qtpy}" in env_l)+check("cockpit started on 8904 when absent", any("tools/moss_cockpit.py --port 8904" in ln for ln in ran(log, "nohup")))+check("cockpit probed on 127.0.0.1:8904/state.json",+ any("http://127.0.0.1:8904/state.json" in ln for ln in ran(log, "curl")))+check("prints the cockpit URL and the stop line",+ "cockpit: http://rover-test-address:8904/" in out and "stop: tools/moss-fly.sh stop" in out)+check("map never touches exploration", not ran(log, "explore_ctl"))+rc, out, log, case = fly(["map"], FLY_TEST_COCKPIT_UP=1)+check("cockpit already answering -> not started twice", rc == 0 and not ran(log, "tools/moss_cockpit.py --port"))+rc, out, log, case = fly(["map"], MOSS_NERF_SPEED="0.3", MOSS_MAX_PCT="60", TRANSPORT="zenoh")+run_l = ran(log, "| ENV")+check("MOSS_NERF_SPEED / MOSS_MAX_PCT / TRANSPORT pass through, the bus under both names",+ bool(run_l) and "--nerf-speed 0.3" in run_l[0] and "MOSS_MAX_PCT=60 " in run_l[0]+ and " TRANSPORT=zenoh " in run_l[0] and "DIMOS_TRANSPORT=zenoh " in run_l[0], run_l[0][-160:] if run_l else "")+rc, out, log, case = fly(["map"], DIMOS_TRANSPORT="zenoh", TRANSPORT="lcm")+run_l = ran(log, "| ENV")+check("DIMOS_TRANSPORT wins over TRANSPORT, as in dimOS: both exported as zenoh",+ bool(run_l) and " TRANSPORT=zenoh " in run_l[0] and "DIMOS_TRANSPORT=zenoh " in run_l[0], run_l[0][-160:] if run_l else "")+for val in ("0", "off", "No"):+ rc, out, log, case = fly(["map"], MOSS_RECORD=val)+ check(f"MOSS_RECORD={val} -> 'not recording', never 'recording into'",+ rc == 0 and "recording into" not in out and f"not recording (MOSS_RECORD={val})" in out, f"rc {rc}")+rc, out, log, case = fly(["map"], FLY_TEST_PROOF="MOSS base: connected on X|lidar odometry up (kiss-icp)")+check("no lidar line at all in PROOF_S -> refused naming the lidar",+ rc == 1 and "STACK NOT PROVEN" in out and "lidar never up" in out, f"rc {rc}")+check("... the stack is stopped and the cockpit never probed", bool(ran(log, "dimos stop")) and not ran(log, "curl"))+rc, out, log, case = fly(["map"], FLY_TEST_PROOF="MOSS base: connected on X|RPLIDAR C1 up on X|lidar odometry up (kiss-icp)")+check("'RPLIDAR C1 up' without 'RPLIDAR C1 scanning' (answers, never scans) -> refused, never READY",+ rc == 1 and "lidar answers but never sent a scan" in out and "READY" not in out and bool(ran(log, "dimos stop")),+ f"rc {rc}")+rc, out, log, case = fly(["map"], FLY_TEST_PROOF="MOSS base: connected on X|RPLIDAR C1 up on X|"+ "RPLIDAR C1 unavailable on X (RuntimeError: scan stream timed out) - retrying every 2 s|"+ "lidar odometry up (kiss-icp)")+check("'RPLIDAR C1 unavailable' and no scan -> refused, the unavailable line named",+ rc == 1 and "lidar unavailable, never scanning" in out and "READY" not in out, f"rc {rc}")+rc, out, log, case = fly(["map"], FLY_TEST_RUNDIR=0,+ stale_run=["MOSS base: connected on X", "RPLIDAR C1 up on X",+ "RPLIDAR C1 scanning: 412 points in the first turn", "lidar odometry up (kiss-icp)"])+check("an OLD run dir full of proof lines does not prove this launch",+ rc == 1 and "STACK NOT PROVEN" in out and "no new run dir" in out, f"rc {rc}")+rc, out, log, case = fly(["map"], FLY_TEST_PROOF="MOSS base: cannot open /dev/serial/by-id/x|Traceback (most recent call last)")+check("a Traceback / cannot open stops the wait early and refuses",+ rc == 1 and "saw:" in out and bool(ran(log, "dimos stop")), f"rc {rc}")+rc, out, log, case = fly(["map"], FLY_TEST_PROOF="MOSS base: MOCK BUS (MOSS_MOCK_BUS set)|RPLIDAR C1 up on X|"+ "RPLIDAR C1 scanning: 380 points in the first turn|lidar odometry up", MOCK=1)+check("the MOCK BUS line proves the base in MOCK", rc == 0, f"rc {rc}")+rc, out, log, case = fly(["map"], FLY_TEST_LAUNCH="error")+check("proofs logged, then the launcher says 'Error:' (a module failed later) -> refused, never READY",+ rc == 1 and "STACK NOT PROVEN" in out and "saw: Error: RuntimeError: Failed to start camera pipeline" in out+ and "READY" not in out and not ran(log, "curl") and bool(ran(log, "dimos stop")), f"rc {rc}")+rc, out, log, case = fly(["map"], FLY_TEST_LAUNCH="silent")+check("proofs logged but the launcher never reports success in PROOF_S -> refused naming it",+ rc == 1 and "never said 'DimOS running in background'" in out and "READY" not in out, f"rc {rc}")+rc, out, log, case = fly(["map"], FLY_TEST_COCKPIT_DEAD=1)+check("dead cockpit -> refused and the stack stopped",+ rc == 1 and "NO COCKPIT" in out and bool(ran(log, "dimos stop")), f"rc {rc}")++rc, out, log, case = fly(["explore"], EXPLORE=1, MOCK=1, ports=("c1",))+ck = ran(log, "cockpit-env")+check("a cockpit started by EXPLORE=1 MOCK=1 explore gets none of the run's switches",+ rc == 0 and ck == ["cockpit-env EXPLORE=unset MOCK=unset MOSS_MOCK_BUS=unset fd9=closed"], str(ck))+check("... while the stack itself still runs on the fake bus", any("MOSS_MOCK_BUS=1 " in ln for ln in ran(log, "| ENV")))++print("D3. only this checkout's cockpit counts")+HERE = ROOT.resolve()+OTHER = TMPD / "other_checkout"+OTHER.mkdir()+FILE_T = (ROOT / "tools" / "moss_cockpit.py").stat().st_mtime+rc, out, log, case = fly(["map"], cockpits=[(5151, OTHER, FILE_T + 60)])+check("a cockpit from another checkout -> map refused before the preflight, its cwd named, with the fix",+ rc == 1 and "A COCKPIT FROM ANOTHER CHECKOUT is running (pid 5151, in " in out and str(OTHER) in out+ and "sudo systemctl restart moss-cockpit" in out and not ran(log, "moss_preflight") and not ran(log, "--daemon"),+ f"rc {rc}")+check("... and it is never killed, nor a second cockpit started",+ not ran(log, "kill ") and not ran(log, "moss_cockpit.py --port"))+rc, out, log, case = fly(["cockpit"], cockpits=[(5151, OTHER, FILE_T + 60)], FLY_TEST_COCKPIT_UP=1)+check("`cockpit` with another checkout's cockpit answering -> rc 1, refused (not 'cockpit: <url>'), untouched",+ rc == 1 and "ANOTHER CHECKOUT" in out and "cockpit: http" not in out and not ran(log, "kill ")+ and not ran(log, "moss_cockpit.py --port"), f"rc {rc}")+rc, out, log, case = fly(["map"], cockpits=[(5151, HERE, FILE_T + 60), (5152, OTHER, FILE_T + 60)])+check("this checkout's cockpit plus another one -> still refused", rc == 1 and "pid 5152" in out, f"rc {rc}")+rc, out, log, case = fly(["map"], cockpits=[(5151, HERE, FILE_T + 60)], FLY_TEST_COCKPIT_UP=1)+check("this checkout's cockpit, newer than its file -> used as is: READY, not killed, not restarted",+ rc == 0 and "== READY" in out and not ran(log, "kill ") and not ran(log, "moss_cockpit.py --port")+ and "restarting" not in out, f"rc {rc}")+rc, out, log, case = fly(["cockpit"], cockpits=[(5151, HERE, FILE_T - 600)])+check("this checkout's cockpit, older than its file -> killed and started again",+ rc == 0 and "cockpit pid 5151 is older than tools/moss_cockpit.py - restarting it" in out+ and ran(log, "kill 5151") and bool(ran(log, "moss_cockpit.py --port 8904")), f"rc {rc}")+rc, out, log, case = fly(["cockpit"], cockpits=[(5151, HERE, FILE_T - 600)], FLY_TEST_UNIT_ACTIVE=1, FLY_TEST_COCKPIT_UP=1)+check("... under the unit: the unit restarted (sudo -n first), never killed by pid",+ rc == 0 and "restarting the unit" in out and bool(ran(log, "sudo -n systemctl restart moss-cockpit"))+ and not ran(log, "kill ") and "COULD NOT RESTART" not in out, f"rc {rc}")+rc, out, log, case = fly(["cockpit"], cockpits=[(5151, HERE, FILE_T - 600)], FLY_TEST_UNIT_ACTIVE=1,+ FLY_TEST_COCKPIT_UP=1, FLY_TEST_SUDO_OK=0, FLY_TEST_RESTART_OK=0)+check("... and when neither sudo -n nor systemctl may restart it: said, with the command",+ "COULD NOT RESTART moss-cockpit" in out and "sudo systemctl restart moss-cockpit" in out+ and bool(ran(log, "systemctl --no-ask-password restart moss-cockpit")), f"rc {rc}")++print("D2. the run dir from the dimOS registry")+rc, out, log, case = fly(["map"], FLY_TEST_REGISTRY=1)+clone_runs = sorted((case / "clone_logs").glob("*-moss-dimos-moss-nav"))+check("logs outside the log root, registry entry -> proven in that run dir, READY",+ rc == 0 and len(clone_runs) == 1 and f"build done, in {clone_runs[0].name}" in out+ and "== READY: moss-dimos.moss-nav ==" in out,+ f"rc {rc} " + next((ln.strip() for ln in out.splitlines() if "proven:" in ln), "no proven line"))+check("... nothing under the log root was needed", not any((case / "logs").iterdir()))+FULL = ["MOSS base: connected on X", "RPLIDAR C1 up on X", "RPLIDAR C1 scanning: 412 points in the first turn",+ "lidar odometry up (kiss-icp)"]+rc, out, log, case = fly(["map"], FLY_TEST_RUNDIR=0,+ registry=[("20200101-000000-moss-dimos-moss-nav", os.getpid(), "moss-dimos.moss-nav", FULL, 1_577_836_800)])+check("an OLDER registry entry (live pid, full proof lines) does not prove this launch",+ rc == 1 and "STACK NOT PROVEN" in out and "no new run dir" in out, f"rc {rc}")+rc, out, log, case = fly(["map"], FLY_TEST_RUNDIR=0,+ registry=[("29990101-000000-moss-dimos-moss-gamepad", os.getpid(), "moss-dimos.moss-gamepad", FULL,+ time.time() + 5)])+check("a fresh registry entry for ANOTHER blueprint (moss-gamepad) does not prove moss-nav",+ rc == 1 and "STACK NOT PROVEN" in out and "no new run dir" in out, f"rc {rc}")+dead = subprocess.Popen([sys.executable, "-c", "pass"])+dead.wait()+rc, out, log, case = fly(["map"], FLY_TEST_RUNDIR=0,+ registry=[("29990101-000000-moss-dimos-moss-nav", dead.pid, "moss-dimos.moss-nav", FULL, time.time() + 5)])+check("a fresh registry entry whose pid is dead does not prove the launch",+ rc == 1 and "STACK NOT PROVEN" in out and "no new run dir" in out, f"rc {rc}")++print("E. explore without EXPLORE=1: never armed")+rc, out, log, case = fly(["explore"])+run_l = ran(log, "| ENV")+check("rc 0, stack moss-dimos.moss-explore", rc == 0 and bool(run_l) and "run moss-dimos.moss-explore --daemon" in run_l[0], f"rc {rc}")+check("explore_cmd never published", not ran(log, "explore_ctl.py start"))+check("says exploration is NOT armed", "exploration NOT armed" in out)++print("F. EXPLORE=1 explore: published only after the proofs")+rc, out, log, case = fly(["explore"], EXPLORE=1)+start = first(log, "explore_ctl.py start")+check("rc 0 and 4/4 exploration armed", rc == 0 and "== 4/4 exploration ==" in out and "exploration armed" in out, f"rc {rc}")+check("start published exactly once", len(ran(log, "explore_ctl.py start")) == 1)+check("... after the stack launch and the cockpit proof",+ first(log, " run moss-dimos.moss-explore") < start and first(log, "curl") < start)+check("no stop on the happy path", not ran(log, "explore_ctl.py stop") and not ran(log, "dimos stop"))+rc, out, log, case = fly(["explore"], EXPLORE=1, FLY_TEST_EXPLORE_PROOF=0)+check("no explorer line in 15 s -> refused", rc == 1 and "EXPLORATION NOT PROVEN" in out, f"rc {rc}")+check("... explore stop THEN dimos stop",+ 0 <= first(log, "explore_ctl.py stop") < first(log, "dimos stop"), f"{first(log, 'explore_ctl.py stop')} < {first(log, 'dimos stop')}")+rc, out, log, case = fly(["explore"], EXPLORE=1, FLY_TEST_PROOF="MOSS base: connected on X|RPLIDAR C1 up on X")+check("stack not proven -> exploration never published", rc == 1 and not ran(log, "explore_ctl.py start"))+rc, out, log, case = fly(["map"], EXPLORE=1)+check("EXPLORE=1 with map -> contradiction refused, nothing ran",+ rc == 1 and "CONTRADICTORY" in out and log.strip() == "", log[:80])++print("G. stop, log, cockpit, usage")+rc, out, log, case = fly(["stop"])+check("stop: rc 0", rc == 0, f"rc {rc}")+check("explore_ctl stop BEFORE dimos stop", 0 <= first(log, "explore_ctl.py stop") < first(log, "dimos stop"))+check("stop never launches anything", not ran(log, "--daemon") and not ran(log, "moss_preflight"))+rc, out, log, case = fly(["stop"], FLY_TEST_STACK_UP=1, FLY_TEST_SURVIVES=0)+check("stop: still up after dimos stop -> pkill fallback", any("pkill -f" in ln for ln in ran(log, "pkill")))+rc, out, log, case = fly(["log", "5"])+check("log 5: the last 5 filtered lines of a 200-line window",+ rc == 0 and ran(log, "dimos log") == ["dimos log -n 200"]+ and out.splitlines() == [f"MOSS telemetry {i}" for i in range(294, 299)], " / ".join(out.splitlines()[-5:]))+rc, out, log, case = fly(["log", "300"])+lines = out.splitlines()+check("log 300 (300-line run log): reads 1500, shows all 300, startup lines included",+ ran(log, "dimos log") == ["dimos log -n 1500"] and len(lines) == 300+ and lines[:2] == ["MOSS base: connected on X", "RPLIDAR C1 up on X"], f"{len(lines)} lines")+rc, out, log, case = fly(["log"])+check("log: 30 lines by default", rc == 0 and len(out.splitlines()) == 30 and ran(log, "dimos log") == ["dimos log -n 200"])+rc, out, log, case = fly(["log", "x5"])+check("log x5 -> usage, rc 1, dimos never asked", rc == 1 and "usage:" in out and not ran(log, "dimos log"))+rc, out, log, case = fly(["cockpit"])+check("cockpit: starts it and prints the URL",+ rc == 0 and "cockpit: http://rover-test-address:8904/" in out and bool(ran(log, "moss_cockpit.py --port 8904")))+rc, out, log, case = fly(["cockpit"], MOSS_COCKPIT_PORT="8911", FLY_TEST_COCKPIT_DEAD=1)+check("cockpit dead -> rc 1, on the port asked", rc == 1 and "NO COCKPIT on :8911" in out)+rc, out, log, case = fly([])+check("no subcommand -> usage, rc 1, nothing ran", rc == 1 and "usage:" in out and log.strip() == "")+rc, out, log, case = fly(["fly"])+check("unknown subcommand -> usage, rc 1", rc == 1 and "usage:" in out)++print("H. the scripts parse")+check("no bash error (unbound variable, syntax) in any case run above", SHELL_ERRORS == [], "; ".join(SHELL_ERRORS)[:200])+for f in (SCRIPT, STUB_FILE):+ check(f"bash -n {f.name}", subprocess.run(["bash", "-n", str(f)]).returncode == 0)+text = SCRIPT.read_text()+check("the FLY_TEST hook comes after set -u", 0 <= text.find("set -u") < text.find("FLY_TEST_STUBS"))+check("no LAN address literal in the script",+ not any(tok.count(".") == 3 and all(p.isdigit() for p in tok.split(".")) and not tok.startswith("127.")+ for tok in text.replace(":", " ").replace("/", " ").split()))++print("I. the cockpit is found by its interpreter, not by its file name (real pgrep)")+cockpit_re = subprocess.run(["bash", "-c", f'eval "$(grep -m1 "^COCKPIT_RE=" {SCRIPT})"; printf %s "$COCKPIT_RE"'],+ capture_output=True, text=True).stdout+fake_repo = TMPD / "fake_repo"+(fake_repo / "tools").mkdir(parents=True)+(fake_repo / "tools" / "moss_cockpit.py").write_text("import time\nprint('ready', flush=True)\ntime.sleep(30)\n")+procs = {}+for name, argv0 in (("cockpit", "/home/x/moss/.venv/bin/python"), ("viewer", "less"), ("editor", "vim"),+ ("ssh line", "ssh")):+ procs[name] = subprocess.Popen(["bash", "-c", f'exec -a "{argv0}" "{sys.executable}" tools/moss_cockpit.py --port 8904'],+ cwd=fake_repo, stdout=subprocess.PIPE, text=True)+procs["shell line"] = subprocess.Popen(["bash", "-c", f'cd {fake_repo} && .venv/bin/python tools/moss_cockpit.py; '+ f'"{sys.executable}" -c "import time; print(1, flush=True); time.sleep(30)"'],+ stdout=subprocess.PIPE, stderr=subprocess.DEVNULL, text=True)+try:+ for pr in procs.values():+ pr.stdout.readline()+ hits = {int(x) for x in subprocess.run(["pgrep", "-f", cockpit_re], capture_output=True, text=True).stdout.split()}+ check("COCKPIT_RE read from the script", cockpit_re.startswith("^") and "moss_cockpit" in cockpit_re, cockpit_re)+ check("`.venv/bin/python tools/moss_cockpit.py --port 8904` IS the cockpit", procs["cockpit"].pid in hits)+ check("`less` / `vim` / `ssh` / a `bash -c` line naming the file are NOT",+ not hits & {procs[k].pid for k in ("viewer", "editor", "ssh line", "shell line")}, str(hits))+finally:+ for pr in procs.values():+ pr.kill()+ pr.wait()++print("J. moss-cockpit.service: a cockpit restart never ends a flight it started")+svc = ROOT / "tools" / "moss-cockpit.service"+section, killmode = "", "control-group"+for ln in svc.read_text().splitlines():+ ln = ln.strip()+ if ln.startswith("["):+ section = ln+ elif section == "[Service]" and ln.startswith("KillMode="):+ killmode = ln.split("=", 1)[1]+check("[Service] KillMode=process", killmode == "process", killmode)+unit_lines = [ln for ln in svc.read_text().splitlines() if not ln.lstrip().startswith("#")]+check("no NoNewPrivileges (a stack launched from the page must be able to sudo, or find the boot unit's settings)",+ not any(ln.strip().startswith("NoNewPrivileges") for ln in unit_lines))+check("ordered after tools/lcm-loopback.service, which it wants",+ "Wants=lcm-loopback.service" in unit_lines and any(ln.startswith("After=") and "lcm-loopback.service" in ln+ for ln in unit_lines))+header = [ln for ln in svc.read_text().splitlines() if ln.startswith("#")]+check("the install line restarts the unit (enable --now does not replace a running cockpit)",+ any("sudo systemctl daemon-reload && sudo systemctl enable moss-cockpit && sudo systemctl restart moss-cockpit" in ln+ for ln in header) and not any("enable --now moss-cockpit" in ln for ln in header))+check("the unit is a template: User=MOSS_USER, the checkout as MOSS_DIR, no home path",+ "User=MOSS_USER" in unit_lines and "WorkingDirectory=MOSS_DIR" in unit_lines+ and not any("/home/" in ln for ln in unit_lines), " | ".join(ln for ln in unit_lines if "=" in ln)[:160])+filled = subprocess.run(["sed", "-e", "s|MOSS_USER|alice|g", "-e", "s|MOSS_DIR|/srv/moss|g", str(svc)],+ capture_output=True, text=True).stdout+fl = [ln for ln in filled.splitlines() if not ln.lstrip().startswith("#")]+check("filled by the install sed: User=alice, WorkingDirectory=/srv/moss, the checkout's venv runs the cockpit",+ "User=alice" in fl and "WorkingDirectory=/srv/moss" in fl+ and "ExecStart=/srv/moss/.venv/bin/python tools/moss_cockpit.py" in fl, " | ".join(fl)[:200])+check("... and no placeholder is left", "MOSS_USER" not in "\n".join(fl) and "MOSS_DIR" not in "\n".join(fl))++lb = ROOT / "tools" / "lcm-loopback.service"+lb_lines = [ln.strip() for ln in lb.read_text().splitlines() if ln.strip() and not ln.lstrip().startswith("#")]+check("lcm-loopback.service: a oneshot that stays active after it ran",+ "Type=oneshot" in lb_lines and "RemainAfterExit=yes" in lb_lines, " | ".join(lb_lines)[:160])+check("... lo multicast on, 224.0.0.0/4 on lo (idempotent), both buffers to 67108864, before the cockpit",+ "ExecStart=/usr/sbin/ip link set lo multicast on" in lb_lines+ and "ExecStart=/usr/sbin/ip route replace 224.0.0.0/4 dev lo" in lb_lines+ and "ExecStart=/usr/sbin/sysctl -w net.core.rmem_max=67108864 net.core.rmem_default=67108864" in lb_lines+ and "Before=moss-cockpit.service" in lb_lines and "WantedBy=multi-user.target" in lb_lines)+check("... the buffer size is the one the flight check asks for (RMEM_TARGET)",+ "RMEM_TARGET = 67_108_864" in (ROOT / "tools" / "moss_preflight.py").read_text())+++def unit_keys_parse(text):+ """Strict KEY=VALUE parse of a unit: every line is blank, a comment, a [Section] or Key=value."""+ bad = [ln for ln in text.splitlines()+ if ln.strip() and not ln.lstrip().startswith(("#", ";"))+ and not re.fullmatch(r"\[[A-Za-z]+\]", ln.strip()) and not re.fullmatch(r"[A-Za-z][A-Za-z0-9]*=.*", ln.strip())]+ return bad+++check("strict key=value parse of both unit files", unit_keys_parse(filled) == [] and unit_keys_parse(lb.read_text()) == [],+ str(unit_keys_parse(filled) + unit_keys_parse(lb.read_text()))[:120])+if shutil.which("systemd-analyze"):+ ud = TMPD / "units"+ (ud / "checkout" / ".venv" / "bin").mkdir(parents=True)+ (ud / "checkout" / "tools").mkdir()+ (ud / "checkout" / ".venv" / "bin" / "python").symlink_to(sys.executable)+ (ud / "moss-cockpit.service").write_text(subprocess.run(+ ["sed", "-e", f"s|MOSS_USER|{os.environ.get('USER') or 'root'}|g", "-e", f"s|MOSS_DIR|{ud / 'checkout'}|g", str(svc)],+ capture_output=True, text=True).stdout)+ shutil.copy(lb, ud / "lcm-loopback.service")+ va = subprocess.run(["systemd-analyze", "verify", "./lcm-loopback.service", "./moss-cockpit.service"],+ cwd=ud, capture_output=True, text=True, timeout=60)+ (ud / "broken.service").write_text(lb.read_text().replace("Type=oneshot", "Type=oneshoot"))+ vb = subprocess.run(["systemd-analyze", "verify", "./broken.service"], cwd=ud, capture_output=True, text=True, timeout=60)+ check("systemd-analyze verify: both units (the cockpit filled by the install sed) pass",+ va.returncode == 0 and not va.stderr.strip(), " / ".join((va.stderr or va.stdout).split("\n"))[:200])+ check("... control: the same check refuses a broken copy", vb.returncode != 0,+ " / ".join((vb.stderr or "").strip().split("\n"))[-120:])+else:+ print(" -- no systemd-analyze here: the unit verify is skipped (the strict parse above still ran)")+++def survivor_after_main_exits(mode):+ """A transient user unit with this KillMode whose main process starts a+ new-session child (as Launcher.start does), then exits. Returns whether the+ child is still alive once systemd is done with the unit, or None without+ a user systemd."""+ unit = f"moss-killmode-bench-{os.getpid()}-{mode}"+ pidf = TMPD / f"child_{mode}"+ r = subprocess.run(["systemd-run", "--user", "--quiet", "--collect", f"--unit={unit}", "-p", f"KillMode={mode}",+ "bash", "-c", f'setsid sleep 30 < /dev/null > /dev/null 2>&1 & echo $! > {pidf}; '+ f'while [ ! -s {pidf} ]; do :; done'], capture_output=True, text=True)+ if r.returncode != 0:+ return None+ t0 = time.time()+ while time.time() - t0 < 10:+ st = subprocess.run(["systemctl", "--user", "is-active", unit], capture_output=True, text=True).stdout.strip()+ if pidf.exists() and pidf.read_text().strip() and st not in ("active", "activating", "deactivating"):+ break+ time.sleep(0.1)+ pid = int(pidf.read_text().strip())+ t0 = time.time()+ while time.time() - t0 < 3 and os.path.exists(f"/proc/{pid}"): # SIGTERM takes a moment+ if mode == "process":+ break+ time.sleep(0.1)+ alive = os.path.exists(f"/proc/{pid}")+ if alive:+ os.kill(pid, 9)+ subprocess.run(["systemctl", "--user", "stop", unit], capture_output=True)+ return alive+++if shutil.which("systemd-run"):+ with_file, default = survivor_after_main_exits(killmode), survivor_after_main_exits("control-group")+ if with_file is None or default is None:+ print(" -- no user systemd here: the live KillMode check is skipped")+ else:+ check("systemd, the unit's KillMode: the launcher it started outlives the cockpit", with_file is True)+ check("... control check: the default control-group would have killed it", default is False)+else:+ print(" -- no systemd-run here: the live KillMode check is skipped")++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/benches/bench_moss_preflight_cold.py b/benches/bench_moss_preflight_cold.pynew file mode 100644index 0000000..811f14f--- /dev/null+++ b/benches/bench_moss_preflight_cold.py@@ -0,0 +1,698 @@+#!/usr/bin/env python3+"""Cold bench for tools/moss_preflight.py: the MOSS flight check, every probe+stubbed, so what is proven is the VERDICT each world gets.++Known input -> known output in physical units (battery mV -> volts, lidar+points/s, GB free). No robot, no serial port, no dimOS: every probe of+the module is replaced by a counting fake, so the bench also proves which+devices the check did NOT open. Groups:++ A. all green - exit 0, "N/N OK - ready to fly", 11612 mV -> 11.61 V,+ the QT Py opened once at 115200 and not one byte written,+ the lidar probed once on the CP2102N path+ B. QT Py missing - KO on the board, firmware skipped, the port never opened+ C. stack running - registry entry with a live pid (and an unregistered+ `bin/dimos run`): firmware and lidar SKIPPED "held by the+ stack", zero opens; a dead registry pid is only stale+ D. battery - 10500 mV -> KO with the BMS sentence; 10100 -> under the+ cut; 10800 -> OK 10.80 V; ina_ok false -> INA219 not seen+ E. gamepad - js0 / 2563:0526 / 057e:2009 (the Nintendo sentence) / none+ F. --json - stdout is ONE JSON document; counts match the checks+ G. --quick / --no-spin - which stubs are never called+ H. firmware and port faults - held port, armed firmware, mute firmware,+ RealSense on USB 2, MOSS_PORT pointing at the lidar+ H2. switches - MOSS_CAMERA off skips the RealSense (no pyrealsense2+ needed), on it is a KO with the fix; MOSS_WRIST off skips+ the wrist camera; MOSS_MOCK_BUS / MOSS_REQUIRE_PAD read+ as the stack reads them (y, off, ...)+ I. software and env - the MOSS geometry line, a profile that fails, missing+ entry points (reinstall line), pygame missing, the venv+ importing moss_dimos from elsewhere or not at all,+ MOSS_MOCK_BUS=1+ J. --save - the file holds the same document (what the cockpit reads)+ L. LCM settings - loopback multicast, the 224.0.0.0/4 route and rmem:+ missing + passwordless sudo = OK (dimOS sets them),+ missing without sudo = KO with the boot unit; sudo is+ never probed when nothing is missing+ K. real probes on this machine - /proc fd scan finds a child holding a file,+ sysfs USB ids are vvvv:pppp, our own cmdline is listed,+ only the moss-dimos distribution's entry points count+ and never the checkout's own egg-info, the run registry+ follows XDG_STATE_HOME, the venv's moss_dimos is found+ where the stack would import it, the pygame banner never+ reaches stdout, a relative MOSS_STATE_DIR resolves to an+ absolute path++Run: PYTHONDONTWRITEBYTECODE=1 PYTHONPATH=. python3 benches/bench_moss_preflight_cold.py+"""++import contextlib+import fnmatch+import io+import json+import os+import subprocess+import sys+import tempfile+import time+import types+from pathlib import Path++ROOT = Path(__file__).resolve().parent.parent+sys.path.insert(0, str(ROOT))+sys.path.insert(0, str(ROOT / "tools"))++import moss_preflight as mp # noqa: E402++OK = 0+KO = 0+++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+++TMP = tempfile.TemporaryDirectory()+TMPD = Path(TMP.name)+QTPY = "/dev/serial/by-id/usb-Adafruit_QT_Py_ESP32-S3_No_PSRAM_00:00:00:00:00:00-if00"+C1 = "/dev/serial/by-id/usb-Silicon_Labs_CP2102N_USB_to_UART_Bridge_Controller_0000-if00-port0"+WRIST = "/dev/v4l/by-id/usb-Sonix_Technology_Co.__Ltd._KD-USB_Cameras_KD-USB_Cameras-video-index0"+TELEMETRY = {"ms": 1000, "fw": "qtpy-mdd3a-v3", "mode": 0, "reason": "boot",+ "target_pct": [0, 0], "output_pct": [0, 0], "ramp_ms": [5, 5],+ "ticks": [160585, 158263], "invalid_edges": [0, 0], "ina_ok": True,+ "bus_mV": 11612, "shunt_uV": 0, "current_mA": None}+D455 = {"name": "Intel RealSense D455F", "serial": "000000000001", "firmware": "5.17.0.10", "usb": "3.2"}+++class FakeSerial:+ def __init__(self, world, port, baud):+ self.world, self.port, self.baud = world, port, baud+ self.chunks = list(world["serial_chunks"])+ world["opens"].append((port, baud))++ def reset_input_buffer(self):+ pass++ def read(self, n):+ if self.chunks:+ return self.chunks.pop(0)+ time.sleep(0.005)+ return b""++ def write(self, data):+ self.world["written"].append(data)+ return len(data)++ def close(self):+ self.world["closed"] += 1+++def default_world():+ return {+ "files": {QTPY, C1, WRIST, "/dev/input/js0"},+ "runs": [], # (run_id, pid)+ "alive": set(),+ "cmdlines": [(1, "/sbin/init")],+ "holders": {},+ "telemetry": dict(TELEMETRY),+ "serial_chunks": None, # None = built from "telemetry"+ "usb": {"239a:8119", "10c4:ea60", "8086:0b5c", "2c7f:4a60"},+ "free_gb": 79.0,+ "mem_kb": 6_100_000,+ "rmem": "212992",+ "lo_flags": 0x9,+ "route": True, # the 224.0.0.0/4 route (None = `ip` cannot run)+ "sudo": True, # passwordless sudo+ "module_file": str(ROOT / "moss_dimos" / "__init__.py"), # where the stack imports moss_dimos+ "realsense_exc": None,+ "lidar": {"info": {"model": 65, "serial": "ABCD"}, "health": ("Good", 0),+ "turns": 3, "points_per_s": 1500.0, "hz": 10.0, "sectors": 36},+ "lidar_exc": None,+ "realsense": [dict(D455)],+ "imports_missing": set(),+ "entry_points": {"moss-nav", "moss-explore", "moss-gamepad", "moss-base"},+ "profile": "moss",+ "profile_exc": None,+ "env": {},+ # counters+ "opens": [], "written": [], "closed": 0, "lidar_calls": [], "holder_calls": [],+ "sudo_calls": 0, "realsense_calls": 0,+ }+++ENV_KEYS = ("MOSS_PORT", "MOSS_LIDAR_PORT", "MOSS_MOCK_BUS", "MOSS_RS_SERIAL", "MOSS_CAMERA",+ "MOSS_WRIST", "MOSS_REQUIRE_PAD", "MOSS_STATE_DIR")+++def run(argv=(), **over):+ w = default_world()+ w.update(over)+ if w["serial_chunks"] is None:+ line = json.dumps(w["telemetry"]).encode() + b"\n"+ w["serial_chunks"] = [b"MOSS V0.4 firmware banner\n" + line[:40], line[40:] + line, line]+ runs_dir = TMPD / f"runs_{len(list(TMPD.glob('runs_*')))}"+ runs_dir.mkdir()+ for run_id, pid in w["runs"]:+ (runs_dir / f"{run_id}.json").write_text(json.dumps(+ {"run_id": run_id, "pid": pid, "blueprint": "moss-dimos.moss-nav",+ "started_at": "2026-01-01T00:00:00+00:00", "log_dir": "/x"}))+ state = TMPD / f"state_{runs_dir.name}"++ mp._ls = lambda pat: sorted(f for f in w["files"] if fnmatch.fnmatch(f, pat))+ mp._exists = lambda p: p in w["files"]+ mp._runs_dir = lambda: runs_dir+ mp._pid_alive = lambda pid: pid in w["alive"]+ mp._proc_cmdlines = lambda: list(w["cmdlines"])+ mp._port_holders = lambda dev: (w["holder_calls"].append(dev), w["holders"].get(dev, []))[1]+ mp._open_serial = lambda port, baud: FakeSerial(w, port, baud)+ mp._usb_devices = lambda: set(w["usb"])+ mp._statvfs = lambda path: types.SimpleNamespace(f_bavail=int(w["free_gb"] * 1e9 / 4096), f_frsize=4096)+ mp._meminfo = lambda: {"MemTotal": 7_600_000, "MemAvailable": w["mem_kb"]}+ mp._sysctl = lambda name: w["rmem"]+ mp._lo_flags = lambda: w["lo_flags"]+ mp._mcast_route = lambda: w["route"]++ def sudo():+ w["sudo_calls"] += 1+ return w["sudo"]+ mp._sudo_ok = sudo+ mp._module_file = lambda: w["module_file"]++ def lidar(port):+ w["lidar_calls"].append(port)+ if w["lidar_exc"]:+ raise w["lidar_exc"]+ return dict(w["lidar"])+ mp._lidar_probe = lidar++ def realsense():+ w["realsense_calls"] += 1+ if w["realsense_exc"]:+ raise w["realsense_exc"]+ return [dict(d) for d in w["realsense"]]+ mp._realsense_probe = realsense++ def imp(name):+ if name in w["imports_missing"]:+ raise ModuleNotFoundError(f"No module named '{name}'")+ return types.SimpleNamespace(__version__="9.9")+ mp._import = imp+ mp._entry_points = lambda group="dimos.blueprints": set(w["entry_points"])++ def profile():+ if w["profile_exc"]:+ raise w["profile_exc"]+ return (types.SimpleNamespace(name=w["profile"], robot_width_m=0.34, pivot_diameter_m=0.5, lidar_height_m=0.2),+ {"lethal_clearance_m": 0.22, "pivot_clearance_m": 0.25})+ mp._robot_profile = profile+ mp.LISTEN_S = 0.05++ saved = {k: os.environ.get(k) for k in ENV_KEYS}+ for k in ENV_KEYS:+ os.environ.pop(k, None)+ os.environ["MOSS_STATE_DIR"] = str(state)+ os.environ.update(w["env"])+ buf = io.StringIO()+ try:+ with contextlib.redirect_stdout(buf):+ rc = mp.main(list(argv))+ finally:+ for k, v in saved.items():+ if v is None:+ os.environ.pop(k, None)+ else:+ os.environ[k] = v+ w["state"] = state+ return rc, buf.getvalue(), list(mp.RESULTS), w+++def find(results, label):+ return [r for r in results if r["label"] == label]+++def line_of(out, label):+ for ln in out.splitlines():+ if f" {label}" in ln and (ln.startswith(" OK") or ln.startswith(" KO") or ln.startswith(" --")):+ return ln.strip()+ return ""+++def last_line(out):+ lines = [ln for ln in out.strip().splitlines() if ln.strip()]+ return lines[-1] if lines else ""+++HEADERS = ("ports (by-id)", "firmware (QT Py, listen only)", "lidar (RPLidar C1, ~5 s spin)",+ "camera (RealSense, enumeration only)", "wrist camera (UVC)", "gamepad", "system", "software", "state dir")++print("A. all green")+rc, out, res, w = run()+counted = [r for r in res if not r.get("skipped")]+check("exit 0", rc == 0, f"rc {rc}")+check("summary N/N OK - ready to fly", last_line(out) == f"{len(counted)}/{len(counted)} OK - ready to fly", last_line(out))+check("no KO line at all", " KO " not in out)+check("battery 11612 mV -> 11.61 V", line_of(out, "battery") == "OK battery - 11.61 V", line_of(out, "battery"))+check("firmware version read from telemetry", "qtpy-mdd3a-v3" in line_of(out, "firmware version"))+check("firmware disarmed: mode 0", "mode 0 (off)" in line_of(out, "firmware disarmed"))+check("ticks L 160585 R 158263", "L 160585 R 158263" in line_of(out, "encoder ticks"))+check("driver absent -> informational OK with the build note",+ line_of(out, "motor driver") == "OK motor driver - not reported (a firmware build older than the driver key)",+ line_of(out, "motor driver"))+check("telemetry parsed across chunk boundaries (3 lines, banner ignored)",+ "3 line(s)" in line_of(out, "firmware telemetry"), line_of(out, "firmware telemetry"))+check("QT Py opened ONCE, at 115200, on the by-id path", w["opens"] == [(QTPY, 115200)], str(w["opens"]))+check("not one byte written to the firmware", w["written"] == [], str(w["written"]))+check("port closed after listening", w["closed"] == 1)+check("lidar probed once on the CP2102N path", w["lidar_calls"] == [C1], str(w["lidar_calls"]))+check("lidar 1500 points/s, 36/36 sectors", "1500 points/s, 36/36 sectors" in line_of(out, "lidar scan"))+check("RealSense on USB 3.2 with serial and firmware",+ "serial 000000000001 fw 5.17.0.10 USB 3.2" in line_of(out, "RealSense D455"))+check("disk 79.0 GB free", "79.0 GB free" in line_of(out, "disk for recordings"), line_of(out, "disk for recordings"))+check("RAM 6.1 GB", "6.1 GB" in line_of(out, "RAM available"))+check("rmem 212992 is a warning, never a KO",+ line_of(out, "socket buffers").startswith("OK") and "warning" in line_of(out, "socket buffers"))+check("lo without MULTICAST, passwordless sudo: OK, dimOS turns it on at launch",+ line_of(out, "loopback multicast") == "OK loopback multicast - off - dimOS turns it on with sudo at launch",+ line_of(out, "loopback multicast"))+check("the multicast route is checked on its own line",+ line_of(out, "multicast route") == "OK multicast route - 224.0.0.0/4 routed", line_of(out, "multicast route"))+check("pygame is one of the software imports, pyrealsense2 is not (the camera section owns it)",+ line_of(out, "import pygame").startswith("OK") and not line_of(out, "import pyrealsense2"))+check("the stack's moss_dimos comes from this checkout",+ line_of(out, "moss-dimos install") == "OK moss-dimos install - the stack imports moss_dimos from this checkout",+ line_of(out, "moss-dimos install"))+check("the state dir and recordings/ were created", (w["state"] / "recordings").is_dir())+check("sections in the spec order",+ [r["section"].split(" ")[0] for r in res][:1] == ["stack"]+ and [out.find("\n" + h + "\n") for h in HEADERS] == sorted(out.find("\n" + h + "\n") for h in HEADERS)+ and -1 not in [out.find("\n" + h + "\n") for h in HEADERS])+rc, out, res, w = run(lo_flags=0x1009)+check("lo flags 0x1009 -> multicast on", line_of(out, "loopback multicast") == "OK loopback multicast - on")++print("B. QT Py missing")+rc, out, res, w = run(files={C1, WRIST, "/dev/input/js0"})+check("exit 1", rc == 1, f"rc {rc}")+check("KO on the motor board with the fix", line_of(out, "motor board (QT Py)").startswith("KO")+ and "not on USB" in line_of(out, "motor board (QT Py)"))+check("summary names it", "KO: motor board (QT Py)" in last_line(out), last_line(out))+check("firmware skipped, the serial port never opened",+ w["opens"] == [] and "skipped: no free QT Py port" in line_of(out, "firmware telemetry"))+check("the lidar still checked on its own port", w["lidar_calls"] == [C1])++print("C. a stack already running")+STACK_CMD = "/home/x/moss/.venv/bin/python3 /home/x/moss/.venv/bin/dimos --rerun-open none run moss-dimos.moss-nav --daemon"+rc, out, res, w = run(runs=[("20260101-040000-moss-dimos-moss-nav", 4242)], alive={4242},+ cmdlines=[(1, "/sbin/init"), (4242, STACK_CMD)])+check("stack verdict OK with run id and pid",+ line_of(out, "stack") == "OK stack - 20260101-040000-moss-dimos-moss-nav pid 4242", line_of(out, "stack"))+fw, li = find(res, "firmware telemetry"), find(res, "lidar scan")+check("firmware SKIPPED: held by the stack",+ len(fw) == 1 and fw[0].get("skipped") and fw[0]["detail"].startswith("skipped: held by the stack"),+ fw[0]["detail"] if fw else "none")+check("lidar SKIPPED: held by the stack",+ len(li) == 1 and li[0].get("skipped") and li[0]["detail"].startswith("skipped: held by the stack"))+check("zero serial opens, zero lidar spins", w["opens"] == [] and w["lidar_calls"] == [])+check("no port-holder scan either (the stack holds them by design)", w["holder_calls"] == [])+check("exit 0 (a stack is not a hardware fault) and skips counted apart",+ rc == 0 and "(2 skipped)" in last_line(out), last_line(out))+rc, out, res, w = run(cmdlines=[(1, "/sbin/init"), (777, "/home/x/moss/.venv/bin/dimos --rerun-open none run moss-dimos.moss-nav --daemon")])+check("unregistered `bin/dimos run` process -> probes skipped too",+ "unregistered dimos run pid 777" in line_of(out, "stack") and w["opens"] == [] and w["lidar_calls"] == [],+ line_of(out, "stack"))+rc, out, res, w = run(cmdlines=[(1, "/sbin/init"), (778, "/home/x/moss/.venv/bin/python3 /home/x/moss/.venv/bin/dimos log"),+ (779, "python3 tools/moss_cockpit.py --port 8904")])+check("`dimos log` and the cockpit are not a stack", line_of(out, "stack") == "OK stack - none"+ and len(w["opens"]) == 1, line_of(out, "stack"))+rc, out, res, w = run(runs=[("20260101-155115-moss-dimos-moss-gamepad", 999)], alive=set())+check("dead registry pid = stale, not a stack: probes run",+ "none (1 stale registry entry)" in line_of(out, "stack") and len(w["opens"]) == 1 and w["lidar_calls"] == [C1],+ line_of(out, "stack"))++rc, out, res, w = run(runs=[("20260101-155115-moss-dimos-moss-gamepad", 818205)], alive={818205},+ cmdlines=[(1, "/sbin/init"), (818205, "-bash")])+fw = find(res, "firmware telemetry")+check("registry pid alive but reused by a shell = stale, not a stack: battery read, lidar spun",+ "none (1 stale registry entry)" in line_of(out, "stack") and len(w["opens"]) == 1+ and w["lidar_calls"] == [C1] and len(fw) == 1 and not fw[0].get("skipped")+ and "11.61 V" in line_of(out, "battery"), line_of(out, "stack"))++print("D. battery (3S pack, INA219 voltage)")+for mv, want_ok, needle in ((10500, False, "10.50 V - under 10.8 V: the BMS cuts at 10.2 V under load"),+ (10100, False, "10.10 V - under the BMS cut (10.2 V)"),+ (10800, True, "10.80 V"),+ (12600, True, "12.60 V")):+ rc, out, res, w = run(telemetry=dict(TELEMETRY, bus_mV=mv))+ ln = line_of(out, "battery")+ check(f"{mv} mV -> {'OK' if want_ok else 'KO'} '{needle}'",+ ln.startswith("OK" if want_ok else "KO") and needle in ln and rc == (0 if want_ok else 1), ln)+rc, out, res, w = run(telemetry=dict(TELEMETRY, ina_ok=False, bus_mV=None))+check("ina_ok false -> KO INA219 not seen", line_of(out, "battery").startswith("KO") and "INA219 not seen" in out)++print("E. gamepad diagnosis")+files_nopad = {QTPY, C1, WRIST}+rc, out, res, w = run(files=files_nopad, usb={"057e:2009", "239a:8119"})+check("057e:2009 -> the Nintendo sentence, as a warning (nothing refuses a missing pad)",+ "pad enumerated as a Nintendo Pro Controller (057e:2009) without a driver: switch the pad to its "+ "Android/X-input mode and replug" in line_of(out, "gamepad") and line_of(out, "gamepad").startswith("OK")+ and "WARN" in line_of(out, "gamepad"),+ line_of(out, "gamepad"))+rc, out, res, w = run(files=files_nopad, usb={"2563:0526"})+check("2563:0526 -> receiver visible, no js0", "receiver visible (2563:0526), no js0: replug or reboot" in line_of(out, "gamepad"))+rc, out, res, w = run(files=files_nopad, usb=set())+check("nothing -> no pad, still a warning", "no pad" in line_of(out, "gamepad") and line_of(out, "gamepad").startswith("OK"))+rc, out, res, w = run(files=files_nopad, usb=set(), env={"MOSS_REQUIRE_PAD": "1"})+check("MOSS_REQUIRE_PAD=1 -> no pad is a KO and the check fails", line_of(out, "gamepad").startswith("KO") and rc == 1)+rc, out, res, w = run(usb={"057e:2009"})+check("js0 present wins over any USB id", line_of(out, "gamepad").startswith("OK"))++print("F. --json")+rc, out, res, w = run(["--json"], telemetry=dict(TELEMETRY, bus_mV=10500))+try:+ doc = json.loads(out)+ parsed = True+except ValueError as exc:+ doc, parsed = {}, False+ print(" ", exc, out[:200])+check("stdout is exactly one JSON document", parsed)+checks = doc.get("checks", [])+counted = [c for c in checks if not c.get("skipped")]+check("keys ts, checks, ok_count, ko_count, summary",+ all(k in doc for k in ("ts", "checks", "ok_count", "ko_count", "summary")))+check("each check has ok, label, detail, section",+ bool(checks) and all(set(("ok", "label", "detail", "section")) <= set(c) for c in checks))+check("ok_count / ko_count match the checks",+ doc.get("ok_count") == sum(1 for c in counted if c["ok"]) and doc.get("ko_count") == sum(1 for c in counted if not c["ok"])+ and doc.get("ko_count") == 1, f"{doc.get('ok_count')} / {doc.get('ko_count')}")+check("summary carries the KO label", doc.get("summary", "").endswith("KO: battery"), doc.get("summary"))+check("exit 1 with a KO in JSON mode too", rc == 1)+check("ts is now", abs(doc.get("ts", 0) - time.time()) < 60)++print("G. --quick and --no-spin")+rc, out, res, w = run(["--quick"])+check("--quick: the serial stub is never called", w["opens"] == [])+check("--quick: the lidar stub is never called", w["lidar_calls"] == [])+check("--quick: both reported skipped, exit 0",+ rc == 0 and "skipped: --quick" in line_of(out, "firmware telemetry") and "skipped" in line_of(out, "lidar scan"))+rc, out, res, w = run(["--no-spin"])+check("--no-spin: the firmware IS listened to (battery judged)", len(w["opens"]) == 1 and "11.61 V" in out)+check("--no-spin: the lidar never spins", w["lidar_calls"] == [])++print("H. firmware and port faults")+rc, out, res, w = run(holders={QTPY: [3131]})+check("QT Py held by pid 3131 -> KO naming it", "held by pid 3131" in line_of(out, "motor board (QT Py) free")+ and rc == 1, line_of(out, "motor board (QT Py) free"))+check("... and the firmware is never opened against it", w["opens"] == [])+rc, out, res, w = run(telemetry=dict(TELEMETRY, mode=2, reason="drive"))+check("mode 2 -> KO firmware disarmed with the reason",+ line_of(out, "firmware disarmed") == "KO firmware disarmed - mode 2 (drive) - something else is driving it",+ line_of(out, "firmware disarmed"))+rc, out, res, w = run(serial_chunks=[b"", b"garbage\n"])+check("mute firmware -> KO, no further firmware verdict",+ line_of(out, "firmware telemetry").startswith("KO") and "mute" in out and not find(res, "battery"))+rc, out, res, w = run(telemetry=dict(TELEMETRY, driver="mdd10a"))+check("driver reported -> shown", line_of(out, "motor driver") == "OK motor driver - mdd10a")+rc, out, res, w = run(realsense=[dict(D455, usb="2.1")])+check("RealSense on USB 2.1 -> KO 5 fps max", "on USB 2: 5 fps max" in line_of(out, "RealSense D455")+ and line_of(out, "RealSense D455").startswith("KO"))+rc, out, res, w = run(realsense=[])+check("camera on, no D455 -> KO with both fixes (USB 3, or MOSS_CAMERA=0)",+ line_of(out, "RealSense D455").startswith("KO") and "MOSS_CAMERA=0" in line_of(out, "RealSense D455") and rc == 1,+ line_of(out, "RealSense D455"))+rc, out, res, w = run(env={"MOSS_PORT": C1})+check("MOSS_PORT pointing at the lidar -> KO, and the lidar is never written as a motor port",+ line_of(out, "motor port is not the lidar").startswith("KO") and w["opens"] == [], line_of(out, "motor port is not the lidar"))+rc, out, res, w = run(lidar=dict(default_world()["lidar"], sectors=31, points_per_s=1400.0))+check("31/36 sectors -> KO lidar scan", line_of(out, "lidar scan").startswith("KO") and "31/36" in out)+rc, out, res, w = run(lidar_exc=RuntimeError("no descriptor for a550"))+check("lidar mute -> KO with the exception", "no descriptor for a550" in line_of(out, "lidar answers")+ and line_of(out, "lidar answers").startswith("KO"))+rc, out, res, w = run(files={QTPY, C1, "/dev/input/js0"})+check("wrist camera absent -> OK with a warning (the stack runs without wrist_image)",+ line_of(out, "wrist camera").startswith("OK") and "WARN" in line_of(out, "wrist camera")+ and "without wrist_image" in line_of(out, "wrist camera"), line_of(out, "wrist camera"))+rc, out, res, w = run(free_gb=3.2, mem_kb=1_900_000)+check("3.2 GB free -> KO disk; 1.9 GB RAM -> KO", "3.2 GB free" in line_of(out, "disk for recordings")+ and line_of(out, "disk for recordings").startswith("KO") and line_of(out, "RAM available").startswith("KO"))++print("H2. the MOSS_* switches, read as the stack reads them")+NO_RS = ImportError("No module named 'pyrealsense2'")+for val in ("0", "off", " No "):+ rc, out, res, w = run(realsense_exc=NO_RS, imports_missing={"pyrealsense2"}, env={"MOSS_CAMERA": val})+ rs = find(res, "RealSense")+ check(f"MOSS_CAMERA={val!r}, no pyrealsense2 -> RealSense skipped, never probed, exit 0",+ rc == 0 and len(rs) == 1 and rs[0].get("skipped") and "this flight builds no RealSense" in rs[0]["detail"]+ and w["realsense_calls"] == 0 and " KO " not in out, f"rc {rc} {line_of(out, 'RealSense')}")+rc, out, res, w = run(realsense_exc=NO_RS, imports_missing={"pyrealsense2"})+check("MOSS_CAMERA unset, no pyrealsense2 -> KO RealSense with the install / MOSS_CAMERA=0 fix, exit 1",+ rc == 1 and line_of(out, "RealSense").startswith("KO") and "pyrealsense2 not importable" in line_of(out, "RealSense")+ and "MOSS_CAMERA=0" in line_of(out, "RealSense") and last_line(out).endswith("KO: RealSense"),+ f"rc {rc} {last_line(out)}")+rc, out, res, w = run(realsense_exc=NO_RS, env={"MOSS_CAMERA": "1"})+check("MOSS_CAMERA=1 -> the same KO", rc == 1 and line_of(out, "RealSense").startswith("KO"))+rc, out, res, w = run(files={QTPY, C1, "/dev/input/js0"}, env={"MOSS_WRIST": "false"})+wr = find(res, "wrist camera")+check("MOSS_WRIST=false -> wrist camera skipped (the stack builds none)",+ len(wr) == 1 and wr[0].get("skipped") and "MOSS_WRIST=false" in wr[0]["detail"] and rc == 0, line_of(out, "wrist camera"))+rc, out, res, w = run(files={C1, WRIST, "/dev/input/js0"}, env={"MOSS_MOCK_BUS": "y"})+check("MOSS_MOCK_BUS=y mocks the bus as the adapter does: no QT Py needed, never opened, exit 0",+ rc == 0 and "MOSS_MOCK_BUS=y: fake firmware" in line_of(out, "motor board (QT Py)") and w["opens"] == [],+ line_of(out, "motor board (QT Py)"))+rc, out, res, w = run(files={C1, WRIST, "/dev/input/js0"}, env={"MOSS_MOCK_BUS": "off"})+check("MOSS_MOCK_BUS=off -> the real board is required (KO without it)",+ rc == 1 and line_of(out, "motor board (QT Py)").startswith("KO"), line_of(out, "motor board (QT Py)"))+rc, out, res, w = run(files={QTPY, C1, WRIST}, usb=set(), env={"MOSS_REQUIRE_PAD": "yes"})+check("MOSS_REQUIRE_PAD=yes -> no pad is a KO", rc == 1 and line_of(out, "gamepad").startswith("KO"))+rc, out, res, w = run(files={QTPY, C1, WRIST}, usb=set(), env={"MOSS_REQUIRE_PAD": "0"})+check("MOSS_REQUIRE_PAD=0 -> no pad stays a warning", rc == 0 and line_of(out, "gamepad").startswith("OK"))++print("I. software and env")+rc, out, res, w = run()+check("robot profile: the MOSS geometry, lethal 0.22 m",+ line_of(out, "robot profile") == "OK robot profile - moss: width 0.34 m, pivot 0.5 m, lidar 0.2 m up, "+ "lethal 0.22 m (placeholders until measured)", line_of(out, "robot profile"))+check("blueprints registered under the moss-dimos namespace",+ line_of(out, "blueprint moss-nav") == "OK blueprint moss-nav - registered as moss-dimos.moss-nav",+ line_of(out, "blueprint moss-nav"))+rc, out, res, w = run(profile_exc=ValueError("MOSS_ROBOT_WIDTH_M: not a number"))+check("a profile that fails -> KO robot profile with the reason",+ line_of(out, "robot profile") == "KO robot profile - ValueError: MOSS_ROBOT_WIDTH_M: not a number" and rc == 1,+ line_of(out, "robot profile"))+rc, out, res, w = run(entry_points={"moss-gamepad", "moss-base"})+nav = line_of(out, "blueprint moss-nav")+check("moss-nav / moss-explore missing -> KO with the reinstall line for this checkout",+ nav.startswith("KO") and "missing from moss-dimos" in nav+ and f"reinstall in {ROOT}: uv pip install --python .venv/bin/python -e . --no-deps" in nav+ and ".venv/bin/pip install -e . --no-deps" in nav+ and line_of(out, "blueprint moss-explore").startswith("KO") and line_of(out, "blueprint moss-gamepad").startswith("OK"),+ nav)+rc, out, res, w = run(imports_missing={"kiss_icp"})+check("kiss_icp missing -> KO import kiss_icp", line_of(out, "import kiss_icp").startswith("KO"))+rc, out, res, w = run(imports_missing={"pygame"})+check("pygame missing -> KO import pygame, named in the summary, exit 1 (nothing would drive)",+ line_of(out, "import pygame").startswith("KO") and "KO: import pygame" in last_line(out) and rc == 1,+ last_line(out))+rc, out, res, w = run(module_file=None)+check("moss_dimos not importable in the venv -> KO with the install line",+ line_of(out, "moss-dimos install").startswith("KO") and "does not import" in line_of(out, "moss-dimos install")+ and "uv pip install --python .venv/bin/python -e . --no-deps" in line_of(out, "moss-dimos install") and rc == 1,+ line_of(out, "moss-dimos install"))+rc, out, res, w = run(module_file="/srv/old_checkout/moss_dimos/__init__.py")+check("the venv imports moss_dimos from another checkout -> KO naming it, reinstall here",+ line_of(out, "moss-dimos install").startswith("KO") and "/srv/old_checkout/moss_dimos" in line_of(out, "moss-dimos install")+ and f"reinstall in {ROOT}" in line_of(out, "moss-dimos install") and rc == 1, line_of(out, "moss-dimos install"))+rc, out, res, w = run(files={C1, WRIST, "/dev/input/js0"}, env={"MOSS_MOCK_BUS": "1"})+check("MOSS_MOCK_BUS=1: no QT Py needed, firmware skipped, exit 0",+ rc == 0 and "MOSS_MOCK_BUS=1" in line_of(out, "motor board (QT Py)") and w["opens"] == [], last_line(out))++print("J. --save (the document the cockpit serves)")+target = TMPD / "saved" / "preflight.json"+rc, out, res, w = run(["--save", str(target)])+try:+ saved = json.loads(target.read_text())+except (OSError, ValueError):+ saved = {}+check("file written, human lines still on stdout", bool(saved) and "ready to fly" in last_line(out))+check("same checks and counts as the run", saved.get("ok_count") == len([r for r in res if not r.get("skipped")])+ and saved.get("ko_count") == 0 and len(saved.get("checks", [])) == len(res))+check("no .tmp left behind", not (target.parent / "preflight.json.tmp").exists())++print("L. LCM system settings (what dimOS sets with sudo at launch)")+FIX = "install tools/lcm-loopback.service to set it at boot"+rc, out, res, w = run(sudo=False)+check("lo multicast off and no passwordless sudo -> KO with the boot unit, exit 1",+ line_of(out, "loopback multicast").startswith("KO") and FIX in line_of(out, "loopback multicast")+ and "dimOS cannot turn it on at launch" in line_of(out, "loopback multicast") and rc == 1 and w["sudo_calls"] == 1,+ line_of(out, "loopback multicast"))+check("... rmem low without sudo stays a warning (dimOS treats the buffers as optional)",+ line_of(out, "socket buffers").startswith("OK") and "warning" in line_of(out, "socket buffers")+ and FIX in line_of(out, "socket buffers"), line_of(out, "socket buffers"))+rc, out, res, w = run(sudo=False, lo_flags=0x1009, route=False, rmem="67108864")+check("route missing and no sudo -> KO multicast route, lo on is OK",+ line_of(out, "multicast route").startswith("KO") and "no 224.0.0.0/4 route" in line_of(out, "multicast route")+ and line_of(out, "loopback multicast") == "OK loopback multicast - on" and rc == 1, line_of(out, "multicast route"))+rc, out, res, w = run(sudo=True, lo_flags=0x1009, route=False)+check("route missing with passwordless sudo -> OK, dimOS adds it",+ line_of(out, "multicast route") == "OK multicast route - no 224.0.0.0/4 route - dimOS adds it on lo with sudo at launch"+ and rc == 0, line_of(out, "multicast route"))+rc, out, res, w = run(route=None, lo_flags=0x1009, rmem="67108864")+check("`ip` cannot run -> KO multicast route", line_of(out, "multicast route").startswith("KO")+ and "iproute2" in line_of(out, "multicast route") and rc == 1, line_of(out, "multicast route"))+rc, out, res, w = run(sudo=False, lo_flags=0x1009, route=True, rmem="67108864")+check("everything set at boot -> all OK and sudo never even probed",+ rc == 0 and w["sudo_calls"] == 0 and line_of(out, "socket buffers") == "OK socket buffers - rmem_max 67108864"+ and line_of(out, "loopback multicast") == "OK loopback multicast - on", f"rc {rc} sudo calls {w['sudo_calls']}")++print("K. the real probes, on this machine (no device opened)")+import importlib # noqa: E402+real = importlib.reload(mp)+real_runs_dir = real._runs_dir # the real one: a case below replaces it on the module+held = TMPD / "held_port"+held.write_text("x")+child = subprocess.Popen([sys.executable, "-c",+ f"import time; f = open({str(held)!r}); print('ready', flush=True); time.sleep(20)"],+ stdout=subprocess.PIPE, text=True)+child.stdout.readline()+try:+ holders = real._port_holders(str(held))+ check("/proc fd scan finds the child holding the file", child.pid in holders, f"{holders} vs {child.pid}")+ check("... and never reports this process", os.getpid() not in holders)+finally:+ child.kill()+ child.wait()+check("nobody holds it once the child is gone", real._port_holders(str(held)) == [])+ids = real._usb_devices()+check("sysfs USB ids look like vvvv:pppp", all(len(i) == 9 and i[4] == ":" for i in ids), f"{len(ids)} devices")+check("our own cmdline is in the /proc scan", any(pid == os.getpid() and "bench_moss_preflight_cold" in cmd+ for pid, cmd in real._proc_cmdlines()))+check("lo flags read as an int (or None off Linux)", real._lo_flags() is None or isinstance(real._lo_flags(), int))+check("nothing ran at import (RESULTS empty after reload)", real.RESULTS == [])+# a registry entry naming a LIVE pid that is not dimos (pid reuse after a reboot)+reused = subprocess.Popen([sys.executable, "-c", "import time; print('ready', flush=True); time.sleep(20)"],+ stdout=subprocess.PIPE, text=True)+reused.stdout.readline()+try:+ real_runs = TMPD / "real_runs"+ real_runs.mkdir()+ (real_runs / "20260101-155115-moss-dimos-moss-gamepad.json").write_text(json.dumps(+ {"run_id": "20260101-155115-moss-dimos-moss-gamepad", "pid": reused.pid}))+ real._runs_dir = lambda: real_runs+ ctx = {}+ with contextlib.redirect_stdout(io.StringIO()) as sbuf:+ real.check_stack(ctx)+ check("real /proc: a registry pid that is alive but not `dimos run` is stale, not a stack",+ real._pid_alive(reused.pid) and f"pid {reused.pid}" not in (ctx.get("stack") or "")+ and "1 stale registry entry" in sbuf.getvalue(),+ sbuf.getvalue().strip())+finally:+ reused.kill()+ reused.wait()++# Entry points through the real importlib.metadata: two installed distributions+# register blueprints in a private group; only the one named moss-dimos (any+# spelling that canonicalizes to it) counts, as `dimos run moss-dimos.<name>` does.+site = TMPD / "fake_site"+for dist_dir, name, eps in (("Moss_Dimos-0.1.0.dist-info", "Moss_Dimos",+ ("moss-nav = moss_dimos.blueprints:moss_nav_blueprint",+ "moss-base = moss_dimos.blueprints:moss_base_blueprint")),+ ("other_dimos-0.0.1.dist-info", "other-dimos",+ ("moss-explore = other_dimos.blueprints:explore",+ "moss-nav = other_dimos.blueprints:nav"))):+ (site / dist_dir).mkdir(parents=True)+ (site / dist_dir / "METADATA").write_text(f"Metadata-Version: 2.1\nName: {name}\nVersion: 0.0.1\n")+ (site / dist_dir / "entry_points.txt").write_text("[moss.bench.blueprints]\n" + "\n".join(eps) + "\n")+sys.path.insert(0, str(site))+try:+ got = real._entry_points("moss.bench.blueprints")+finally:+ sys.path.remove(str(site))+check("real entry points: moss-nav + moss-base from Moss_Dimos; other-dimos's moss-explore ignored",+ got == {"moss-nav", "moss-base"}, str(sorted(got)))+# The same metadata found in the checkout itself (the egg-info an editable+# install leaves there) is not on the stack's path: it never counts.+real_root = real.ROOT+real.ROOT = site.resolve()+sys.path.insert(0, str(site))+try:+ got_root = real._entry_points("moss.bench.blueprints")+finally:+ sys.path.remove(str(site))+ real.ROOT = real_root+check("real entry points: metadata sitting in the checkout itself is ignored", got_root == set(), str(sorted(got_root)))++# The run registry follows XDG_STATE_HOME, as dimOS, moss-fly.sh and the cockpit do.+old_xdg = os.environ.get("XDG_STATE_HOME")+try:+ os.environ["XDG_STATE_HOME"] = str(TMPD / "xdg")+ rd_xdg = real_runs_dir()+ os.environ["XDG_STATE_HOME"] = ""+ rd_empty = real_runs_dir()+ os.environ.pop("XDG_STATE_HOME")+ rd_home = real_runs_dir()+finally:+ if old_xdg is None:+ os.environ.pop("XDG_STATE_HOME", None)+ else:+ os.environ["XDG_STATE_HOME"] = old_xdg+check("real registry dir: XDG_STATE_HOME=<tmp>/xdg -> <tmp>/xdg/dimos/runs", rd_xdg == TMPD / "xdg" / "dimos" / "runs", str(rd_xdg))+check("... unset or empty -> ~/.local/state/dimos/runs",+ rd_home == rd_empty == Path.home() / ".local" / "state" / "dimos" / "runs", f"{rd_home} / {rd_empty}")++# Where a fresh interpreter of this venv imports moss_dimos from (as the stack would).+other = TMPD / "other_checkout"+(other / "moss_dimos").mkdir(parents=True)+(other / "moss_dimos" / "__init__.py").write_text('"""another checkout"""\n')+old_pp = os.environ.get("PYTHONPATH")+try:+ os.environ["PYTHONPATH"] = str(ROOT)+ mf_here = real._module_file()+ os.environ["PYTHONPATH"] = str(other)+ mf_other = real._module_file()+finally:+ if old_pp is None:+ os.environ.pop("PYTHONPATH", None)+ else:+ os.environ["PYTHONPATH"] = old_pp+check("real module probe: this checkout on the path -> moss_dimos from this checkout",+ mf_here is not None and Path(mf_here).resolve() == ROOT / "moss_dimos" / "__init__.py", str(mf_here))+check("... another checkout first on the path -> that one is reported",+ mf_other is not None and Path(mf_other).resolve() == (other / "moss_dimos" / "__init__.py").resolve(), str(mf_other))++# pygame's import banner never reaches stdout (where --json writes its document).+probe = subprocess.run(+ [sys.executable, "-c",+ "import sys; sys.path.insert(0, 'tools'); import moss_preflight as m\n"+ "m.SOFTWARE_IMPORTS = ('pygame',); m._entry_points = lambda group='': set()\n"+ "m._module_file = lambda: None; m._QUIET[0] = True\n"+ "m._robot_profile = lambda: (_ for _ in ()).throw(RuntimeError('not needed'))\n"+ "m.check_software({})\n"+ "sys.stderr.write('pygame ' + ('OK' if m.RESULTS[0]['ok'] else 'missing') + '\\n')\n"],+ cwd=ROOT, env={k: v for k, v in os.environ.items() if k != "PYGAME_HIDE_SUPPORT_PROMPT"},+ capture_output=True, text=True, timeout=120)+if "pygame missing" in probe.stderr:+ print(" -- pygame is not installed here: the banner check is skipped")+else:+ check("real pygame import through the flight check: nothing on stdout",+ probe.returncode == 0 and probe.stdout == "" and "pygame OK" in probe.stderr,+ (probe.stdout or probe.stderr).strip()[-160:])++# The state dir as the recorder resolves it: a relative MOSS_STATE_DIR is made absolute.+old_state = os.environ.get("MOSS_STATE_DIR")+os.environ["MOSS_STATE_DIR"] = "rel/moss_state"+try:+ sd = real._state_dir()+finally:+ if old_state is None:+ os.environ.pop("MOSS_STATE_DIR", None)+ else:+ os.environ["MOSS_STATE_DIR"] = old_state+check("MOSS_STATE_DIR=rel/moss_state -> <cwd>/rel/moss_state (absolute, as the recorder)",+ sd == Path.cwd() / "rel" / "moss_state" and sd.is_absolute(), str(sd))++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/benches/bench_recovering_planner_cold.py b/benches/bench_recovering_planner_cold.pynew file mode 100644index 0000000..65d2e91--- /dev/null+++ b/benches/bench_recovering_planner_cold.py@@ -0,0 +1,290 @@+"""Cold bench for RecoveringGlobalPlanner.++Under test: stuck = goal abandoned, NO recovery motion of any kind (a blind+back-off on every stuck verdict walks the robot into whatever is behind it).+The only scripted move left is the contact escape (the `bump` stream; on MOSS+its producer is the explorer's back-off), and a second contact aborts it dead+instead of being dropped.++No robot. A fake local planner integrates the escape twists it receives into+the odometry the planner reads. Needs dimOS and reactivex.++Run from the repository root: python benches/bench_recovering_planner_cold.py+"""++import sys+import threading+import time+from pathlib import Path as _FsPath++sys.path.insert(0, str(_FsPath(__file__).resolve().parent.parent))++from reactivex import Subject # noqa: E402++from dimos.core.global_config import GlobalConfig # noqa: E402+from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped # noqa: E402+from dimos.msgs.geometry_msgs.Twist import Twist # noqa: E402+from dimos.msgs.geometry_msgs.Vector3 import Vector3 # noqa: E402+from dimos.msgs.nav_msgs.Path import Path # noqa: E402+from moss_dimos.recovering_planner import RecoveringGlobalPlanner # noqa: E402++FOLLOWER_SPEED_MPS = 0.30 # what the fake follower puts on cmd_vel once started+++class FakeLocalPlanner:+ def __init__(self) -> None:+ self.cmd_vel: Subject = Subject()+ self.stopped = 0+ self.starts = 0+ self.events: list[str] = []+ self.sent: list[Twist] = []+ self._lock = threading.Lock()+ self._state = "path_following"++ def stop_planning(self) -> None:+ self.stopped += 1+ self.events.append("stop")++ def start_planning(self, path) -> None:+ # the real follower drives cmd_vel from here on; one forward twist is+ # enough to show up as a second writer on the escape's Subject+ self.starts += 1+ self.events.append("start")+ self.cmd_vel.on_next(Twist(linear=Vector3(FOLLOWER_SPEED_MPS, 0.0, 0.0)))++ def get_state(self):+ from dimos.navigation.base import NavigationState+ return NavigationState.IDLE+++def make_planner(moves: bool):+ planner = RecoveringGlobalPlanner(GlobalConfig())+ fake = FakeLocalPlanner()+ planner._local_planner = fake+ planner._current_odom = PoseStamped(position=Vector3(2.0, 1.0, 0.0))+ last = [time.perf_counter()]++ def integrate(tw: Twist) -> None:+ fake.sent.append(tw)+ now = time.perf_counter()+ dt, last[0] = now - last[0], now+ if moves and tw.linear.x != 0.0:+ with planner._lock:+ p = planner._current_odom.position+ planner._current_odom = PoseStamped(+ position=Vector3(p.x + tw.linear.x * dt, p.y, 0.0)+ )++ fake.cmd_vel.subscribe(integrate)+ return planner, fake+++def straight_path(start: Vector3, goal: Vector3) -> Path:+ return Path(poses=[+ PoseStamped(position=Vector3(start.x + (goal.x - start.x) * f,+ start.y + (goal.y - start.y) * f, 0.0))+ for f in (0.0, 0.5, 1.0)+ ])+++def plannable(planner) -> None:+ """No costmap on a cold bench: every goal gets a straight path."""+ planner._find_safe_goal = lambda goal: goal+ planner._find_wide_path = lambda goal, robot: straight_path(robot, goal)+++def wait_until(predicate, timeout: float = 4.0) -> bool:+ t0 = time.perf_counter()+ while not predicate() and time.perf_counter() - t0 < timeout:+ time.sleep(0.02)+ return predicate()+++def wait_escape_done(planner, timeout: float = 8.0) -> None:+ t0 = time.perf_counter()+ while planner._escaping and time.perf_counter() - t0 < timeout:+ time.sleep(0.02)+ assert not planner._escaping, "escape never finished"+++def test_stuck_abandons_goal_without_any_motion() -> None:+ planner, fake = make_planner(moves=True)+ failed = []+ planner.goal_reached.subscribe(lambda b: failed.append(bool(b.data)))+ planner._current_goal = PoseStamped(position=Vector3(4.0, 1.0, 0.0))+ planner._path_started_at = time.perf_counter() - 30.0 # has been driving a path+ planner._position_tracker.is_stuck = lambda: True # type: ignore[method-assign]+ planner._replan_path()+ assert fake.sent == [], "stuck must not command ANY motion"+ assert planner._current_goal is None, "the goal must be abandoned"+ assert failed == [False], "goal_reached=False is what makes the explorer exclude the spot"+ print(" stuck -> zero twists, goal abandoned, goal_reached=False published")+++def test_stuck_needs_a_driven_path_and_ignores_rotation() -> None:+ calls = []+ planner, fake = make_planner(moves=True)+ planner._current_goal = PoseStamped(position=Vector3(4.0, 1.0, 0.0))+ planner._position_tracker.is_stuck = lambda: True # type: ignore[method-assign]+ base = RecoveringGlobalPlanner.__mro__[1]+ base_replan = base._replan_path+ base._replan_path = lambda self: calls.append("replan") # type: ignore[method-assign]+ try:+ planner._path_started_at = float("inf") # fresh goal, no path driven yet+ planner._replan_path()+ finally:+ base._replan_path = base_replan # type: ignore[method-assign]+ assert calls == ["replan"] and planner._current_goal is not None, calls+ print(" stuck but not driving a path -> normal replan, goal kept")++ calls.clear()+ planner._path_started_at = time.perf_counter() - 30.0+ fake._state = "initial_rotation" # turning in place: position still, not stuck+ resets = []+ planner._position_tracker.reset_data = lambda: resets.append(1) # type: ignore[method-assign]+ planner._replan_path()+ assert calls == [] and planner._current_goal is not None, calls+ assert resets, "rotation must RESTART the stuck tracker window (else a 1 Hz replan wiggle)"+ planner._in_stop_message = True # but a follower stop message is handled+ planner._replan_path()+ planner._in_stop_message = False+ assert planner._current_goal is None, "stuck while driving (stop message path) -> abandoned"+ fake._state = "path_following"+ print(" rotating in place -> ignored; follower stop message -> handled")+++def test_escape_moves_away_then_abandons() -> None:+ planner, fake = make_planner(moves=True)+ failed = []+ planner.goal_reached.subscribe(lambda b: failed.append(bool(b.data)))+ planner._current_goal = PoseStamped(position=Vector3(4.0, 1.0, 0.0))+ t0 = time.perf_counter()+ assert planner.escape(direction=-1.0, trigger="bump") is True+ wait_escape_done(planner)+ dt = time.perf_counter() - t0+ assert fake.stopped >= 1, "local follower must be stopped before the escape"+ assert all(tw.linear.x <= 0.0 for tw in fake.sent), "a bump escapes in REVERSE"+ assert fake.sent[-1].linear.x == 0.0, "must end with a zero twist"+ assert planner._current_odom.position.x < 1.85, "must have moved ~0.20 m away"+ assert 1.5 < dt < 4.0, dt+ assert planner._current_goal is None and failed == [False], "escape ends by abandoning the goal"+ print(f" bump -> escaped {2.0 - planner._current_odom.position.x:.2f} m in {dt:.1f} s, goal abandoned")+++def test_escape_times_out_when_not_moving() -> None:+ planner, fake = make_planner(moves=False)+ planner.escape_timeout_s = 1.0+ assert planner.escape(direction=+1.0, trigger="bench") is True+ wait_escape_done(planner)+ assert all(tw.linear.x >= 0.0 for tw in fake.sent), "a forward escape only drives forward"+ assert fake.sent[-1].linear.x == 0.0+ print(" blocked escape times out and ends with a zero twist")+++def test_second_contact_aborts_the_escape() -> None:+ planner, fake = make_planner(moves=False) # never reaches 0.20 m on its own+ planner.escape_timeout_s = 5.0+ t0 = time.perf_counter()+ assert planner.escape(direction=-1.0, trigger="bump") is True+ time.sleep(0.15)+ assert planner.escape(direction=+1.0, trigger="bump") is False, \+ "a second contact must ABORT, not start a new scripted move"+ wait_escape_done(planner)+ dt = time.perf_counter() - t0+ assert dt < 2.0, f"abort must stop the escape early, took {dt:.1f} s"+ assert fake.sent[-1].linear.x == 0.0+ print(f" second contact aborted the escape after {dt:.2f} s, wheels zeroed")+++def test_stuck_is_silent_during_an_escape() -> None:+ planner, fake = make_planner(moves=False)+ planner.escape_timeout_s = 1.0+ planner._current_goal = PoseStamped(position=Vector3(4.0, 1.0, 0.0))+ planner._path_started_at = time.perf_counter() - 30.0+ planner._position_tracker.is_stuck = lambda: True # type: ignore[method-assign]+ assert planner.escape(direction=-1.0, trigger="bump") is True+ planner._replan_path() # the monitor fires mid-escape+ assert planner._escaping, "the escape must keep the wheels"+ wait_escape_done(planner)+ print(" monitor stuck check during an escape -> ignored")+++def test_goal_during_an_escape_is_held_then_planned() -> None:+ """The explorer wakes 3 s into a 5 s escape and sends a goal. Without the+ hold there are two writers on cmd_vel, then the escape cancels the fresh+ goal and publishes goal_reached=False - the explorer excludes a frontier+ it never drove to. Same timeline here, 1 s escape."""+ planner, fake = make_planner(moves=False) # wedged: the escape runs its full timeout+ plannable(planner)+ failed = []+ planner.goal_reached.subscribe(lambda b: failed.append(bool(b.data)))+ planner._current_goal = PoseStamped(position=Vector3(4.0, 1.0, 0.0)) # the goal being driven+ planner.escape_timeout_s = 1.0++ assert planner.escape(direction=-1.0, trigger="bump") is True+ time.sleep(0.3) # mid-escape, as the explorer wakes up+ frontier = PoseStamped(position=Vector3(6.0, 1.0, 0.0))+ planner.handle_goal_request(frontier)++ assert fake.starts == 0, "the follower must NOT be started while the escape owns the wheels"+ assert planner._held_goal is frontier, "the goal is held, not dropped"+ time.sleep(0.2) # let the escape keep writing+ assert [tw for tw in fake.sent if tw.linear.x > 0.0] == [], \+ "no forward follower twist may interleave with the reverse escape"++ wait_escape_done(planner)+ assert wait_until(lambda: fake.starts == 1), "the held goal must be planned once the escape ends"+ assert planner._current_goal is frontier, "the held goal becomes the current goal"+ assert planner._held_goal is None+ assert failed == [], "no goal_reached=False for a goal the rover never drove"+ assert fake.events.count("start") == 1 and fake.events[-1] == "start"+ print(" goal mid-escape -> zero follower twists during the escape, planned after, no false failure")+++def test_goal_outside_an_escape_is_planned_at_once() -> None:+ planner, fake = make_planner(moves=False)+ plannable(planner)+ goal = PoseStamped(position=Vector3(4.0, 1.0, 0.0))+ planner.handle_goal_request(goal)+ assert fake.starts == 1 and planner._current_goal is goal, "unchanged outside an escape"+ assert getattr(planner, "_held_goal", None) is None # getattr: this case also runs pre-fix+ assert [round(tw.linear.x, 2) for tw in fake.sent] == [FOLLOWER_SPEED_MPS], \+ "the follower is alone on cmd_vel"+ print(" goal outside an escape -> planned immediately, follower alone on cmd_vel")+++def test_a_plan_racing_the_escape_start_never_leaves_the_follower_running() -> None:+ planner, fake = make_planner(moves=False)+ plannable(planner)+ planner._current_goal = PoseStamped(position=Vector3(4.0, 1.0, 0.0))++ planner._escaping = True # the escape took the wheels first+ planner._plan_path()+ assert fake.starts == 0, "a replan during an escape must not start the follower"+ assert planner._held_goal is planner._current_goal++ planner._escaping = False # ... and the other way round:+ planner._held_goal = None+ def contact_while_astar_runs(goal, robot): # a contact arrives while A* runs+ planner._escaping = True+ return straight_path(robot, goal)+ planner._find_wide_path = contact_while_astar_runs+ planner._plan_path()+ assert fake.events[-1] == "stop", "a follower started under a contact must be stopped again"+ assert planner._held_goal is planner._current_goal+ planner._escaping = False+ print(" plan racing an escape -> follower never left running, goal held")+++if __name__ == "__main__":+ for t in (test_stuck_abandons_goal_without_any_motion,+ test_stuck_needs_a_driven_path_and_ignores_rotation,+ test_escape_moves_away_then_abandons,+ test_escape_times_out_when_not_moving,+ test_second_contact_aborts_the_escape,+ test_stuck_is_silent_during_an_escape,+ test_goal_during_an_escape_is_held_then_planned,+ test_goal_outside_an_escape_is_planned_at_once,+ test_a_plan_racing_the_escape_start_never_leaves_the_follower_running):+ print(t.__name__); t()+ print("TEST PASSED")diff --git a/benches/bench_robot_profile_cold.py b/benches/bench_robot_profile_cold.pynew file mode 100644index 0000000..4433eb8--- /dev/null+++ b/benches/bench_robot_profile_cold.py@@ -0,0 +1,646 @@+"""Cold bench for moss_dimos.robot_profile: MOSS's body, read at call time.++Known input -> known output in physical units. Groups:++ A. the profile - MOSS's placeholders (lidar 0.20 m up, body box 0.22 x 0.18,+ body clear 0.20 x 0.16, width 0.34, pivot 0.50 -> lethal+ 0.22 m, pivot clearance 0.25 m), exactly the fields MOSS reads+ B. selection - current() is MOSS whatever ROBOT_PROFILE says (one warning+ for a foreign value), the env overrides read at call time+ (MOSS_ROBOT_WIDTH_M=0.40 -> lethal 0.25 m), a malformed value+ keeps the placeholder+ C. devices - by-id resolution of the QT Py and the CP2102N lidar bridge,+ the sentinels, the env overrides, the absolute state dir;+ env_on, the one parser of the MOSS_* on/off switches+ L. the log - outside dimOS the warnings go to the stdlib logger (stderr)+ and dimOS is never imported; inside a dimOS process they+ land in the run's main.jsonl (subprocess; needs dimOS)+ D. the lidar - rplidar_c1: a return at 0.30 m dead ahead is kept, one inside+ MOSS's body box is dropped, polar_to_xy(90 deg, 1000 mm) =+ (0, -1.0) m+ E. the map - costmap2d tags lidar returns at the profile's height (0.20 m),+ body_clear clears the profile's footprint, a stop never+ freezes the map; the planner's clearances and the odometry's+ lidar lift and depth projection follow the profile++A-C need nothing. L needs dimOS for its main.jsonl half (SKIPPED with a line+without it). D and E import the modules: with dimOS when it is there, else+behind a stand-in for every dimOS name - the functions under test are pure+numpy and never touch dimOS objects. A reader module that does not import,+or lacks a function under test, is a KO.++Run: PYTHONDONTWRITEBYTECODE=1 PYTHONPATH=. python3 benches/bench_robot_profile_cold.py+"""+import ast+import asyncio+import copy+import dataclasses+import importlib+import importlib.abc+import importlib.machinery+import logging+import math+import os+import subprocess+import sys+import tempfile+import threading+import time+import types+from pathlib import Path++ROOT = Path(__file__).resolve().parent.parent+sys.path.insert(0, str(ROOT))++import numpy as np # noqa: E402++from moss_dimos import robot_profile as rp # noqa: E402++OK = 0+KO = 0+++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+++def close(a, b, tol=1e-9):+ return abs(a - b) <= tol+++def clear_env():+ for var in [k for k in os.environ if k.startswith("MOSS_")] + ["ROBOT_PROFILE"]:+ os.environ.pop(var, None)+++def load(name):+ """moss_dimos.<name>, or None after a KO when it does not import."""+ try:+ return importlib.import_module(f"moss_dimos.{name}")+ except Exception as exc: # noqa: BLE001+ check(f"moss_dimos.{name} imports", False, f"{type(exc).__name__}: {exc}")+ return None+++class _Capture(logging.Handler):+ def __init__(self):+ super().__init__(logging.WARNING)+ self.lines = []++ def emit(self, record):+ self.lines.append(record.getMessage())+++clear_env()++# ── A. the MOSS profile ──────────────────────────────────────────────────+print("A. the MOSS profile: placeholders, derived clearances, the fields MOSS reads")+M = rp.MOSS+check("name 'moss'", M.name == "moss")+check("lidar 0.20 m up, yaw 0 deg",+ (M.lidar_height_m, M.lidar_yaw_offset_deg) == (0.20, 0.0))+check("min range 0.15 m, no mask sector, mask range 0.30 m",+ (M.scan_min_range_m, M.scan_mask_sectors_deg, M.scan_mask_range_m) == (0.15, (), 0.30))+check("body box 0.22 x 0.18, body clear 0.20 x 0.16",+ M.body_box_m == (0.22, 0.18) and M.body_clear_m == (0.20, 0.16))+check("width 0.34, pivot 0.50, margin 0.05, track 0.266 m",+ (M.robot_width_m, M.pivot_diameter_m, M.control_margin_m, M.track_m) == (0.34, 0.50, 0.05, 0.266))+check("camera (0.156, 0, 0.075) m, pitch 0",+ M.camera_xyz_base_m == (0.156, 0.0, 0.075) and M.camera_pitch_deg == 0.0)+dm = rp.derived(M)+check("lethal clearance 0.22 m, pivot clearance 0.25 m",+ close(dm.lethal_clearance_m, 0.22) and close(dm.pivot_clearance_m, 0.25),+ f"{dm.lethal_clearance_m:.4f} / {dm.pivot_clearance_m:.4f}")+check("derived == width / 2 + margin and diameter / 2, bit for bit",+ dm.lethal_clearance_m == 0.34 / 2 + 0.05 and dm.pivot_clearance_m == 0.50 / 2)+fields = {f.name for f in dataclasses.fields(rp.RobotProfile)}+want = {"name", "lidar_height_m", "lidar_yaw_offset_deg", "scan_min_range_m",+ "scan_mask_sectors_deg", "scan_mask_range_m", "body_box_m", "body_clear_m",+ "robot_width_m", "pivot_diameter_m", "control_margin_m", "track_m",+ "camera_xyz_base_m", "camera_pitch_deg"}+check("RobotProfile holds exactly the 14 fields MOSS reads", fields == want,+ f"extra {sorted(fields - want)}, missing {sorted(want - fields)}")+check("every numeric field has its MOSS_* override",+ set(rp._MOSS_ENV) == want - {"name"}, f"{sorted(set(rp._MOSS_ENV) ^ (want - {'name'}))}")+check("one robot only: no profile table, no second profile",+ not hasattr(rp, "PROFILES") and [n for n in vars(rp)+ if isinstance(getattr(rp, n), rp.RobotProfile)] == ["MOSS"])+try:+ M.lidar_height_m = 1.0 # type: ignore[misc]+ check("assigning a field raises", False, "no exception")+except Exception as exc: # dataclasses.FrozenInstanceError+ check("assigning a field raises", True, type(exc).__name__)+++# ── B. selection and env overrides, at call time ─────────────────────────+print("B. current(): MOSS, read now; env overrides; ROBOT_PROFILE ignored")+cap = _Capture()+check("outside dimOS the warnings go to the stdlib logger 'moss_dimos.robot_profile'",+ rp._log() is logging.getLogger("moss_dimos.robot_profile"))+rp._log().addHandler(cap)+check("nothing set -> MOSS", rp.current() is M)+os.environ["ROBOT_PROFILE"] = "moss"+check("ROBOT_PROFILE=moss -> MOSS, no warning", rp.current() == M and cap.lines == [])+os.environ["ROBOT_PROFILE"] = " MOSS "+check("ROBOT_PROFILE=' MOSS ' (case, spaces) -> no warning", rp.current() == M and cap.lines == [])+os.environ["ROBOT_PROFILE"] = "rover9"+p1, p2 = rp.current(), rp.current()+check("ROBOT_PROFILE=rover9 -> still MOSS", p1 == M and p2 == M)+check("... and ONE warning saying it is ignored, not one per call",+ len(cap.lines) == 1 and "ROBOT_PROFILE is ignored" in cap.lines[0], f"{cap.lines}")+os.environ.pop("ROBOT_PROFILE", None)+os.environ["MOSS_ROBOT_WIDTH_M"] = "0.40"+check("MOSS_ROBOT_WIDTH_M=0.40 -> lethal 0.25 m (set after import, read now)",+ close(rp.derived().lethal_clearance_m, 0.25), f"{rp.derived().lethal_clearance_m:.4f}")+check("... the MOSS constant itself is untouched (0.34 m)", rp.MOSS.robot_width_m == 0.34)+os.environ["MOSS_BODY_BOX_M"] = "0.25 0.20"+os.environ["MOSS_SCAN_MASK_DEG"] = "350 10, 170 190"+os.environ["MOSS_CAMERA_XYZ_M"] = "0.16,0,0.08"+p = rp.current()+check("MOSS_BODY_BOX_M='0.25 0.20' -> (0.25, 0.20)", p.body_box_m == (0.25, 0.20))+check("MOSS_SCAN_MASK_DEG='350 10, 170 190' -> two sectors", p.scan_mask_sectors_deg == ((350.0, 10.0), (170.0, 190.0)))+check("MOSS_CAMERA_XYZ_M='0.16,0,0.08' -> (0.16, 0.0, 0.08)", p.camera_xyz_base_m == (0.16, 0.0, 0.08))+os.environ["MOSS_SCAN_MASK_DEG"] = "-30 30"+check("MOSS_SCAN_MASK_DEG='-30 30' -> (330, 30): wrapped into 0..360",+ rp.current().scan_mask_sectors_deg == ((330.0, 30.0),))+os.environ["MOSS_LIDAR_HEIGHT_M"] = "twenty"+os.environ["MOSS_BODY_CLEAR_M"] = "0.3"+os.environ["MOSS_SCAN_MASK_DEG"] = "350"+p = rp.current()+check("MOSS_LIDAR_HEIGHT_M=twenty -> placeholder 0.20 kept", p.lidar_height_m == 0.20)+check("MOSS_BODY_CLEAR_M with one number -> placeholder kept", p.body_clear_m == (0.20, 0.16))+check("MOSS_SCAN_MASK_DEG with an odd count -> no sector (placeholder)", p.scan_mask_sectors_deg == ())+os.environ["MOSS_LIDAR_HEIGHT_M"] = "nan"+check("MOSS_LIDAR_HEIGHT_M=nan -> placeholder kept (finite numbers only)", rp.current().lidar_height_m == 0.20)+os.environ["MOSS_LIDAR_HEIGHT_M"] = "0.215"+check("MOSS_LIDAR_HEIGHT_M=0.215 -> 0.215 m", rp.current().lidar_height_m == 0.215)+check("an explicit env dict wins over os.environ",+ rp.current({"MOSS_LIDAR_HEIGHT_M": "0.30"}).lidar_height_m == 0.30)+clear_env()+check("env cleared -> the plain MOSS placeholders again", rp.current() == M)+logging.getLogger("moss_dimos.robot_profile").removeHandler(cap)+++# ── C. devices and state ─────────────────────────────────────────────────+print("C. devices (by-id) and the state dir")+QTPY = "usb-Adafruit_QT_Py_ESP32-S3_No_PSRAM_00:00:00:00:00:00-if00"+C1 = "usb-Silicon_Labs_CP2102N_USB_to_UART_Bridge_Controller_0000-if00-port0"+OTHER_UART = "usb-Silicon_Labs_CP2102_USB_to_UART_Bridge_Controller_0001-if00-port0"+saved_root = rp.BYID_ROOT+with tempfile.TemporaryDirectory() as d:+ rp.BYID_ROOT = d # read at call time: patching the module variable is enough+ check("nothing plugged -> both sentinels",+ rp.moss_qtpy_port({}) == rp.MOSS_QTPY_MISSING and rp.moss_lidar_port({}) == rp.MOSS_LIDAR_MISSING,+ f"{rp.moss_qtpy_port({})}, {rp.moss_lidar_port({})}")+ open(os.path.join(d, OTHER_UART), "w").close()+ check("a CP2102 (no N) bridge is never taken for the lidar", rp.moss_lidar_port({}) == rp.MOSS_LIDAR_MISSING)+ open(os.path.join(d, C1), "w").close()+ check("the CP2102N bridge -> the lidar, by-id path kept", rp.moss_lidar_port({}) == os.path.join(d, C1))+ check("... and never for the motor board", rp.moss_qtpy_port({}) == rp.MOSS_QTPY_MISSING)+ open(os.path.join(d, QTPY), "w").close()+ check("the QT Py -> the motor board", rp.moss_qtpy_port({}) == os.path.join(d, QTPY))+ check("MOSS_PORT / MOSS_LIDAR_PORT win over the by-id scan",+ rp.moss_qtpy_port({"MOSS_PORT": "/dev/ttyACM9"}) == "/dev/ttyACM9"+ and rp.moss_lidar_port({"MOSS_LIDAR_PORT": "/dev/ttyUSB7"}) == "/dev/ttyUSB7")+ check("a blank MOSS_PORT falls back to the by-id scan", rp.moss_qtpy_port({"MOSS_PORT": " "}) == os.path.join(d, QTPY))+ check("resolve_by_id with an explicit root and no match -> the sentinel given",+ rp.resolve_by_id("*nothing*", "/dev/null/x", root=d) == "/dev/null/x")+rp.BYID_ROOT = saved_root+check("the sentinels cannot be opened (no such path)",+ not os.path.exists(rp.MOSS_QTPY_MISSING) and not os.path.exists(rp.MOSS_LIDAR_MISSING))+check("state dir default ~/.local/state/moss, recordings/moss.db under it",+ rp.moss_recording_path({}) == Path.home() / ".local/state/moss/recordings/moss.db",+ str(rp.moss_recording_path({})))+rel = rp.moss_state_dir({"MOSS_STATE_DIR": "rel/state"})+check("a relative MOSS_STATE_DIR is made absolute (cwd-based)",+ rel.is_absolute() and rel == Path.cwd() / "rel/state", str(rel))+check("MOSS_STATE_DIR=/srv/moss -> /srv/moss/recordings/moss.db",+ rp.moss_recording_path({"MOSS_STATE_DIR": "/srv/moss"}) == Path("/srv/moss/recordings/moss.db"))++# env_on: MOSS_CAMERA / MOSS_WRIST / MOSS_RECORD / MOSS_RERUN / MOSS_MOCK_BUS,+# one parser for the blueprint, the adapter and the preflight+check("env_on: unset or blank -> the default given",+ rp.env_on("MOSS_X", True, {}) is True and rp.env_on("MOSS_X", False, {}) is False+ and rp.env_on("MOSS_X", True, {"MOSS_X": " "}) is True and rp.env_on("MOSS_X", False, {"MOSS_X": ""}) is False)+check("env_on: the default is True when none is given", rp.env_on("MOSS_X", env={}) is True)+_offs = ["0", "false", "no", "off", " OFF ", "False", "NO", "\t0\n"]+check(f"env_on: {_offs} -> off (any case, spaces stripped)",+ all(rp.env_on("MOSS_X", True, {"MOSS_X": v}) is False for v in _offs))+_ons = ["1", "true", "yes", "on", "y", "2", "enabled", " On "]+check(f"env_on: {_ons} -> on (anything else)",+ all(rp.env_on("MOSS_X", False, {"MOSS_X": v}) is True for v in _ons))+os.environ["MOSS_CAMERA"] = "off"+check("env_on reads os.environ by default: MOSS_CAMERA=off -> off", rp.env_on("MOSS_CAMERA", True) is False)+clear_env()+++# ── L. where the warnings go ─────────────────────────────────────────────+print("L. the warnings: stderr outside dimOS, the run's main.jsonl inside a dimOS process")+_PROBE = """+import sys+if sys.argv[1] == "dimos":+ try:+ import dimos.utils.logging_config # loaded in every dimOS process (CLI and workers)+ except ImportError as exc:+ print("NODIMOS", exc)+ raise SystemExit(0)+from moss_dimos import robot_profile as rp+p = rp.current({"ROBOT_PROFILE": "rover9", "MOSS_LIDAR_HEIGHT_M": "0,25"})+print("HEIGHT", p.lidar_height_m)+print("DIMOS_LOADED", "dimos" in sys.modules)+"""+_WANT = ("ROBOT_PROFILE is ignored: this package only knows MOSS (got 'rover9')",+ "MOSS_LIDAR_HEIGHT_M='0,25' ignored (expected 1 number(s)) - placeholder kept")+++def _probe(mode, run_dir):+ env = {k: v for k, v in os.environ.items()+ if not k.startswith("MOSS_") and k not in ("ROBOT_PROFILE", "DIMOS_RUN_LOG_DIR")}+ env["PYTHONPATH"] = str(ROOT) + os.pathsep + env.get("PYTHONPATH", "")+ env["PYTHONDONTWRITEBYTECODE"] = "1"+ env["DIMOS_RUN_LOG_DIR"] = run_dir # what a dimOS worker inherits+ return subprocess.run([sys.executable, "-c", _PROBE, mode], env=env,+ capture_output=True, text=True, timeout=300)+++with tempfile.TemporaryDirectory() as _run:+ _r = _probe("plain", _run)+ _ok = all(w in _r.stderr for w in _WANT) and "HEIGHT 0.2" in _r.stdout+ check("outside dimOS: both warnings on stderr, the placeholder kept (0.2 m)",+ _ok, "" if _ok else f"stdout {_r.stdout.strip()[-200:]!r} stderr {_r.stderr.strip()[-300:]!r}")+ _ok = "DIMOS_LOADED False" in _r.stdout and not os.path.exists(os.path.join(_run, "main.jsonl"))+ check("... and robot_profile never imports dimOS (pure: the preflight, the cockpit)",+ _ok, "" if _ok else f"stdout {_r.stdout.strip()[-200:]!r}")+with tempfile.TemporaryDirectory() as _run:+ _r = _probe("dimos", _run)+ if "NODIMOS" in _r.stdout:+ print(f" -- SKIP the main.jsonl half: no dimOS here ({_r.stdout.strip()}): run this on the robot")+ else:+ _log_file = Path(_run) / "main.jsonl"+ _events = []+ if _log_file.exists():+ import json+ for _line in _log_file.read_text().splitlines():+ try:+ _events.append(json.loads(_line).get("event", ""))+ except ValueError:+ pass+ _ok = all(w in _events for w in _WANT)+ check("inside a dimOS process: both warnings land in the run's main.jsonl",+ _ok, "" if _ok else f"events {_events} stderr {_r.stderr.strip()[-300:]!r}")+ _ok = "HEIGHT 0.2" in _r.stdout+ check("... with the placeholder kept (0.2 m)", _ok, "" if _ok else f"stdout {_r.stdout.strip()[-200:]!r}")+++# ── D/E need the modules: real dimOS, or a stand-in for every dimOS name ──++class _Stub:+ """Any dimOS class / function / decorator: constructible, subscriptable,+ subclassable; never called by the functions under test."""++ def __init__(self, *args, **kwargs):+ pass++ def __class_getitem__(cls, item):+ return cls++ def __call__(self, *args, **kwargs):+ return args[0] if len(args) == 1 and callable(args[0]) else self++ def __getattr__(self, name):+ return lambda *a, **k: None+++class _StubModule(types.ModuleType):+ def __getattr__(self, name):+ if name.startswith("__"):+ raise AttributeError(name)+ cls = type(name, (_Stub,), {})+ setattr(self, name, cls)+ return cls+++class _StubFinder(importlib.abc.MetaPathFinder, importlib.abc.Loader):+ def __init__(self, roots):+ self.roots = roots++ def find_spec(self, fullname, path, target=None):+ if fullname.split(".")[0] in self.roots:+ return importlib.machinery.ModuleSpec(fullname, self, is_package=True)+ return None++ def create_module(self, spec):+ mod = _StubModule(spec.name)+ mod.__path__ = []+ return mod++ def exec_module(self, module):+ pass+++def _install_stubs():+ """Stand-ins only for what does not import here; returns what was stubbed."""+ stubbed = []+ for root, probe in (("dimos", "dimos.core.module"), ("dimos_lcm", "dimos_lcm.std_msgs")):+ try:+ __import__(probe)+ except Exception: # noqa: BLE001 - absent, or present without its deps+ for name in [n for n in sys.modules if n == root or n.startswith(root + ".")]:+ del sys.modules[name]+ stubbed.append(root)+ if stubbed:+ sys.meta_path.insert(0, _StubFinder(set(stubbed)))+ return stubbed+++stubbed = _install_stubs()+if stubbed:+ print(f" (stand-ins for {', '.join(stubbed)}: not importable here)")+++def bare(cls, **kwargs):+ """An instance with the class's own __init__ run and dimOS's base+ __init__ skipped: the fields as the module lays them, no runtime."""+ base = cls.__mro__[1]+ saved = base.__dict__.get("__init__")+ base.__init__ = lambda self, *a, **k: None+ try:+ return cls(**kwargs)+ finally:+ if saved is None:+ del base.__init__+ else:+ base.__init__ = saved+++print("D. rplidar_c1 reads the MOSS profile")+rpl = load("rplidar_c1")+if rpl is not None:+ x, y = rpl.polar_to_xy(90.0, 1000.0)+ check("polar_to_xy(90 deg, 1000 mm), yaw 0 -> (0, -1.0) m (SLAMTEC clockwise: y negated)",+ abs(x) < 1e-6 and abs(y + 1.0) < 1e-6, f"({x:.9f}, {y:.9f})")+ x, y = rpl.polar_to_xy(0.0, 2000.0)+ check("polar_to_xy(0 deg, 2000 mm) -> (2.0, 0) m: no mounting term", close(x, 2.0) and close(y, 0.0, 1e-9))+ os.environ["MOSS_LIDAR_YAW_DEG"] = "90"+ x, y = rpl.polar_to_xy(270.0, 1000.0)+ check("MOSS_LIDAR_YAW_DEG=90: 270 deg raw + 90 = the nose, (1.0, 0) m (read at call time)",+ close(x, 1.0, 1e-6) and abs(y) < 1e-6, f"({x:.6f}, {y:.6f})")+ os.environ.pop("MOSS_LIDAR_YAW_DEG", None)+ scan = [(15, 0.0, 300.0), # 0.30 m dead ahead+ (15, 0.0, 250.0), # 0.25 m dead ahead+ (15, 0.0, 200.0), # 0.20 m dead ahead: inside MOSS's body box (0.22)+ (15, 90.0, 100.0), # 0.10 m: under the 0.15 m floor everywhere+ (15, 180.0, 1500.0)] # 1.5 m behind: a wall+ pts_m = rpl.scan_to_points(scan, 0)+ ahead = sorted(round(p[0], 3) for p in pts_m if abs(p[1]) < 1e-6 and p[0] > 0)+ check("the 0.30 m and 0.25 m returns ahead are kept", ahead == [0.25, 0.3], f"kept ahead {ahead}")+ check("the 0.20 m return (inside the 0.22 x 0.18 body box) and the 0.10 m one are dropped",+ len(pts_m) == 3, f"{len(pts_m)} points: {[(round(a, 3), round(b, 3)) for a, b, _ in pts_m]}")+ check("the 1.5 m wall behind -> (-1.5, 0) m", any(close(p[0], -1.5, 1e-6) and abs(p[1]) < 1e-6 for p in pts_m))+ check("an explicit profile argument gives the same 3 points",+ len(rpl.scan_to_points(scan, 0, profile=rp.MOSS)) == 3)+ os.environ["MOSS_BODY_BOX_M"] = "0.18 0.18"+ n_small = len(rpl.scan_to_points(scan, 0))+ os.environ.pop("MOSS_BODY_BOX_M", None)+ check("MOSS_BODY_BOX_M='0.18 0.18' -> the 0.20 m return is kept too (4 points, env read per scan)",+ n_small == 4, f"{n_small} points")+ os.environ["MOSS_SCAN_MASK_DEG"] = "348 12"+ n_mask = len(rpl.scan_to_points(scan, 0))+ os.environ.pop("MOSS_SCAN_MASK_DEG", None)+ check("MOSS_SCAN_MASK_DEG='348 12' (under 0.30 m) -> the 0.25 m return is dropped too (2 points)",+ n_mask == 2, f"{n_mask} points")+ check("an explicit mask wins over the profile (348..12 deg under 0.30 m drops the 0.25 m return)",+ len(rpl.scan_to_points(scan, 0, mask=((348.0, 12.0),), mask_range_m=0.30, profile=rp.MOSS)) == 2)+ src = (ROOT / "moss_dimos/rplidar_c1.py").read_text()+ check("scan_to_points reads the body box from the profile, no literal",+ "body_box_m" in src and "> 0.33 or" not in src)+++print("E. costmap2d / recovering_planner / lidar_odometry read the profile")+cmod = load("costmap2d")+if cmod is not None:+ cloud = np.array([[1.0, 0.0, np.float32(0.37)], [2.0, 0.0, np.float32(0.20)],+ [3.0, 0.0, 0.25]], dtype=np.float32)+ check("the z=0.20 point (float32) is the lidar return, 0.37 and 0.25 are not",+ list(cmod.lidar_returns(cloud)) == [False, True, False])+ os.environ["MOSS_LIDAR_HEIGHT_M"] = "0.37"+ check("MOSS_LIDAR_HEIGHT_M=0.37 -> now the z=0.37 point is the lidar return (read at call time)",+ list(cmod.lidar_returns(cloud)) == [True, False, False])+ os.environ.pop("MOSS_LIDAR_HEIGHT_M", None)++ def cleared(hx=None, hy=None):+ """Cells body_clear marks at pose (0, 0, 0) on a 4 m grid; with hx, hy+ given, the reference formula with those literals instead+ (arange(-hx, hx + 1e-9, res / 2) x arange(-hy, hy + 1e-9, res / 2))."""+ g = cmod.ScoredGrid(resolution=0.05, span_m=4.0)+ if hx is None:+ g.body_clear((0.0, 0.0, 0.0))+ return g.seen+ lx = np.arange(-hx, hx + 1e-9, g.res / 2)+ ly = np.arange(-hy, hy + 1e-9, g.res / 2)+ bx, by = np.meshgrid(lx, ly)+ gx, gy = g.cell(bx.ravel(), by.ravel())+ seen = np.zeros_like(g.seen)+ seen[gy, gx] = True+ return seen++ moss_cells = cleared()+ check("body_clear = a 0.20 x 0.16 footprint, cell for cell",+ np.array_equal(moss_cells, cleared(0.20, 0.16)), f"{int(moss_cells.sum())} cells")+ os.environ["MOSS_BODY_CLEAR_M"] = "0.31 0.23"+ big_cells = cleared()+ os.environ.pop("MOSS_BODY_CLEAR_M", None)+ check("MOSS_BODY_CLEAR_M='0.31 0.23' -> a 0.31 x 0.23 footprint, cell for cell",+ np.array_equal(big_cells, cleared(0.31, 0.23)), f"{int(big_cells.sum())} cells")+ check("the default body clears less floor than the 0.31 x 0.23 override",+ 0 < moss_cells.sum() < big_cells.sum(), f"{int(moss_cells.sum())} < {int(big_cells.sum())}")++ def _pose(x, y):+ return types.SimpleNamespace(position=types.SimpleNamespace(x=x, y=y, z=0.0),+ orientation=types.SimpleNamespace(x=0.0, y=0.0, z=0.0, w=1.0))++ def after_stop(handler, open_first=True):+ """The costmap as its __init__ lays it (no dimOS runtime): first odom+ at (0, 0), optionally the explore order, then `handler` (Bool True)+ when the module still has it, then the rover at (1.0, 0). Returns+ (cells written by that move, the 0.20 x 0.16 footprint at (1.0, 0))."""+ vc = bare(cmod.MossCostMap)+ asyncio.run(vc.handle_odom(_pose(0.0, 0.0)))+ if vc._grid is None:+ return None, None+ if open_first:+ asyncio.run(vc.handle_explore_cmd(types.SimpleNamespace(data=True)))+ if handler and hasattr(vc, handler):+ asyncio.run(getattr(vc, handler)(types.SimpleNamespace(data=True)))+ before = int(vc._grid.seen.sum())+ ref = copy.deepcopy(vc._grid)+ ref.body_clear((1.0, 0.0, 0.0))+ asyncio.run(vc.handle_odom(_pose(1.0, 0.0)))+ return int(vc._grid.seen.sum()) - before, int(ref.seen.sum()) - before++ if hasattr(cmod, "MossCostMap"):+ n, fp = after_stop(None)+ check("the grid is created on the first odom (no frame precondition)", n is not None)+ if n is not None:+ check("explore_cmd, then the rover at (1.0, 0) -> body_clear writes its 0.20 x 0.16 footprint",+ n == fp and n > 0, f"{n} cells written, footprint {fp}")+ for h in ("handle_stop_explore_cmd", "handle_explore_done"):+ if not hasattr(cmod.MossCostMap, h):+ check(f"MossCostMap.{h} exists", False)+ continue+ n, fp = after_stop(h)+ check(f"{h.removeprefix('handle_')} -> the map keeps writing: the move writes the footprint",+ n == fp and n > 0, f"{n} cells, footprint {fp}")+ n, fp = after_stop("handle_stop_explore_cmd", open_first=False)+ check("a stop BEFORE the map opened -> the 5 cm gate still opens it (a 1.0 m move writes)",+ n is not None and n > 0, f"{n} cells")+ else:+ check("costmap2d defines MossCostMap", False, "class not found")++rpm = load("recovering_planner")+if rpm is not None:+ lth, pv = rpm.clearances()+ check("clearances() = (0.22, 0.25) m", close(lth, 0.22) and close(pv, 0.25), f"({lth:.4f}, {pv:.4f})")+ os.environ["MOSS_ROBOT_WIDTH_M"] = "0.40"+ check("MOSS_ROBOT_WIDTH_M=0.40 -> planner lethal 0.25 m", close(rpm.clearances()[0], 0.25))+ os.environ.pop("MOSS_ROBOT_WIDTH_M", None)++ # clearance_cost_map itself, as _find_wide_path calls it (no radii): one+ # obstacle cell on a 41 x 41 grid at 0.05 m, cells read along +x. The+ # dimOS grid types and the Voronoi field get minimal stand-ins (the+ # Voronoi term is not under test: a zero field keeps lethal = 100 exact).+ class _Grid:+ def __init__(self, grid, resolution, origin=None, frame_id="world", ts=0.0):+ self.grid, self.resolution, self.origin, self.frame_id, self.ts = grid, resolution, origin, frame_id, ts++ saved_pl = (rpm.OccupancyGrid, rpm.CostValues, rpm.voronoi_gradient)+ rpm.OccupancyGrid = _Grid+ rpm.CostValues = types.SimpleNamespace(FREE=0, OCCUPIED=100, UNKNOWN=-1)+ rpm.voronoi_gradient = lambda g, max_distance: _Grid(g.grid.copy(), g.resolution)+ try:+ raw = np.zeros((41, 41), dtype=np.int8)+ raw[20, 20] = 100+ binary = _Grid(raw, 0.05)++ def row(**kw):+ return [int(v) for v in rpm.clearance_cost_map(binary, **kw).grid[20, 24:27]] # 0.20, 0.25, 0.30 m++ r = row()+ check("clearance_cost_map(): 0.20 m lethal, 0.25 m passable (lethal 0.22 m)",+ r[0] == 100 and r[1] < 100, f"costs at 0.20/0.25/0.30 m: {r}")+ r = row(lethal_m=0.30, pivot_m=0.39)+ check("explicit 0.30/0.39 radii -> 0.25 m lethal, 0.30 m passable (explicit wins)",+ r[1] == 100 and r[2] < 100, f"{r}")+ finally:+ rpm.OccupancyGrid, rpm.CostValues, rpm.voronoi_gradient = saved_pl++ # the soft STOP (stop_movement) vs a running contact escape+ class _Odom:+ position = types.SimpleNamespace(distance=lambda other: 0.0) # never moves: only abort/timeout end it++ sent = []+ g = rpm.RecoveringGlobalPlanner.__new__(rpm.RecoveringGlobalPlanner)+ g._lock = threading.Lock()+ g._escape_abort = threading.Event()+ g._escaping, g._escape_id, g._held_goal = False, 0, None+ g._current_odom = _Odom()+ g._local_planner = types.SimpleNamespace(stop_planning=lambda: None,+ cmd_vel=types.SimpleNamespace(on_next=sent.append))+ g._position_tracker = types.SimpleNamespace(reset_data=lambda: None)+ g.escape_timeout_s = 3.0+ cancels, planned = [], []+ g.cancel_goal = lambda *a, **k: cancels.append(1)+ g.handle_goal_request = planned.append+ module = types.SimpleNamespace(_planner=g)++ def wait_idle(limit_s):+ t0 = time.monotonic()+ while g._escaping and time.monotonic() - t0 < limit_s:+ time.sleep(0.01)+ return time.monotonic() - t0++ check("bump -> escape starts", g.escape(-1.0, trigger="bump") is True)+ time.sleep(0.25)+ rpm.RecoveringGlobalPlanner.handle_goal_request(g, "goal-A") # held: the escape owns the tracks+ n_before = len(sent)+ check("0.25 s into the escape: reversing (twists sent), a goal held",+ g._escaping and n_before >= 3 and g._held_goal == "goal-A", f"{n_before} twists")+ rpm.RecoveringPlanner._on_stop_movement(module, types.SimpleNamespace(data=True))+ dt = wait_idle(1.0)+ time.sleep(0.2)+ check("stop_movement -> the escape ends within 0.3 s (not its 3 s timeout)", not g._escaping and dt < 0.3,+ f"{dt:.2f} s")+ check("... at most one more reverse twist + the zero twist after the STOP, none 0.2 s later",+ len(sent) - n_before <= 2, f"{len(sent) - n_before} twists after STOP")+ check("... the held goal is dropped, never planned; the goal is cancelled",+ planned == [] and g._held_goal is None and len(cancels) >= 1, f"planned {planned}")+ sent.clear()+ check("a new contact after the STOP still escapes (abort flag cleared)", g.escape(-1.0, trigger="bump") is True)+ time.sleep(0.2)+ check("... and reverses", g._escaping and len(sent) >= 2, f"{len(sent)} twists")+ check("a second contact during an escape aborts it (returns False)",+ g.escape(+1.0, trigger="bump") is False)+ check("... and the escape stops", wait_idle(1.0) < 0.3 and not g._escaping)++lom = load("lidar_odometry")+if lom is not None:+ if hasattr(lom, "_profile"):+ check("lidar_odometry._profile(): MOSS at 0.20 m", lom._profile().lidar_height_m == 0.20)+ os.environ["MOSS_LIDAR_HEIGHT_M"] = "0.215"+ check("MOSS_LIDAR_HEIGHT_M=0.215 -> _profile() at 0.215 m (read at call time)",+ lom._profile().lidar_height_m == 0.215)+ os.environ.pop("MOSS_LIDAR_HEIGHT_M", None)+ else:+ check("lidar_odometry reads the profile through _profile()", False, "no _profile()")++ check("LidarOdometry has its depth handler (handle_depth_image)", hasattr(lom.LidarOdometry, "handle_depth_image"))+ if hasattr(lom.LidarOdometry, "handle_depth_image"):+ # handle_depth_image on one pixel: depth 1.000 m at the principal point+ # (optical (0, 0, 1)), rover at (0, 0, 0). Base point = (cos p + cx, 0,+ # -sin p + cz): MOSS (pitch 0, cam 0.156/0/0.075) -> (1.156, 0, 0.075).+ def depth_point():+ me = types.SimpleNamespace(_depth_n=lom.DEPTH_EVERY - 1, _K=(600.0, 600.0, 0.0, 0.0),+ _searching=False, pose2d=(0.0, 0.0, 0.0), _pose_hist=[],+ camera_floor=types.SimpleNamespace(publish=lambda m: None),+ world_frame="world", _depth_pts_last=0, _pending_cam_pts=None)+ msg = types.SimpleNamespace(data=np.array([[1000]], dtype=np.uint16), ts=0.0)+ saved_pc = lom.PointCloud2+ lom.PointCloud2 = types.SimpleNamespace(from_numpy=lambda arr, **k: arr)+ try:+ asyncio.run(lom.LidarOdometry.handle_depth_image(me, msg))+ finally:+ lom.PointCloud2 = saved_pc+ return None if me._pending_cam_pts is None else [float(v) for v in me._pending_cam_pts[0][0]]++ pt = depth_point()+ check("depth projection: 1 m at the principal point -> (1.156, 0, 0.075) m",+ pt is not None and all(abs(a - b) < 1e-5 for a, b in zip(pt, (1.156, 0.0, 0.075))), f"{pt}")+ os.environ["MOSS_CAMERA_PITCH_DEG"] = "6.3"+ os.environ["MOSS_CAMERA_XYZ_M"] = "0.14 0 0.32"+ pt = depth_point()+ clear_env()+ vp = math.radians(6.3)+ want_pt = (math.cos(vp) + 0.14, 0.0, -math.sin(vp) + 0.32)+ check("MOSS_CAMERA_PITCH_DEG=6.3, MOSS_CAMERA_XYZ_M='0.14 0 0.32' -> (1.1340, 0, 0.2103) m",+ pt is not None and all(abs(a - b) < 1e-5 for a, b in zip(pt, want_pt)), f"{pt}")++ # the lidar lift (world cloud z and base_link->lidar_link tf) sits inside+ # handle_pointcloud next to kiss-icp, too heavy for a cold call: pin the+ # source instead - both use sites read the ONE profile value+ lo_src = (ROOT / "moss_dimos/lidar_odometry.py").read_text()+ seg = ast.get_source_segment(lo_src, next(n for n in ast.walk(ast.parse(lo_src))+ if isinstance(n, ast.AsyncFunctionDef) and n.name == "handle_pointcloud"))+ check("handle_pointcloud: lidar_h = _profile().lidar_height_m feeds world_pts z AND the lidar_link tf",+ "lidar_h = _profile().lidar_height_m" in seg and "np.array([x, y, lidar_h])" in seg+ and "Vector3(0.0, 0.0, lidar_h)" in seg)++clear_env()+print()+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/benches/bench_rplidar_cold.py b/benches/bench_rplidar_cold.pynew file mode 100644index 0000000..1c96f3e--- /dev/null+++ b/benches/bench_rplidar_cold.py@@ -0,0 +1,439 @@+"""Cold bench: the C1 reader, the lidar polar->xyz maths, and the retry loop with no sensor.++Known input -> known output, in degrees, millimetres and metres:+ A. polar_to_xy / scan_to_points with MOSS's profile (mounting yaw 0):+ 73.0 deg at 1500 mm -> (0.43856, -1.43446) m; a canned revolution ->+ its known points.+ A2. what is dropped: the 0.15 m minimum range, the body box+ (0.22 x 0.18 m half extents), a masked sector set by MOSS_SCAN_MASK_DEG.+ B. the module's retry loop driven by a FAKE device class: sensor absent+ (the sentinel port) -> retried and logged once; on the by-id port: not+ answering -> answers -> yanked mid-scan -> back; a sensor that answers+ but sends no scan (motor dead). The "RPLIDAR C1 scanning" proof line:+ once per connection, on the first published cloud, never without one.+ Plus the rpc and stream tables.+ C. the real C1 reader (pyserial) against a port that does not exist.+ D. the C1 sample decoder on canned bytes: a sample worth 73.0 deg at+ 1.5 m, quality 15, decoded from its 5 bytes; one revolution through+ C1Lidar.iter_scans in millimetres.++Run with the robot's dimOS stack STOPPED: a live stack on the same LCM bus+starves the bench module's pub/sub and section B reads zero clouds.++Run: PYTHONPATH=. .venv/bin/python benches/bench_rplidar_cold.py+"""+import os+import shutil+import sys+import tempfile+import time+from pathlib import Path++sys.path.insert(0, str(Path(__file__).resolve().parent.parent))++# MOSS's own numbers, whatever the shell exported+for _var in [v for v in os.environ if v.startswith("MOSS_")]:+ del os.environ[_var]++from moss_dimos import robot_profile # noqa: E402+from moss_dimos import rplidar_c1 as rp # noqa: E402+from moss_dimos.rplidar_c1 import (DEFAULT_BAUDRATE, DEFAULT_PORT, RPLidarC1, # noqa: E402+ polar_to_xy, scan_to_points)++OK = 0+KO = 0+++def check(cond, label):+ global OK, KO+ print((" OK " if cond else " KO ") + label)+ if cond:+ OK += 1+ else:+ KO += 1+++def close(a, b, tol=1e-6):+ return abs(a - b) <= tol+++def wait_for(predicate, timeout=6.0, tick=0.02):+ deadline = time.monotonic() + timeout+ while time.monotonic() < deadline:+ if predicate():+ return True+ time.sleep(tick)+ return bool(predicate())+++class LogSpy:+ """Records what the module logs (dimOS's logger is structlog, not stdlib)."""++ def __init__(self):+ self.lines = []++ def _record(self, msg, *args, **kwargs):+ self.lines.append(msg % args if args else msg)++ info = warning = error = debug = _record++ def count(self, needle):+ return sum(needle in line for line in self.lines)+++# --- A. polar -> xyz, known values ----------------------------------------+# The reader yields (quality, angle_deg in [0,360), distance_mm). Convention:+# theta = angle + the profile's mounting yaw (0 on MOSS), x = d*cos(theta),+# y = -d*sin(theta) (SLAMTEC heading grows clockwise, the robot frame is CCW).+print("A. polar_to_xy / scan_to_points (MOSS profile, no device)")++x, y = polar_to_xy(73.0, 1500.0)+check(close(x, 0.43856, 1e-5) and close(y, -1.43446, 1e-5),+ f"73.0 deg, 1500 mm -> (0.43856, -1.43446) m (got ({x:.5f}, {y:.5f}))")+x, y = polar_to_xy(90.0, 2000.0)+check(close(x, 0.0) and close(y, -2.0), f"90 deg, 2000 mm -> (0.0, -2.0) m (got ({x:.9f}, {y:.9f}))")+x, y = polar_to_xy(0.0, 1500.0)+check(close(x, 1.5) and close(y, 0.0), "0 deg, 1500 mm -> (+1.5, 0.0) m: no mounting term on MOSS")+x, y = polar_to_xy(180.0, 1000.0)+check(close(x, -1.0) and close(y, 0.0), "180 deg, 1000 mm -> (-1.0, 0.0) m")+x, y = polar_to_xy(270.0, 500.0)+check(close(x, 0.0) and close(y, 0.5), "270 deg, 500 mm -> (0.0, +0.5) m")+x, y = polar_to_xy(45.0, 1414.213562)+check(close(x, 1.0) and close(y, -1.0), "45 deg, 1414.2136 mm -> (+1.0, -1.0) m")+x, y = polar_to_xy(30.0, 4000.0)+check(close(x, 3.464101615) and close(y, -2.0), "30 deg, 4000 mm -> (+3.4641016, -2.0) m")+x, y = polar_to_xy(87.25, 2000.0, yaw_offset_deg=2.75)+check(close(x, 0.0) and close(y, -2.0), "an explicit 2.75 deg mounting yaw: 87.25 deg raw -> theta 90 -> (0.0, -2.0) m")+os.environ["MOSS_LIDAR_YAW_DEG"] = "2.75"+x, y = polar_to_xy(87.25, 2000.0)+check(close(x, 0.0) and close(y, -2.0), "MOSS_LIDAR_YAW_DEG=2.75 is read at call time: 87.25 deg raw -> (0.0, -2.0) m")+del os.environ["MOSS_LIDAR_YAW_DEG"]+x, y = polar_to_xy(90.0, 2000.0)+check(close(x, 0.0) and close(y, -2.0), "override removed -> back to yaw 0")++# One canned revolution: 3 keepers, one weak return, one invalid (distance 0).+SCAN = [(15, 0.0, 1000.0), # kept -> (+1.0, 0.0)+ (15, 90.0, 2000.0), # kept -> ( 0.0, -2.0)+ (5, 180.0, 3000.0), # dropped at min_quality 10: quality 5+ (20, 270.0, 500.0), # kept -> ( 0.0, +0.5)+ (18, 45.0, 0.0)] # dropped: distance 0 = invalid measure+EXPECTED = [(1.0, 0.0, 0.0), (0.0, -2.0, 0.0), (0.0, 0.5, 0.0)]+EXPECTED_ALL = [(1.0, 0.0, 0.0), (0.0, -2.0, 0.0), (-3.0, 0.0, 0.0), (0.0, 0.5, 0.0)] # + the weak (quality 5) return, kept by the module default++points = scan_to_points(SCAN, min_quality=10)+check(len(points) == 3, f"min_quality=10 keeps 3 of 5 measures (got {len(points)})")+check(all(close(g, e) for gp_, ep in zip(points, EXPECTED)+ for g, e in zip(gp_, ep)),+ f"points = {[tuple(round(v, 3) for v in p) for p in points]} m")+check(all(p[2] == 0.0 for p in points), "every point is flat: z = 0.0")+# At min_quality=16 only the quality-20 measure survives: the quality-18 one+# is the invalid (distance 0) return, dropped on distance regardless.+check(len(scan_to_points(SCAN, min_quality=16)) == 1,+ "raising min_quality to 16 keeps only the quality-20 return")+check(scan_to_points([], 10) == [], "an empty scan yields no points")+++# --- A2. what is dropped: minimum range, body box, masked sectors ------------+print("\nA2. minimum range, body box, masked sectors")+M = robot_profile.MOSS+check(M.scan_min_range_m == 0.15 and M.body_box_m == (0.22, 0.18) and M.scan_mask_sectors_deg == (),+ "MOSS profile: 0.15 m minimum range, 0.22 x 0.18 m body box, no masked sector")+scan = [(40, 37.0, 120.0), # 0.12 m: under the 0.15 m minimum range: dropped+ (40, 0.0, 200.0), # (+0.20, 0): inside the body box: dropped+ (40, 90.0, 170.0), # (0, -0.17): inside the body box: dropped+ (40, 0.0, 250.0), # (+0.25, 0): outside the box: kept+ (40, 90.0, 190.0), # (0, -0.19): just outside |y| 0.18: kept+ (40, 20.0, 450.0), # 0.45 m: kept+ (40, 5.0, 800.0), # 0.80 m: kept+ (40, 180.0, 600.0), # (-0.60, 0): kept+ (3, 60.0, 500.0), (40, 60.0, 0.0)] # weak / invalid: dropped+pts = scan_to_points(scan, min_quality=10)+check(len(pts) == 5, f"minimum range and body box drop 3, weak/invalid drop 2, 5 real points kept ({len(pts)})")+check(any(close(p[0], 0.25) and close(p[1], 0.0) for p in pts), "the 0.25 m return dead ahead survives at (+0.25, 0)")+check(any(close(p[0], 0.0) and close(p[1], -0.19) for p in pts), "the 0.19 m return at 90 deg survives at (0, -0.19)")+os.environ["MOSS_SCAN_MASK_DEG"] = "348 12"+pts_m = scan_to_points(scan, min_quality=10)+check(len(pts_m) == 4 and not any(close(p[0], 0.25) and close(p[1], 0.0) for p in pts_m),+ f"MOSS_SCAN_MASK_DEG='348 12' (under 0.30 m) drops the 0.25 m return ahead, keeps the 0.80 m one at 5 deg ({len(pts_m)} kept)")+del os.environ["MOSS_SCAN_MASK_DEG"]+os.environ["MOSS_BODY_BOX_M"] = "0.30 0.18"+check(len(scan_to_points(scan, min_quality=10)) == 4, "MOSS_BODY_BOX_M='0.30 0.18' swallows the 0.25 m return too (4 kept)")+del os.environ["MOSS_BODY_BOX_M"]+check(len(scan_to_points(scan, 10, mask=((348.0, 12.0),), mask_range_m=0.30)) == 4,+ "an explicit mask argument wins over the profile's (none)")+wide = robot_profile.moss_profile({"MOSS_BODY_BOX_M": "0.30 0.30"})+check(len(scan_to_points(scan, 10, profile=wide)) == 3,+ "an explicit profile argument wins over the environment (0.30 x 0.30 box: 3 kept)")+++# --- B. retry loop on a fake device class ---------------------------------+print("\nB. retry loop (fake device, no sensor)")+check({"start", "stop"} <= set(RPLidarC1.rpcs), "RPLidarC1.rpcs carries start and stop (@rpc kept on the overrides)")+from dimos.core.coordination.blueprints import BlueprintAtom # noqa: E402+from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 # noqa: E402++streams = {(s.name, s.type): s.direction for s in BlueprintAtom.create(RPLidarC1, {}).streams}+check(streams == {("pointcloud", PointCloud2): "out"}, f"one stream: pointcloud Out[PointCloud2] ({streams})")+check(RPLidarC1.name == "rplidarc1", f"dimOS instance name {RPLidarC1.name!r}")+++class FakeLidarError(Exception):+ """Stand-in for a reader error."""+++class Bench:+ """What the test flips to move the sensor in and out."""++ def __init__(self):+ self.present = False # is the port openable?+ self.unplugged = False # yanked mid-scan?+ self.mute = False # answers on its port, but no scan ever comes (motor dead)+ self.opens = [] # (port, baudrate) per attempt+ self.shutdowns = 0+++bench = Bench()+++class FakeLidar:+ def __init__(self, port, baudrate=115200, timeout=1, logger=None):+ bench.opens.append((port, baudrate))+ if not bench.present or not os.path.exists(port):+ raise FakeLidarError(+ "Failed to connect to the sensor due to: [Errno 2] "+ "could not open port %s" % port)++ def get_info(self):+ return {"model": 65, "firmware": (1, 1)}++ def iter_scans(self):+ if bench.mute:+ raise FakeLidarError("scan stream timed out")+ while True:+ if bench.unplugged:+ raise FakeLidarError("device reports readiness to read but "+ "returned no data (port closed?)")+ yield list(SCAN)+ time.sleep(0.05)++ def _dead_port(self):+ bench.shutdowns += 1+ if bench.unplugged: # a yanked USB dongle fails every close step+ raise FakeLidarError("port is gone")++ stop = stop_motor = disconnect = _dead_port+++byid = tempfile.mkdtemp(prefix="byid_cold_") # an empty /dev/serial/by-id+saved_root = robot_profile.BYID_ROOT+robot_profile.BYID_ROOT = byid+rp.RPLidarC1.lidar_class = FakeLidar # the bench drives a fake device class+spy = LogSpy()+real_logger, rp.logger = rp.logger, spy++lidar_module = RPLidarC1(retry_period_s=0.5)+clouds = []+lidar_module.pointcloud.subscribe(clouds.append)+check(DEFAULT_PORT == robot_profile.MOSS_LIDAR_MISSING == "/dev/serial/by-id/MISSING-C1",+ f"DEFAULT_PORT is the sentinel {DEFAULT_PORT}")+check(lidar_module.port == DEFAULT_PORT and lidar_module.baudrate == 460800,+ f"no port given, nothing plugged -> {lidar_module.port} at 460800 baud")+check(DEFAULT_BAUDRATE == 460800, "DEFAULT_BAUDRATE is the C1 line rate")+os.environ["MOSS_LIDAR_PORT"] = "/dev/ttyUSB7"+check(RPLidarC1(retry_period_s=0.5).port == "/dev/ttyUSB7", "MOSS_LIDAR_PORT is read when the module is built")+del os.environ["MOSS_LIDAR_PORT"]+check(RPLidarC1(port="/dev/ttyUSB3").port == "/dev/ttyUSB3", "an explicit port wins")+lidar_module.start()++# 1. sensor absent at startup: retries, logs once, never dies.+check(wait_for(lambda: len(bench.opens) >= 3, 4.0),+ f"absent sensor -> keeps retrying ({len(bench.opens)} attempts)")+check(spy.count("RPLIDAR C1 unavailable") == 1,+ "the failure is logged ONCE, not once per retry")+check(spy.count(f"RPLIDAR C1 unavailable on {robot_profile.MOSS_LIDAR_MISSING}") == 1,+ "the log names the sentinel port")+check(lidar_module._thread.is_alive() and not clouds,+ "loop alive, nothing published while the sensor is missing")+check(lidar_module._thread.name == "rplidar-c1", f"thread name {lidar_module._thread.name!r}")+check(all(open_args == (DEFAULT_PORT, 460800) for open_args in bench.opens),+ "every attempt opens the sentinel at 460800 baud")++lidar_module.stop()+check(not lidar_module._thread.is_alive(), "a module on the sentinel stops cleanly")++# 2. a module built with the by-id port; the sensor appears later on it.+plugged = os.path.join(byid, "usb-Silicon_Labs_CP2102N_USB_to_UART_Bridge_Controller_0000-if00-port0")+open(plugged, "w").close()+check(robot_profile.moss_lidar_port() == plugged, "the CP2102N by-id link resolves as the lidar port")+bench.opens.clear()+lidar_module = RPLidarC1(retry_period_s=0.5)+lidar_module.pointcloud.subscribe(clouds.append)+check(lidar_module.port == plugged, f"no port given -> the by-id link ({os.path.basename(lidar_module.port)})")+spy.lines.clear()+lidar_module.start()+check(wait_for(lambda: len(bench.opens) >= 2, 3.0) and spy.count("RPLIDAR C1 unavailable") == 1,+ "the sensor not answering yet -> retried, logged once")+bench.present = True+check(wait_for(lambda: len(clouds) >= 2, 4.0),+ f"sensor answers -> publishes clouds ({len(clouds)} so far)")+check(spy.count(f"RPLIDAR C1 up on {plugged} @ 460800 baud") == 1,+ "logs the sensor coming up, with its port and baud rate")+check(wait_for(lambda: len(clouds) >= 4, 4.0)+ and spy.count("RPLIDAR C1 scanning: 4 points in the first turn") == 1+ and spy.count("RPLIDAR C1 scanning") == 1,+ f"'RPLIDAR C1 scanning: 4 points in the first turn' logged ONCE, not once per cloud ({len(clouds)} clouds)")+_up_at = next(i for i, line in enumerate(spy.lines) if line.startswith("RPLIDAR C1 up"))+_scan_at = next(i for i, line in enumerate(spy.lines) if line.startswith("RPLIDAR C1 scanning"))+check(_up_at < _scan_at, "the scanning line comes after the 'up' line (answers first, then spins)")+check(rp.SCANNING_LINE == "RPLIDAR C1 scanning: %d points in the first turn",+ "the proof line the launcher greps keeps its words")+xyz = clouds[0].as_numpy()[0]+# the module's default is min_quality=0: every measure with a distance is+# kept (the weak return included), only the distance-0 one is dropped+check(xyz.shape == (4, 3), f"cloud carries the 4 measures that have a distance {xyz.shape}")+check(all(close(g, e) for row, exp in zip(xyz, EXPECTED_ALL)+ for g, e in zip(row, exp)),+ f"cloud points = {[tuple(round(float(v), 3) for v in r) for r in xyz]} m")+check(clouds[0].frame_id == "lidar_link", "frame_id survives ModuleConfig")++# 3. yanked mid-scan: logged, closing steps all fail, loop keeps going.+published_before = len(clouds)+shutdowns_before = bench.shutdowns+opens_before = len(bench.opens)+bench.unplugged = True+check(wait_for(lambda: spy.count("returned no data") >= 1, 3.0),+ "unplugged mid-scan -> the reader error is logged")+check(wait_for(lambda: bench.shutdowns > shutdowns_before, 2.0),+ "the close sequence runs even though the port is gone")+check(lidar_module._thread.is_alive(), "loop survives the unplug")+check(wait_for(lambda: len(bench.opens) > opens_before, 3.0)+ and len(clouds) == published_before,+ f"re-opens the port ({opens_before} -> {len(bench.opens)}), no cloud published while unplugged")+check(all(p == plugged for p, _ in bench.opens), "every attempt opens the by-id port")++# 4. plugged back in: publishing resumes.+resumed_at = len(clouds)+bench.unplugged = False+check(wait_for(lambda: len(clouds) > resumed_at, 4.0),+ "sensor back -> publishing resumes")+check(wait_for(lambda: spy.count("RPLIDAR C1 scanning") == 2, 2.0),+ f"a new connection logs its first scan again: 2 scanning lines for 2 connections "+ f"({spy.count('RPLIDAR C1 scanning')})")++t0 = time.monotonic()+lidar_module.stop()+stop_s = time.monotonic() - t0+check(not lidar_module._thread.is_alive() and stop_s < 3.0,+ f"stop() joins the loop in {stop_s:.2f} s (retry wait is interruptible)")++# 5. a C1 that answers on its port but never sends a scan (motor dead): "up",+# no cloud, and never the scanning line - the launcher must not prove it.+bench.mute = True+spy.lines.clear()+clouds_before = len(clouds)+bench.opens.clear()+lidar_module = RPLidarC1(retry_period_s=0.2)+lidar_module.pointcloud.subscribe(clouds.append)+lidar_module.start()+check(wait_for(lambda: len(bench.opens) >= 3, 3.0) and spy.count("RPLIDAR C1 up") >= 2,+ f"motor dead: the C1 answers ('up' x{spy.count('RPLIDAR C1 up')}), its scan stream times out, retried")+check(spy.count("RPLIDAR C1 unavailable") >= 1 and spy.count("scan stream timed out") >= 1,+ "the log names the cause (scan stream timed out)")+check(spy.count("RPLIDAR C1 scanning") == 0 and len(clouds) == clouds_before,+ "no cloud and NO scanning line: 'up' alone never proves a lidar that does not spin")+lidar_module.stop()+check(not lidar_module._thread.is_alive(), "stops cleanly")+bench.mute = False++rp.logger = real_logger+rp.RPLidarC1.lidar_class = None+robot_profile.BYID_ROOT = saved_root+shutil.rmtree(byid, ignore_errors=True)+++# --- C. the real reader, against a port that does not exist ---------------+print("\nC. real C1 reader (pyserial) on a missing port")+spy = LogSpy()+real_logger, rp.logger = rp.logger, spy+absent = RPLidarC1(port="/dev/ttyUSB_moss_cold_test", retry_period_s=0.5)+absent.start()+check(wait_for(lambda: spy.count("RPLIDAR C1 unavailable") == 1, 4.0),+ "missing device -> the open error is caught and logged once")+check(any("No such file or directory" in line for line in spy.lines),+ "the log names the real cause")+check(absent._thread.is_alive(), "module still running with no sensor")+t0 = time.monotonic()+absent.stop()+check(not absent._thread.is_alive() and time.monotonic() - t0 < 3.0,+ "clean stop with the sensor never present")+rp.logger = real_logger+++# --- D. the C1 sample decoder on canned bytes -----------------------------+print("\nD. c1_serial: 5-byte samples -> degrees and metres")+from moss_dimos import c1_serial # noqa: E402+++class FakeSerial:+ """Serves canned bytes; records what was written."""++ def __init__(self, data: bytes):+ self.data = data+ self.written = b""++ @property+ def in_waiting(self):+ return len(self.data)++ def read(self, n):+ out, self.data = self.data[:n], self.data[n:]+ return out++ def write(self, b):+ self.written += b++ def reset_input_buffer(self):+ pass+++def sample(start: bool, quality: int, angle_deg: float, dist_m: float) -> bytes:+ q6 = int(round(angle_deg * 64))+ d4 = int(round(dist_m * 1000 * 4))+ b0 = (quality << 2) | (0b01 if start else 0b10)+ return bytes([b0, ((q6 & 0x7F) << 1) | 1, q6 >> 7, d4 & 0xFF, d4 >> 8])+++check(sample(True, 15, 73.0, 1.5) == bytes([0x3D, 0x81, 0x24, 0x70, 0x17]),+ "73.0 deg, 1.5 m, quality 15, new turn = 3D 81 24 70 17")+sc = c1_serial.C1Scanner("/dev/null")+sc._ser = FakeSerial(c1_serial.SCAN_DESCRIPTOR + b"\x00" + bytes([0x3D, 0x81, 0x24, 0x70, 0x17])+ + sample(False, 20, 180.5, 0.0) + sample(False, 40, 359.0, 11.25))+gen = sc.samples()+s1, s2, s3 = next(gen), next(gen), next(gen)+check(sc._ser.written == c1_serial.SCAN, "the scan is requested with A5 20")+check(s1 == (True, 15, 73.0, 1.5), f"one lost-sync byte skipped, then (new turn, q 15, 73.0 deg, 1.5 m) ({s1})")+check(s2 == (False, 20, 180.5, 0.0), f"(same turn, q 20, 180.5 deg, 0.0 m = no return) ({s2})")+check(s3 == (False, 40, 359.0, 11.25), f"(same turn, q 40, 359.0 deg, 11.25 m) ({s3})")++rev = [sample(True, 15, 0.0, 1.0), sample(False, 15, 120.0, 2.0), sample(False, 15, 240.0, 3.0),+ sample(True, 15, 0.5, 1.0)]+lid = c1_serial.C1Lidar.__new__(c1_serial.C1Lidar)+lid._s = c1_serial.C1Scanner("/dev/null")+lid._s._ser = FakeSerial(c1_serial.SCAN_DESCRIPTOR + b"".join(rev))+first = next(lid.iter_scans(min_len=3))+check(first == [(15, 0.0, 1000.0), (15, 120.0, 2000.0), (15, 240.0, 3000.0)],+ f"C1Lidar.iter_scans: one revolution of 3 samples, distances in mm ({first})")+check(scan_to_points(first, 0) and close(scan_to_points(first, 0)[0][0], 1.0),+ "and that revolution feeds scan_to_points: (1.0, 0, 0) m first")+bad = c1_serial.C1Scanner("/dev/null")+bad._ser = FakeSerial(b"\xa5\x5a\x05\x00\x00\x40\x82")+try:+ next(bad.samples())+ check(False, "a wrong scan descriptor is refused")+except RuntimeError as exc:+ check("unexpected scan descriptor" in str(exc), f"a wrong scan descriptor is refused ({exc})")++print()+print(f"{OK} OK, {KO} KO")+print("TEST PASSED" if KO == 0 else "TEST FAILED")+raise SystemExit(1 if KO else 0)diff --git a/benches/hygiene.sh b/benches/hygiene.shnew file mode 100755index 0000000..2a492ed--- /dev/null+++ b/benches/hygiene.sh@@ -0,0 +1,132 @@+#!/usr/bin/env bash+# Public-repo gate: refuses text that does not belong in this repository.+#+# benches/hygiene.sh scan the default paths (below)+# benches/hygiene.sh <path> ... scan only these files or directories+# benches/hygiene_selftest.sh plants one defect per rule and checks each is caught+#+# Prints every hit as "file:line: [rule] text" and exits 1 on any hit, 0 when+# clean, 2 when it cannot do its job (a path that does not exist, a private+# list it cannot read, or a private pattern that does not compile): a gate that+# cannot look never says clean. Runs from anywhere: paths are taken relative+# to the repository root. This file holds the patterns, so it is the one file+# never scanned.+#+# Rules (case-sensitive unless said otherwise):+# vector the word "vector" in any case and spelling, inside identifiers+# too; the dimOS types Vector3 / LcmVector3 and "vectorised" are allowed+# date a dd/mm date+# clock a time written hours-h-minutes (two digits after the h)+# rule an internal rule number ("Rule #")+# launcher a launcher named fly.sh (tools/moss-fly.sh is fine)+# lan a private IPv4 address (loopback is fine)+# home a path under /home/<name>/ (the placeholder /home/x is fine)+# device a MAC-style id (colon or dash, any case) or a 32-hex serial (any+# case) that is not all zeros (benches use zeros for device identifiers)+# accent an accented letter (comments and messages are plain English)+# private one extended regex per line from a local file that is never+# committed: $MOSS_HYGIENE_PRIVATE when set, else+# .git/info/hygiene-private when it exists. Names, places and+# device identifiers belong there, not in this script. A line+# starting with "!" is an allowance instead: a hit whose text+# matches it is not reported (e.g. the copyright line in NOTICE).+set -u+export LC_ALL=C.UTF-8 # the accent rule reads UTF-8 letters, not bytes+cd "$(dirname "$0")/.." || exit 2++if [ "$#" -gt 0 ]; then+ PATHS=("$@")+ for p in "${PATHS[@]}"; do+ [ -e "$p" ] || { echo "hygiene: no such file or directory: $p (nothing scanned)" >&2; exit 2; }+ done+else+ PATHS=()+ for p in moss_dimos tools benches docs Software.md README.md pyproject.toml NOTICE; do+ [ -e "$p" ] && PATHS+=("$p")+ done+fi++GREP=(grep -rnHI --exclude=hygiene.sh --exclude-dir=__pycache__ --exclude-dir=.git)+HITS=0++report() { # $1 = rule; stdin = grep lines "file:line:text"+ local line+ while IFS= read -r line; do+ [ -n "$line" ] || continue+ local file=${line%%:*} rest=${line#*:}+ local num=${rest%%:*} text=${rest#*:}+ printf '%s:%s: [%s] %s\n' "$file" "$num" "$1" "$(printf '%s' "$text" | sed -E 's/^[[:space:]]+//' | cut -c1-200)"+ HITS=$((HITS + 1))+ done+}++# Lines of the grep for $2 whose text still matches $2 once $3 (allowed tokens) is removed.+filtered() { # $1 = grep flag (-i or -E), $2 = pattern, $3 = sed ERE of allowed tokens+ local line text+ "${GREP[@]}" "$1" -e "$2" -- "${PATHS[@]}" 2>/dev/null | while IFS= read -r line; do+ text=${line#*:}; text=${text#*:}+ if printf '%s' "$text" | sed -E "s#$3##g" | grep -q "$1" -e "$2"; then+ printf '%s\n' "$line"+ fi+ done+}++rule() { # $1 = rule, $2 = ERE+ report "$1" < <("${GREP[@]}" -E -e "$2" -- "${PATHS[@]}" 2>/dev/null)+}++report vector < <(filtered -i 'vector' 'LcmVector3|Vector3|[Vv]ectoris(ed|ing|ation)')+rule date '\b[0-3][0-9]/[01][0-9]\b'+rule clock '\b[0-9]{1,2}h[0-5][0-9]\b'+rule rule 'Rule #'+rule launcher '(^|[^[:alnum:]_-])fly\.sh\b'+rule lan '\b(192\.168|10\.[0-9]{1,3}|172\.(1[6-9]|2[0-9]|3[01]))\.[0-9]{1,3}\.[0-9]{1,3}\b'+report home < <(filtered -E '/home/[A-Za-z0-9_.-]+' '/home/x\b')+# by-id names glue the serial to "_": no \b before it+report device < <(filtered -E '([0-9A-Fa-f]{2}[:-]){5}[0-9A-Fa-f]{2}|(^|[^0-9A-Fa-f])[0-9A-Fa-f]{32}([^0-9A-Fa-f]|$)' \+ '(00[:-]){5}00|(^|[^0-9A-Fa-f])0{32}([^0-9A-Fa-f]|$)')+rule accent '[àâäçéèêëîïôöûùüÀÂÄÇÉÈÊËÎÏÔÖÛÙÜœŒ]'++PRIVATE="${MOSS_HYGIENE_PRIVATE:-}"+if [ -z "$PRIVATE" ]; then+ gitdir=$(git rev-parse --git-dir 2>/dev/null) && [ -f "$gitdir/info/hygiene-private" ] && PRIVATE="$gitdir/info/hygiene-private"+fi+if [ -n "$PRIVATE" ]; then+ if [ ! -r "$PRIVATE" ]; then+ echo "hygiene: cannot read the private list $PRIVATE" >&2; exit 2+ fi+ NPRIV=0+ PATS=()+ ALLOW='$^' # matches no text line+ while IFS= read -r pat || [ -n "$pat" ]; do+ case "$pat" in+ ''|'#'*) ;;+ '!'*) ALLOW="$ALLOW|${pat#!}" ;;+ *) PATS+=("$pat") ;;+ esac+ done < "$PRIVATE"+ # A pattern grep cannot compile would match nothing, silently: refuse instead.+ for pat in ${PATS[@]+"${PATS[@]}"} "$ALLOW"; do+ printf '' | grep -E -e "$pat" >/dev/null 2>&1+ if [ "$?" -eq 2 ]; then+ echo "hygiene: a pattern of the private list $PRIVATE does not compile: $pat" >&2; exit 2+ fi+ done+ for pat in ${PATS[@]+"${PATS[@]}"}; do+ NPRIV=$((NPRIV + 1))+ report private < <("${GREP[@]}" -E -e "$pat" -- "${PATHS[@]}" 2>/dev/null | while IFS= read -r line; do+ text=${line#*:}; text=${text#*:}+ printf '%s' "$text" | grep -qE -e "$ALLOW" || printf '%s\n' "$line"+ done)+ done+ PRIV_NOTE="private list: $NPRIV pattern(s)"+else+ PRIV_NOTE="private list: none (set MOSS_HYGIENE_PRIVATE or create .git/info/hygiene-private)"+fi++if [ "$HITS" -gt 0 ]; then+ echo "hygiene: $HITS hit(s) in ${PATHS[*]} ($PRIV_NOTE)"+ exit 1+fi+echo "hygiene: clean - ${PATHS[*]} ($PRIV_NOTE)"+exit 0diff --git a/benches/hygiene_selftest.sh b/benches/hygiene_selftest.shnew file mode 100755index 0000000..d35a2bc--- /dev/null+++ b/benches/hygiene_selftest.sh@@ -0,0 +1,95 @@+#!/usr/bin/env bash+# Self-test of the public-repo gate (benches/hygiene.sh): it plants one defect+# per rule in scratch files, runs the gate on them and expects each one caught,+# under its own rule. It also expects the allowed tokens to pass, and the gate+# to refuse (exit 2) a path that does not exist or a private pattern that does+# not compile, instead of printing "clean".+#+# benches/hygiene_selftest.sh+#+# One line per check, then "N OK, M KO" and TEST PASSED / TEST FAILED; exit 1+# on any KO. benches/run_all.sh runs it with the benches. Every planted string+# is built from pieces at run time, so this file itself passes the gate.+set -u+export LC_ALL=C.UTF-8+cd "$(dirname "$0")/.." || exit 2+GATE=benches/hygiene.sh+T=$(mktemp -d) || exit 2+trap 'rm -rf "$T"' EXIT+OK=0; KO=0+check() { # $1 label, $2 1 = pass, $3 detail shown on a failure+ if [ "$2" = "1" ]; then OK=$((OK + 1)); echo " OK $1"+ else KO=$((KO + 1)); echo " KO $1${3:+ - $3}"; fi+}++# A private list: one made-up name and the allowance of a copyright line.+NAME="Zor""blax"+printf '%s\n' "# a test list" "$NAME" "!^Copyright 2099 $NAME" > "$T/private.txt"+gate() { MOSS_HYGIENE_PRIVATE="$T/private.txt" bash "$GATE" "$@"; }++# rule|file|text: one defect per file, on line 2+V="vec""tor"+S16="0123456789ABCDEF"+s16="0123456789abcdef"+PLANTS=(+ "$V|other_robot|the other robot: ${V^^}"+ "$V|other_package|import ${V}_dimos"+ "date|date|seen on 1""0/1""0"+ "clock|clock|at 16""h04"+ "rule|rule|see Rule"" #2"+ "launcher|launcher|run ./fl""y.sh map"+ "lan|lan_192|the rover at 192"".168.1.20"+ "lan|lan_10|over the tunnel at 10"".8.0.3"+ "home|home|cd /home""/someone/x"+ "device|mac_colon|mac a2:3c:9e"":11:47:05"+ "device|mac_dash|mac A2-3C-9E""-11-47-05"+ "device|serial_lower|usb-Bridge_${s16}${s16}-if00"+ "device|serial_upper|usb-Bridge_${S16}${S16}-if00"+ "accent|accent|caf"$'\xc3\xa9'+ "private|private|hello $NAME"+)+mkdir "$T/plant"+for p in "${PLANTS[@]}"; do+ IFS='|' read -r _rule file text <<< "$p"+ printf 'line one\n%s\n' "$text" > "$T/plant/$file.txt"+done+out=$(gate "$T/plant"); rc=$?+check "planted tree: exit 1" "$([ "$rc" = 1 ] && echo 1)" "rc $rc"+for p in "${PLANTS[@]}"; do+ IFS='|' read -r rule file text <<< "$p"+ n=$(printf '%s\n' "$out" | grep -c -F "$T/plant/$file.txt:2: [$rule]")+ check "caught under [$rule]: $file" "$([ "$n" = 1 ] && echo 1)" "$n hit(s) for it"+done+total=$(printf '%s\n' "$out" | grep -c -F "$T/plant/")+check "exactly one hit per planted file (${#PLANTS[@]})" "$([ "$total" = "${#PLANTS[@]}" ] && echo 1)" "$total hits"+check "the summary counts them" "$(printf '%s\n' "$out" | grep -q -F "hygiene: ${#PLANTS[@]} hit(s)" && echo 1)" \+ "$(printf '%s\n' "$out" | tail -1)"++# Allowed tokens: none of these is a hit.+ZERO_MAC="00:00:00:00"":00:00"+ZERO_DASH="00-00-00-00""-00-00"+Z16="0000000000000000"+cat > "$T/allowed.txt" <<ALLOWED+Vector3 and LcmVector3 are dimOS types; a vectorised loop+/home/x/moss is the placeholder home; tools/moss-fly.sh is the launcher+usb-Adafruit_QT_Py_${ZERO_MAC}-if00 and ${ZERO_DASH}+usb-Silicon_Labs_CP2102N_${Z16}${Z16}-if00-port0+a sha: ${s16}${s16}${s16}${s16}+36/36 sectors, 127.0.0.1, 8.8.8.8+Copyright 2099 $NAME+ALLOWED+out=$(gate "$T/allowed.txt"); rc=$?+check "allowed tokens: clean, exit 0" "$([ "$rc" = 0 ] && echo 1)" "rc $rc: $(printf '%s' "$out" | head -3 | tr '\n' ' ')"++# The gate fails closed.+gate "$T/no_such_dir" > /dev/null 2>&1; rc=$?+check "a path that does not exist -> exit 2, never clean" "$([ "$rc" = 2 ] && echo 1)" "rc $rc"+printf '%s\n' "$NAME(" > "$T/broken.txt"+MOSS_HYGIENE_PRIVATE="$T/broken.txt" bash "$GATE" "$T/plant/private.txt" > /dev/null 2>&1; rc=$?+check "a private pattern that does not compile -> exit 2" "$([ "$rc" = 2 ] && echo 1)" "rc $rc"+MOSS_HYGIENE_PRIVATE="$T/missing.txt" bash "$GATE" "$T/allowed.txt" > /dev/null 2>&1; rc=$?+check "a private list that cannot be read -> exit 2" "$([ "$rc" = 2 ] && echo 1)" "rc $rc"++echo "$OK OK, $KO KO"+if [ "$KO" -eq 0 ]; then echo "TEST PASSED"; exit 0; fi+echo "TEST FAILED"; exit 1diff --git a/benches/run_all.sh b/benches/run_all.shnew file mode 100755index 0000000..6f4420b--- /dev/null+++ b/benches/run_all.sh@@ -0,0 +1,59 @@+#!/usr/bin/env bash+# Run every bench (benches/bench_*.py, plus the public-repo gate's self-test+# benches/hygiene_selftest.sh) and print one line per bench.+#+# benches/run_all.sh with .venv/bin/python+# benches/run_all.sh /path/to/python with another interpreter+# LIVE=1 benches/run_all.sh ... also the live benches (bench_*_live.py)+#+# A live bench talks on the host's real LCM bus (it needs loopback multicast on+# this host, and two runs at once hear each other): it is SKIPPED with a line+# unless LIVE=1. Each bench runs from the repository root with PYTHONPATH=. (a+# PYTHONPATH already set is kept after it), no bytecode written, and never+# under PYTHONOPTIMIZE (some benches check with assert, which -O removes). A+# bench passes when it exits 0; on a failure its last lines are printed under+# its line. Exit 1 if any bench failed (or none was found), 0 otherwise.+set -u+unset PYTHONOPTIMIZE+cd "$(dirname "$0")/.." || exit 2+PY="${1:-.venv/bin/python}"+if ! command -v "$PY" >/dev/null 2>&1; then+ echo "no python at $PY - pass one: benches/run_all.sh /path/to/python"; exit 2+fi+LOGS=$(mktemp -d) || exit 2+trap 'rm -rf "$LOGS"' EXIT+FAILED=0; N=0; SKIPPED=0+for bench in benches/bench_*.py benches/*_selftest.sh; do+ [ -e "$bench" ] || continue+ name=$(basename "$bench"); name=${name%.*}+ case "$name" in+ *_live)+ if [ "${LIVE:-0}" != "1" ]; then+ printf 'SKIPPED %-40s live bench (real LCM bus): LIVE=1 runs it\n' "$name"+ SKIPPED=$((SKIPPED + 1)); continue+ fi ;;+ esac+ N=$((N + 1))+ t0=$(date +%s)+ case "$bench" in+ *.sh) bash "$bench" > "$LOGS/$name.log" 2>&1 ;;+ *) PYTHONDONTWRITEBYTECODE=1 PYTHONPATH=".${PYTHONPATH:+:$PYTHONPATH}" "$PY" "$bench" > "$LOGS/$name.log" 2>&1 ;;+ esac+ rc=$?+ dt=$(( $(date +%s) - t0 ))+ counts=$(grep -E '^[0-9]+ OK, [0-9]+ KO$' "$LOGS/$name.log" | tail -1)+ if [ "$rc" -eq 0 ]; then+ printf 'PASSED %-40s %s (%d s)\n' "$name" "${counts:-exit 0}" "$dt"+ else+ FAILED=$((FAILED + 1))+ printf 'FAILED %-40s %s (exit %d, %d s)\n' "$name" "${counts:-no count line}" "$rc" "$dt"+ tail -n 8 "$LOGS/$name.log" | sed 's/^/ | /'+ fi+done+if [ "$N" -eq 0 ]; then+ echo "no bench found under benches/"; exit 1+fi+summary="$((N - FAILED))/$N benches passed"+[ "$SKIPPED" -gt 0 ] && summary="$summary, $SKIPPED live skipped (LIVE=1 runs them)"+echo "$summary"+[ "$FAILED" -eq 0 ]diff --git a/docs/base.md b/docs/base.mdnew file mode 100644index 0000000..39d5053--- /dev/null+++ b/docs/base.md@@ -0,0 +1,96 @@+# The tracked base adapter++`moss_dimos/moss_adapter.py` lets dimOS drive MOSS: the dimOS control+coordinator and a gamepad module on top, this adapter underneath. MOSS has two+tracks on geared DC motors, driven by a QT Py ESP32-S3 running the firmware in+`firmware/MOSS_QTPY_MDD3A/`, over the board's native USB CDC at 115200 baud.++## Run it (on the robot, MOSS wired)++The gated way, with the pad receiver and the QT Py on the robot's USB:++```+$ tools/moss-teleop.sh # gates: no stack up, js0, QT Py port free, firmware disarmed, battery >= 10.8 V -> stack+$ tools/moss-teleop.sh log # what the base and the pad said+$ tools/moss-teleop.sh stop # stack down (the firmware deadman stopped the tracks 300 ms after the last drive)+$ MOCK=1 tools/moss-teleop.sh # same stack on the fake firmware, no motor+```++Pad (a shanwan receiver in Android mode): **R1 held = deadman** (SDL button 7),+left stick up/down = forward/back, right stick left/right = turn, R2 = boost+(capped at `MOSS_MAX_PCT`). The stick scale comes from the base's envelope+(`teleop_speeds`: full stick = `MOSS_MPS_AT_LIMIT`, full turn = a pivot at that+limit), so the stick stays proportional across its travel. With the default+`MOSS_TURN_GAIN` of 2, the inner track stops at half turn stick and reverses+beyond it; a gain of 1 only ever stops it. Releasing R1 sends 0.5 s of zeros,+then silence (`MOSS_BRAKE_S`, 0 to 1.0 s). Unloaded, the motors start between+5 % and 15 % duty; on the floor they need more (see `MOSS_MIN_PCT` in+[cockpit.md](cockpit.md)).++This is the dimOS path. The bench tool in `moss_pi/` drives the same firmware+without dimOS and reads raw SDL button 4 as its deadman: L1 on an Xbox-layout+pad, not on this one ([Software.md](../Software.md)).++By hand, from the root of the checkout:++```+$ ls /dev/serial/by-id/*QT_Py* # the motor board (the CP2102N next to it is the RPLidar C1)+$ export MOSS_PORT=/dev/serial/by-id/usb-Adafruit_QT_Py_ESP32-S3_..._-if00+$ export GLIBC_TUNABLES=glibc.rtld.optional_static_tls=4194304 # as the launchers do: some aarch64 (Jetson) imports fail without it; harmless elsewhere+$ .venv/bin/dimos run moss-dimos.moss-gamepad # gamepad -> tracks+$ .venv/bin/dimos run moss-dimos.moss-base # base only, cmd_vel from elsewhere+```++Without hardware: `MOSS_MOCK_BUS=1 .venv/bin/dimos run moss-dimos.moss-gamepad`+(same `GLIBC_TUNABLES` export on a Jetson) drives the fake firmware+(`moss_dimos/moss_mock.py`) end to end; the `.venv/bin/dimos log` lines+`MOSS base MOCK: twist ... -> tracks L=..% R=..%` are the proof.++Cold bench (no hardware, no dimOS): `PYTHONPATH=. python3 benches/bench_moss_adapter_cold.py`.++## What the firmware imposes, and what the adapter does about it++| Firmware (Control.h) | Adapter |+|---|---|+| `arm` puts it READY for 10 s, then it disarms itself | a drive refused `ERR not_armed` re-arms and retries once; nothing is guessed from telemetry |+| `drive L R` in percent, `-100..+100`, refused beyond | `MossGeometry.max_pct` <= 100, clamped; the twist is scaled to it |+| a drive not refreshed within 300 ms stops the tracks (`timeout`) | keepalive re-sends the last drive every 100 ms while the last twist is < 0.5 s old; beyond that one `stop`, then silence |+| any invalid line stops and disarms | only `arm` / `drive` / `stop` / `status` ever go on the wire, plus one `ramp UP DOWN` at connect when `MOSS_RAMP_MS` is set |+| telemetry JSON every 200 ms (`mode`, `ticks`, ...) | mode and encoder ticks are read from it; ticks feed odometry once calibrated |++Two deadmen, both on the robot: the adapter's (0.5 s, in the dimOS process)+and the firmware's (0.3 s, on the ESP32). A cockpit on a laptop that loses the+link stops the tracks without anyone's help.++The coordinator publishes the firmware telemetry on `/base_telemetry` every+0.5 s (JSON: `ts`, `fw`, `driver`, `mode`, `reason`, `bus_mV`, `ina_ok`,+`ticks`, `target_pct`, `output_pct`, `ramp_ms`, `firmware_age_s`, `mock`), so+nothing else ever has to open the serial port.++## Calibrate before trusting a number++The firmware is open loop (PWM percent). Three numbers turn a twist into+tracks and ticks into metres; all three ship as PLACEHOLDERS and each has an+env override, so a run can be re-scaled without a code change:++| Env | Meaning | How to measure |+|---|---|---|+| `MOSS_TRACK_M` | distance between the two track centrelines (0.266 m from the CAD) | tape measure, sprocket centre to sprocket centre |+| `MOSS_MPS_AT_LIMIT` | floor speed at `MOSS_MAX_PCT` (0.45 m/s placeholder) | full stick (or `drive 100 100`) for 3 s on the floor, tape measure, divide |+| `MOSS_TICKS_PER_M` | encoder ticks per metre of track travel (unset) | `zero`, push or drive the rover exactly 1 m, read `ticks` in a `status` line, average the two sides |++Until `MOSS_TICKS_PER_M` is set, `read_odometry()` returns `None` and+`read_velocities()` returns the commanded twist: an honest "no measurement"+rather than a pose made of guesses. Set it and both come from the encoders.++The other base knobs: `MOSS_MAX_PCT` (percent ceiling, 100 by default, never+above the firmware's 100), `MOSS_MIN_PCT` (static-friction floor, 0 = off),+`MOSS_RAMP_MS` (`"UP DOWN"`, sent as `ramp UP DOWN` at connect), `MOSS_PORT`+(the serial port; by default the QT Py's `/dev/serial/by-id` path; only a+`dimos run` by hand reads it from the shell, `tools/moss-teleop.sh` and+`tools/moss-fly.sh` always take the by-id path) and `MOSS_MOCK_BUS=1` (the fake+firmware, no motors). The other variables are listed in+[cockpit.md](cockpit.md#the-robot-profile).++`vy` (strafing) is accepted and ignored: a tracked base cannot do it, and the+stock dimOS twist joints still send it. `dof` is reported as 3 for that reason.diff --git a/docs/cockpit.md b/docs/cockpit.mdnew file mode 100644index 0000000..dd340c8--- /dev/null+++ b/docs/cockpit.md@@ -0,0 +1,298 @@+# Running MOSS: flight check, mapping, exploration, recording, cockpit++MOSS is a small tracked rover (two geared DC motors behind a QT Py ESP32-S3+motor board, an RPLidar C1, a RealSense D455F on the nose, a UVC camera on the+arm). This is the operator's manual for running it on dimOS with this+repository's `moss_dimos` package: lidar odometry, a 2D costmap, a planner and+a frontier explorer under the tracked base, with a flight check and a web+cockpit in front of them.++Everything below runs ON THE ROBOT, on the computer where the board, the lidar+and the cameras are plugged in. The cockpit is a web page served by the robot;+open it from any machine on the same network.++## The four pieces++| Piece | File | Runs where |+|---|---|---|+| Flight check | `tools/moss_preflight.py` | robot, read-only, ten seconds |+| Blueprints | `moss_dimos/blueprints.py` → `moss-dimos.moss-nav`, `moss-dimos.moss-explore` | robot, inside `dimos run` |+| Launcher | `tools/moss-fly.sh` | robot, gates then stack |+| Cockpit | `tools/moss_cockpit.py` + `tools/moss_cockpit/` | robot, port 8904 |++The pad-only launcher, `tools/moss-teleop.sh` ([base.md](base.md)), is the+smallest thing that moves the robot.++## Run it++```+$ tools/moss-fly.sh check # flight check only, nothing launched+$ tools/moss-fly.sh map # gates -> moss-nav: lidar map + pad teleop, recording+$ tools/moss-fly.sh explore # gates -> moss-explore: map + navigation + explorer, NOT armed+$ EXPLORE=1 tools/moss-fly.sh explore # ... and arm exploration once the stack is proven+$ tools/moss-fly.sh stop # stop exploration, then the stack+$ tools/moss-fly.sh log # what the base, the lidar, the odometry and the map said+$ tools/moss-fly.sh cockpit # start the cockpit server if it is not up, print the URL+$ MOCK=1 tools/moss-fly.sh map # fake motor board, no motor; the lidar must still be plugged (mapping needs it)+```++Every gate refuses rather than guesses, with the fix on the same line: a stack+already up, a missing board, a port held by another program, a mute firmware, a+battery at the BMS floor, a lidar that does not spin, a stack that does not+prove itself in the log within two minutes.++The map opens when the rover has moved 5 cm from its boot pose (the costmap's+mission gate) or when an `explore_cmd` arrives, and it stays open until the+stack stops: the cockpit's STOP, a goal reached or the end of exploration leave+map writing on. A new launch starts a fresh map.++Nothing moves by itself. `moss-nav` has no planner: the pad drives, the map+builds. `moss-explore` adds the planner, the movement manager and the frontier+explorer, but the explorer only starts on an explicit `explore_cmd` (the+launcher with `EXPLORE=1`, the cockpit's button, or+`.venv/bin/python tools/explore_ctl.py start` from the checkout root), and the+planner only on a goal (a tap on the cockpit map, or `goal_request` on the+bus). Any pad input cancels navigation (dimOS MovementManager), and two deadmen+stay underneath: the adapter stops the tracks 0.5 s after the last twist, the+firmware 300 ms after the last drive.++## What the blueprints contain++`moss-dimos.moss-nav` (mapping):++- the control coordinator (`MossControlCoordinator`) with the MOSS base adapter+ (`moss_dimos/moss_adapter.py`, see [base.md](base.md)), publishing the+ firmware telemetry on `/base_telemetry` (JSON: firmware, mode, battery mV,+ ticks, applied PWM) so nothing else ever has to open the serial port;+- the gamepad module (`moss_dimos/gamepad.py`) with the MOSS envelope, wired+ straight to `cmd_vel`;+- the RPLidar C1 reader (`moss_dimos/rplidar_c1.py`) on its own USB serial adapter;+- lidar odometry (kiss-icp, `moss_dimos/lidar_odometry.py`), the pose source:+ the encoders give metres only once `MOSS_TICKS_PER_M` is set, and no IMU+ feeds the odometry;+- the 2D scored costmap (`moss_dimos/costmap2d.py`), where the lidar writes obstacles;+- the RealSense colour stream at 640x480, 5 fps, carried as JPEG on the bus+ (`MOSS_CAMERA=0` leaves it out; `MOSS_RS_SERIAL=<serial>` picks one camera+ when several are plugged in, and the flight check looks for the same one);+- the wrist camera (the UVC camera on the arm, found by its `/dev/v4l/by-id`+ name, `MOSS_WRIST_DEV` to point at another `/dev/videoN`), 640x480, 5 fps,+ JPEG on the bus as `wrist_image` (`MOSS_WRIST=0` leaves it out; an absent+ camera is a warning and the module is skipped);+- the recorder (below), `MOSS_RECORD=0` leaves it out.++`moss-dimos.moss-explore` adds the recovering A* planner+(`moss_dimos/recovering_planner.py`) with the MOSS footprint, the movement+manager (pad input wins over navigation for one second) and Explorer2+(`moss_dimos/explorer2.py`) with the MOSS clearances. The pad is still there,+through the manager.++`moss-dimos.moss-gamepad` and `moss-dimos.moss-base` are the base alone, with+or without the pad: [base.md](base.md).++None of them starts Rerun or dimOS's own web modules: the cockpit is the+display. `MOSS_RERUN=1` adds the Rerun bridge for the desktop viewer.++The switches `MOSS_CAMERA`, `MOSS_WRIST`, `MOSS_RECORD` and `MOSS_RERUN` read+`0`, `false`, `no` and `off` (any case) as off, anything else as on, and unset+or empty as their default. The flight check reads `MOSS_CAMERA` the same way.++## The robot profile++`moss_dimos/robot_profile.py` carries MOSS's geometry: lidar height and masks,+body box, footprint, planner widths, camera mount. Every module reads it when+it needs a value, so an environment variable set before the launch applies+everywhere. The values are PLACEHOLDERS until measured on the robot:++| Value | Placeholder | Env |+|---|---|---|+| lidar scan plane above the floor | 0.20 m | `MOSS_LIDAR_HEIGHT_M` |+| lidar yaw offset (its 0 deg mark vs the nose) | 0 deg | `MOSS_LIDAR_YAW_DEG` |+| lidar returns ignored closer than | 0.15 m | `MOSS_SCAN_MIN_RANGE_M` |+| masked lidar sectors, and the range they apply within | none, 0.30 m | `MOSS_SCAN_MASK_DEG="a b, c d"`, `MOSS_SCAN_MASK_RANGE_M` |+| returns that are the robot itself (half extents) | 0.22 x 0.18 m | `MOSS_BODY_BOX_M="0.22 0.18"` |+| area cleared under the body (half extents) | 0.20 x 0.16 m | `MOSS_BODY_CLEAR_M="0.20 0.16"` |+| robot width for the planner | 0.34 m | `MOSS_ROBOT_WIDTH_M` |+| pivot diameter | 0.50 m | `MOSS_PIVOT_DIAMETER_M` |+| control margin | 0.05 m | `MOSS_CONTROL_MARGIN_M` |+| track centre distance | 0.266 m (CAD) | `MOSS_TRACK_M` |+| nose camera position on the base | 0.156, 0, 0.075 m (CAD) | `MOSS_CAMERA_XYZ_M="0.156 0 0.075"` |+| nose camera pitch | 0 deg | `MOSS_CAMERA_PITCH_DEG` |+| static-friction floor of the base adapter (percent) | 0 (off) | `MOSS_MIN_PCT` |++The speed knobs are `MOSS_MAX_PCT` (the PWM cap of the base adapter, 100 by+default) and `MOSS_NERF_SPEED` (the launcher's `--nerf-speed` for the planner,+0.5 by default).++`MOSS_MIN_PCT` is the static-friction floor: a non-zero track command below it+is raised to it, sign kept (a command that rounds to zero stays a stop). It+matters under the planner: its slowest pivot (0.2 rad/s) maps to about 6 % PWM+with the default `MOSS_TRACK_M`, `MOSS_MPS_AT_LIMIT` and `MOSS_MAX_PCT`, which+does not turn the tracks on the floor, so without a floor the rover sits+still and the planner logs `Robot is stuck. Replanning.` until the goal times+out. The floor is 0 (off) by default: find the lowest percent that turns both+tracks on the floor, and launch with+`MOSS_MIN_PCT=<that> EXPLORE=1 tools/moss-fly.sh explore`.++The lidar's floor projection is `base_link`, the rover's rotation centre: mount+the C1 there, or the footprint and the pivot clearance are off by its offset.++Other variables:++| Env | What it does |+|---|---|+| `MOSS_PORT`, `MOSS_LIDAR_PORT` | the motor board's and the lidar's serial ports; by default the QT Py's and the CP2102N's `/dev/serial/by-id` paths. Only a `dimos run` by hand reads them from the shell: `tools/moss-fly.sh` and `tools/moss-teleop.sh` always take the by-id paths |+| `MOSS_RS_SERIAL` | the RealSense to use, by serial number |+| `MOSS_REQUIRE_PAD=1` | a missing pad fails the flight check instead of warning |+| `MOSS_COCKPIT_PORT` | the cockpit's port, 8904 by default (also `--port`; `tools/moss-cockpit.service` sets it) |+| `PROOF_S` | how long `tools/moss-fly.sh` waits for the stack to prove itself in its log, 120 s by default |+| `MOSS_MOCK_BUS=1` | the fake firmware, no motors (`MOCK=1` on the launchers sets it) |+| `MOSS_STATE_DIR` | the state directory, `~/.local/state/moss` by default: recordings, the saved flight check |+| `MOSS_BRAKE_S` | how long the pad sends zeros after the deadman is released, 0.5 s by default, 0 to 1.0 s |+| `MOSS_TURN_GAIN` | turn authority against forward speed on the pad, 2 by default, 0.5 to 4 ([base.md](base.md)) |++## Recording++`MossRecorder` (`moss_dimos/memory.py`: a dimOS `Recorder` with a WAL-safe+rotation) writes the whole run into one SQLite database,+`~/.local/state/moss/recordings/moss.db` (`MOSS_STATE_DIR` moves the state+directory; a symlink to a larger disk works). Streams: `odom`,+`global_costmap`, `lidar` (world-frame cloud), `cmd_vel`, `base_telemetry`,+`color_image` (JPEG), `wrist_image` (JPEG) and `episode_marker`. The previous+run is rotated aside with its `-wal` and `-shm` files (`moss.<stamp>.db`, the+last ten kept).++The cockpit's "Record episode" button does not start or stop the recorder: it+writes a marker (`{"action": "start"|"stop", "name": ..., "ts": ...}`) on the+`episode_marker` stream. dimOS's `dataprep` cuts episodes on its own `status`+stream or on hand-entered ranges, not on these markers; cutting on them takes a+small script of your own. Reading a recording without the robot:++```+$ .venv/bin/dimos mem summary ~/.local/state/moss/recordings/moss.db+$ .venv/bin/dimos dataprep build --source <db> --format lerobot --output <dir> --config <json>+```++`--config` is required: a JSON `DataPrepConfig` naming the observation and action+streams (for MOSS: `color_image`, `odom`, `cmd_vel`) and the episode extractor.++## The cockpit++`tools/moss_cockpit.py` is a stdlib HTTP server on port 8904 (`MOSS_COCKPIT_PORT`+or `--port`) that taps the+dimOS bus (explicit LCM channels, decoded on request, never a serial port) and+serves one page, `tools/moss_cockpit/index.html`, in the MOSS brand (Space+Grotesk, studio blue, mint accent, salmon counterpoint; fonts and marks served+locally, no CDN, the field has no internet).++Panels: flight check (the last `moss_preflight.py --json`), base (battery on a+3S scale, mode, applied PWM per track, ticks, firmware), system (CPU, RAM,+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.++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+(300 ms) and `tools/moss-fly.sh stop`.++A goal tapped on the map goes out at once when nothing is exploring. While+exploring, the page stops the explorer first and sends the goal three seconds+later, after the explorer's own last goal, which would override it otherwise.+The cockpit refuses goals, exploration and launches when the running stack is+not a MOSS one (its blueprint name must start with `moss`), so another dimOS+stack sharing the computer and the bus never receives its commands; STOP is+always allowed. Commands that publish on the bus need the stack on LCM+(`DIMOS_TRANSPORT`, else `TRANSPORT`, set to `lcm`; the launchers export both,+`lcm` by default) and are refused with the reason otherwise. Every `POST /cmd/*` must carry the header+`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+`POST /cmd/{explore,goal,stop,episode,stack}`.+`tools/moss_cockpit.py --demo` serves synthetic data for working on the page+without a robot.++## The cockpit as a service++By hand, `tools/moss-fly.sh cockpit` starts the server and prints its address;+the launcher also restarts a server older than `tools/moss_cockpit.py`, and+refuses one that runs from another checkout, naming its directory. To have it+start with the robot, install `tools/moss-cockpit.service`. The unit is a+template: `MOSS_USER` is the account that runs the stack, `MOSS_DIR` the+absolute path of this checkout (with its `.venv`). From the root of the+checkout:++```+$ sed -e "s|MOSS_USER|$USER|g" -e "s|MOSS_DIR|$PWD|g" tools/moss-cockpit.service \+ | sudo tee /etc/systemd/system/moss-cockpit.service >/dev/null+$ sudo systemctl daemon-reload && sudo systemctl enable moss-cockpit && sudo systemctl restart moss-cockpit+```++`restart`, not `enable --now`: `enable --now` leaves a cockpit that is already+running as it is, still serving from its old checkout. `KillMode=process` keeps+a stack launched from the page alive when the cockpit restarts; stop a flight+with `tools/moss-fly.sh stop`. The installed unit holds absolute paths: after+moving the checkout, stop any flight (`tools/moss-fly.sh stop`), run the two+lines above again from the new place, and check that the running cockpit is+this checkout: `readlink /proc/$(systemctl show -p MainPID --value moss-cockpit)/cwd`+prints its path.++A stack launched from the page needs the same LCM settings as one launched from+a shell ([Software.md, First run](../Software.md#first-run-system-settings)): either+passwordless sudo for the stack's user, or `tools/lcm-loopback.service`, which+turns on loopback multicast, adds the `224.0.0.0/4` route on `lo` and raises the+socket buffers once at boot:++```+$ sudo cp tools/lcm-loopback.service /etc/systemd/system/+$ sudo systemctl daemon-reload && sudo systemctl enable --now lcm-loopback+```++## Limits++- The geometry and the base numbers are placeholders until measured: ground+ speed, ticks per metre, track width, lidar pose, the footprint and the+ static-friction floor. [base.md](base.md) says how to measure the base's.+- The lidar odometry runs without a gyro prior (`use_gyro_prior=False`); the+ D455F IMU could feed it once the camera mount is measured, and there is no+ switch for it.+- Every launch starts a fresh map: no map is saved between runs and there are+ no keep-out zones. The recording keeps the run.+- A recording is not replayed through the stack; the SQLite file is readable+ with the dimOS tools above.+- The planner's slowest pivot (0.2 rad/s, about 6 % PWM at the defaults) sits+ inside the motors' dead band at the current gearing: planned turns in place+ stall without a static-friction floor (`MOSS_MIN_PCT`, above).++## Deploying from another machine++The robot can run a git clone or an rsync copy of this repository. With rsync,+leave the venv and the package metadata out of the sync and install the package+on the robot itself:++```+$ rsync -a --exclude .venv --exclude .git --exclude 'moss_dimos.egg-info' ./ <user>@<robot>:moss/+$ ssh <user>@<robot> 'cd moss && uv pip install --python .venv/bin/python -e . --no-deps'+```++uv's installer puts it in `~/.local/bin`, which a command run over ssh may not+have on its `PATH`: give the full path there if `uv` is not found. In a venv made+with `python3.12 -m venv`, `.venv/bin/pip install -e . --no-deps` does the same.++New blueprint names only become runnable after that install on the robot. An+rsync that carries `moss_dimos.egg-info/` from a development machine overwrites+the robot's copy: the flight check then reports the MOSS blueprints missing+while the venv knows them. Run the install again on the robot to regenerate it.++## Cold benches++`benches/bench_moss_preflight_cold.py`, `benches/bench_moss_fly_gates_cold.py`,+`benches/bench_moss_cockpit_cold.py` and `benches/bench_explore_ctl_cold.py`+cover these tools, without the robot. Run one with+`PYTHONDONTWRITEBYTECODE=1 PYTHONPATH=. .venv/bin/python benches/<name>.py`, or+every bench with `benches/run_all.sh` ([Software.md](../Software.md#tests-and-benches)).+`bench_explore_ctl_cold.py` needs `lcm` and `dimos_lcm`, which come with dimOS;+the other three run on any Linux machine and skip, with a line that says so,+the sections that need dimOS.diff --git a/firmware/MOSS_QTPY_MDD3A/Control.h b/firmware/MOSS_QTPY_MDD3A/Control.hindex 27bf7b6..7810d78 100644--- a/firmware/MOSS_QTPY_MDD3A/Control.h+++ b/firmware/MOSS_QTPY_MDD3A/Control.h@@ -5,16 +5,16 @@// Defaults below: 0 -> 70 % in 2.1 s (the slope asked for a rover carrying an// arm), 70 -> 0 in 1.05 s; 0 -> 100 % takes 3 s. Tune at runtime with "ramp"// (the host passes MOSS_RAMP_MS), no reflash.-// MAX_PERCENT stays a compile-time limit, not a serial command: 100 since-// 10/10 (the commissioning 25 % was the bench on blocks, 05/10); the host-// side caps lower with MOSS_MAX_PCT when it wants to.+// MAX_PERCENT stays a compile-time limit, not a serial command: full PWM (100;+// the commissioning builds ran at 25 % with the rover on blocks). The host side+// caps lower with MOSS_MAX_PCT when it wants to.#include <stdint.h>#include <stdlib.h>#include <string.h>#include <errno.h>namespace moss {-constexpr int MAX_PERCENT = 100; // Full PWM = the motors' rated 12 V (bench 05/10 was 25 %, raised 10/10).+constexpr int MAX_PERCENT = 100; // Full PWM = the motors' rated 12 V (the commissioning limit was 25 %).constexpr uint32_t COMMAND_TIMEOUT_MS = 300;constexpr uint32_t ARM_TIMEOUT_MS = 10000;constexpr uint32_t TEST_MS = 1000;diff --git a/firmware/MOSS_QTPY_MDD3A/MOSS_QTPY_MDD3A.ino b/firmware/MOSS_QTPY_MDD3A/MOSS_QTPY_MDD3A.inoindex 836cfe3..95aeef0 100644--- a/firmware/MOSS_QTPY_MDD3A/MOSS_QTPY_MDD3A.ino+++ b/firmware/MOSS_QTPY_MDD3A/MOSS_QTPY_MDD3A.ino@@ -3,7 +3,7 @@// Same serial protocol as the V0.3 DevKitC firmware (see ../README.md), over// the QT Py's native USB: build with "USB CDC On Boot: Enabled"// (arduino-cli: --fqbn esp32:esp32:adafruit_qtpy_esp32s3_nopsram:CDCOnBoot=cdc).-// Motors never start at boot. NOT YET RUN ON HARDWARE: pin map to confirm on the+// Motors never start at boot. On a new board: confirm the pin map on the// terminal block, then bench with "test left 10" before any "drive".#include <Arduino.h>#include <Wire.h>@@ -22,7 +22,7 @@ constexpr int ENC_L_A = SDA, ENC_L_B = SCL; // GPIO 7 / 6 (primary I2C pins reconstexpr int ENC_R_A = TX, ENC_R_B = RX; // GPIO 5 / 16// INA219 on the STEMMA QT connector = Wire1 (SDA1 = GPIO 41, SCL1 = GPIO 40)constexpr uint8_t INA_ADDRESS = 0x40;-constexpr bool INVERT_LEFT = true, INVERT_RIGHT = false; // 10/10 first drive: left track ran backwards (Laurent); ticks follow+constexpr bool INVERT_LEFT = true, INVERT_RIGHT = false; // the left track runs backwards without it; its encoder ticks are inverted with itconstexpr float SHUNT_OHMS = 0.0f; // voltage only (see V0.3 README)constexpr uint32_t PWM_HZ = 20000;constexpr int PWM_BITS = 10;@@ -54,7 +54,7 @@ void ARDUINO_ISR_ATTR encoderRight() {portEXIT_CRITICAL_ISR(&encoderMux);}void sendLine(const char* text) {- // First hardware contact: the native CDC FIFO is 64 bytes, so a guard that required the+ // The native CDC FIFO is 64 bytes, so a guard that required the// whole line to fit dropped every telemetry line (~330 bytes). write() already returns 0 when no// host has the port open (DTR low); with a 20 ms tx timeout a stalled host costs at most 20 ms per// line, which keeps the deadman (300 ms) serviced. A line cut by that timeout is the host's problem@@ -69,7 +69,7 @@ void outputsZero() {// Two drivers, one wiring (A0/A1 left, A2/A3 right), chosen at compile time:// default Cytron MDD3A, two PWM inputs per channel (M1A/M1B): PWM on one,// the other held low; zero = both low = brake.-// -DMOSS_DRIVER_MDD10A Cytron MDD10A (ordered 10/10: 10 A / 30 A per channel), PWM + DIR per+// -DMOSS_DRIVER_MDD10A Cytron MDD10A (10 A continuous, 30 A peak per channel), PWM + DIR per// channel: pinA = PWM1 (magnitude), pinB = DIR1 (held high or low through// the same LEDC channel: duty 100 % = high). Zero = PWM low = brake.// Build: arduino-cli compile ... --build-property "compiler.cpp.extra_flags=-DMOSS_DRIVER_MDD10A"diff --git a/firmware/MOSS_QTPY_MDD3A/bench/flash_qtpy.sh b/firmware/MOSS_QTPY_MDD3A/bench/flash_qtpy.shindex e046c12..c99dcd0 100644--- a/firmware/MOSS_QTPY_MDD3A/bench/flash_qtpy.sh+++ b/firmware/MOSS_QTPY_MDD3A/bench/flash_qtpy.sh@@ -1,21 +1,49 @@#!/bin/bash-# Flash the MOSS V0.4 firmware on the QT Py ESP32-S3 from the Jetson, through its native USB.+# Flash the MOSS V0.4 firmware on the QT Py ESP32-S3 from the onboard computer, through its native USB.# Step 1: ask the running Arduino app to reboot into the ROM bootloader (DTR/RTS trick);# step 2: write; step 3: reset out of the bootloader by toggling RTS with the port held open.+#+# firmware/MOSS_QTPY_MDD3A/bench/flash_qtpy.sh images from the default build dir+# FW_DIR=<dir> firmware/MOSS_QTPY_MDD3A/bench/flash_qtpy.sh images from <dir>+#+# FW_DIR holds the images `arduino-cli compile ... --output-dir <dir>` writes (see README.md+# here): MOSS_QTPY_MDD3A.ino.bootloader.bin, .partitions.bin, .bin and boot_app0.bin. By default,+# the build dir next to the sketch (gitignored: compile first).+# PY needs pyserial and esptool: the 'firmware' extra of this package, in this checkout's venv+# (uv pip install --python .venv/bin/python -e '.[firmware]', or .venv/bin/pip install -e '.[firmware]').set -e-PY=~/vector-dimos/.venv/bin/python-cd ~/moss_fw_v04-port() { ls /dev/serial/by-id/ | grep -E "QT_Py|Espressif" | head -1 | sed 's|^|/dev/serial/by-id/|'; }+HERE="$(cd "$(dirname "$0")" && pwd)"+PY="${PY:-$(cd "$HERE/../../.." && pwd)/.venv/bin/python}" # this checkout's venv; PY=... picks another+FW_DIR="${FW_DIR:-$HERE/../build/esp32.esp32.adafruit_qtpy_esp32s3_nopsram}"+IMAGES="MOSS_QTPY_MDD3A.ino.bootloader.bin MOSS_QTPY_MDD3A.ino.partitions.bin boot_app0.bin MOSS_QTPY_MDD3A.ino.bin"+for f in $IMAGES; do+ [ -f "$FW_DIR/$f" ] || {+ echo "flash_qtpy: no $f in $FW_DIR - compile first (arduino-cli compile ... --output-dir <dir>, see README.md next to this script), then FW_DIR=<dir> $0" >&2+ exit 1; }+done+[ -x "$PY" ] || { echo "flash_qtpy: no python at $PY - make the checkout's .venv (Software.md), or PY=<python with esptool and pyserial>" >&2; exit 1; }+"$PY" -c 'import esptool, serial' 2>/dev/null || {+ echo "flash_qtpy: $PY cannot import esptool and pyserial - install the 'firmware' extra: uv pip install --python .venv/bin/python -e '.[firmware]' (or .venv/bin/pip install -e '.[firmware]'), or PY=<python with them>" >&2+ exit 1; }+cd "$FW_DIR"+port() { ls /dev/serial/by-id/ 2>/dev/null | grep -E "QT_Py|Espressif" | head -1 | sed 's|^|/dev/serial/by-id/|'; }P=$(port)+[ -n "$P" ] || { echo "flash_qtpy: no QT Py on USB (/dev/serial/by-id/*QT_Py* or *Espressif*) - cable, power" >&2; exit 1; }if [[ "$P" == *QT_Py* ]]; then- $PY -m esptool --chip esp32s3 --port "$P" --before default-reset --after no-reset chip-id >/dev/null 2>&1 || true+ "$PY" -m esptool --chip esp32s3 --port "$P" --before default-reset --after no-reset chip-id >/dev/null 2>&1 || truesleep 3; P=$(port)fiecho "bootloader port: $P"-$PY -m esptool --chip esp32s3 --port "$P" --baud 921600 --before no-reset --after watchdog-reset write-flash -z \- --flash-mode dio --flash-freq 80m --flash-size 8MB \- 0x0 MOSS_QTPY_MDD3A.ino.bootloader.bin 0x8000 MOSS_QTPY_MDD3A.ino.partitions.bin 0xe000 boot_app0.bin 0x10000 MOSS_QTPY_MDD3A.ino.bin 2>&1 | grep -E "Wrote|verified|rror"-$PY - <<PYEOF+[ -n "$P" ] || { echo "flash_qtpy: the board left USB and did not come back as a bootloader - power-cycle it, then rerun" >&2; exit 1; }+if ! OUT=$("$PY" -m esptool --chip esp32s3 --port "$P" --baud 921600 --before no-reset --after watchdog-reset write-flash -z \+ --flash-mode dio --flash-freq 80m --flash-size 8MB \+ 0x0 MOSS_QTPY_MDD3A.ino.bootloader.bin 0x8000 MOSS_QTPY_MDD3A.ino.partitions.bin 0xe000 boot_app0.bin 0x10000 MOSS_QTPY_MDD3A.ino.bin 2>&1); then+ printf '%s\n' "$OUT" | tail -n 15 >&2+ echo "flash_qtpy: the write failed (esptool's last lines above)" >&2+ exit 1+fi+printf '%s\n' "$OUT" | grep -E "Wrote|verified" || true+"$PY" - <<PYEOFimport serial, times = serial.Serial("$P", 115200, timeout=0.1)s.dtr = False; s.rts = True; time.sleep(0.1); s.rts = False@@ -23,4 +51,6 @@ try:time.sleep(0.5); s.close()except Exception: passPYEOF-sleep 4; ls /dev/serial/by-id/ | grep -E "QT_Py|Espressif"+sleep 4+ls /dev/serial/by-id/ 2>/dev/null | grep -E "QT_Py|Espressif" \+ || { echo "flash_qtpy: the board is not back on USB - power-cycle it" >&2; exit 1; }diff --git a/firmware/MOSS_QTPY_MDD3A/bench/watch_start.sh b/firmware/MOSS_QTPY_MDD3A/bench/watch_start.shindex dafaff7..5d2eeb9 100644--- a/firmware/MOSS_QTPY_MDD3A/bench/watch_start.sh+++ b/firmware/MOSS_QTPY_MDD3A/bench/watch_start.sh@@ -1,8 +1,15 @@#!/bin/bash# Start the servo watcher in the background (kill any previous one by pidfile, never by pattern).-cd ~/moss_fw_v04+# servo_watch.py sits next to this script; its log, pid and error files go to WATCH_DIR+# (default: <MOSS state dir>/servo_watch, outside the checkout). Arguments are the servo IDs.+HERE="$(cd "$(dirname "$0")" && pwd)"+PY="${PY:-$(cd "$HERE/../../.." && pwd)/.venv/bin/python}" # this checkout's venv (needs pyserial); PY=... picks another+[ -x "$PY" ] || { echo "watch_start: no python at $PY - make the checkout's .venv (Software.md), or PY=<python with pyserial>" >&2; exit 1; }+WATCH_DIR="${WATCH_DIR:-${MOSS_STATE_DIR:-$HOME/.local/state/moss}/servo_watch}"+mkdir -p "$WATCH_DIR" && cd "$WATCH_DIR" || { echo "watch_start: cannot use $WATCH_DIR" >&2; exit 1; }[ -f watch.pid ] && kill "$(cat watch.pid)" 2>/dev/nullrm -f watch.log-nohup ~/vector-dimos/.venv/bin/python servo_watch.py watch.log "$@" >watch.err 2>&1 &+nohup "$PY" "$HERE/servo_watch.py" watch.log "$@" >watch.err 2>&1 &echo $! > watch.pid-sleep 2; cat watch.log; echo "pid $(cat watch.pid) alive: $(kill -0 $(cat watch.pid) 2>/dev/null && echo yes || echo no)"+sleep 2; cat watch.log 2>/dev/null+echo "pid $(cat watch.pid) alive: $(kill -0 "$(cat watch.pid)" 2>/dev/null && echo yes || echo no) - log $WATCH_DIR/watch.log, errors $WATCH_DIR/watch.err"diff --git a/moss_dimos/__init__.py b/moss_dimos/__init__.pyindex 89245b7..2267394 100644--- a/moss_dimos/__init__.py+++ b/moss_dimos/__init__.py@@ -1 +1,5 @@-"""moss_dimos - dimOS modules for the MOSS litter-picking rover (scaffold)."""+"""moss_dimos - dimOS modules for the MOSS litter-picking rover.++Kept free of imports on purpose: every dimOS worker that unpickles a+moss_dimos class imports this package first.+"""diff --git a/moss_dimos/blueprints.py b/moss_dimos/blueprints.pynew file mode 100644index 0000000..97191cf--- /dev/null+++ b/moss_dimos/blueprints.py@@ -0,0 +1,534 @@+"""dimOS blueprints for MOSS, and the coordinator they all share.++Entry points (pyproject [project.entry-points."dimos.blueprints"]):++ dimos run moss-dimos.moss-base coordinator + the tracked base, twist on cmd_vel+ dimos run moss-dimos.moss-gamepad base + gamepad, direct drive+ dimos run moss-dimos.moss-nav mapping + pad teleop, recording+ dimos run moss-dimos.moss-explore mapping + planner + frontier+ exploration + pad teleop, recording++The chain on the mapping blueprints: RPLidarC1 -> LidarOdometry ->+MossCostMap, the coordinator, and on moss-explore RecoveringPlanner,+MovementManager and Explorer2. All four blueprints are built on first+access (module __getattr__ below), never at import: importing this module+costs the coordinator class and the two side effects below, nothing more.++Nothing here moves the robot by itself:+ * moss-gamepad and moss-nav have no planner and no explorer: the pad drives+ the coordinator directly (tele_cmd_vel -> cmd_vel), and only while its+ deadman is held.+ * moss-explore's explorer starts only on Bool(True) on `explore_cmd`+ (tools/explore_ctl.py start, the cockpit), and the planner moves only+ toward a goal on `goal_request` / `clicked_point`. MovementManager+ arbitrates: any pad input cancels navigation.+ * the adapter's 0.5 s deadman and the firmware's 300 ms deadman are+ untouched.++Devices (moss_ports): the QT Py and the C1 by their /dev/serial/by-id path,+MOSS_PORT / MOSS_LIDAR_PORT win. A missing device resolves to a sentinel+path that cannot open - logged as an error at build time and again by the+module that fails to open it - never to another device.++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)+ MOSS_WRIST=0 no wrist camera (default on: the UVC camera on the arm, 640x480 @ 5 fps,+ JPEG on the bus as wrist_image; absent device = warning, module skipped)+ MOSS_WRIST_DEV wrist camera: a /dev/videoN path or an index, instead of the by-id lookup+ MOSS_RS_SERIAL pick the RealSense by serial number+ MOSS_RECORD=0 no recording (default on: MossRecorder, <MOSS_STATE_DIR>/recordings/moss.db)+ MOSS_RERUN=1 add the stock Rerun/websocket viewers (default off: no 7779/9877/3030 on MOSS)++Why the coordinator is a subclass defined HERE. An out-of-tree drive train+needs two things, and the second one is not obvious:++1. The 'moss' adapter name must be registered. dimOS's drive_trains manifest+ scan only walks its own package, so external packages use the public+ registry escape hatch (register_path, below).++2. That registration must also happen in the process that BUILDS the+ adapter. dimOS deploys every module into a `forkserver` worker+ (dimos/core/coordination/python_worker.py), and a forkserver worker is a+ fresh interpreter: it re-imports dimos, gets a brand-new+ twist_base_adapter_registry, and never imports this module on its own.+ Registering in the CLI parent alone is not enough -+ ControlCoordinator._setup_hardware runs in the worker and dies with:++ KeyError: "Unknown twist base adapter: moss.+ Available: ['flowbase', 'mock_twist_base', ...]"++ The worker does import whatever it needs to UNPICKLE the module class it+ was told to deploy, and classes pickle by reference. So the coordinator+ handed to dimOS is our own subclass, defined here: unpickling+ `moss_dimos.blueprints:MossControlCoordinator` in the worker imports this+ module, which runs the registration above the class. That mechanism holds+ for fork, forkserver and spawn alike. The same import installs the LCM+ queue clamp (lcm_latest) in the worker. Keep the registration, the clamp+ and the class in this one module.++ Subclassing the coordinator is dimOS's own documented pattern for this+ (see dimos/robot/manipulators/common/blueprints.py:coordinator(cls=...));+ it requires instance_name="ControlCoordinator" so the shipped RPC clients+ still find the coordinator by the name they expect, and ControlCoordinator+ .start() warns if a subclass omits it.++The wrist camera module (WristCamera) is a class built on first use and+served by the same module __getattr__, so dimOS pickles it by reference too.+"""+from __future__ import annotations++import asyncio+import json+import math+import os+import time+from typing import Any, Callable++from dimos.control.components import (HardwareComponent, HardwareType,+ make_twist_base_joints)+from dimos.control.coordinator import ControlCoordinator, TaskConfig+from dimos.core.core import rpc+from dimos.core.stream import Out+from dimos.hardware.drive_trains.registry import twist_base_adapter_registry+from dimos.msgs.std_msgs.String import String+from dimos.utils.logging_config import setup_logger++from moss_dimos import robot_profile+from moss_dimos.moss_adapter import telemetry_payload+from moss_dimos.robot_profile import (MOSS_LIDAR_MISSING, MOSS_QTPY_MISSING,+ RobotProfile)++_logger = setup_logger()++# Register the MOSS base adapter under its name, in every process that imports+# this module (see the docstring). Registering the same path twice is a no-op;+# dimOS raises only when "moss" already points somewhere else - then the first+# registration stays in force, and that must be said, not swallowed.+MOSS_ADAPTER_NAME = "moss"+MOSS_ADAPTER_PATH = "moss_dimos.moss_adapter:MossBaseAdapter"+try:+ twist_base_adapter_registry.register_path(MOSS_ADAPTER_NAME, MOSS_ADAPTER_PATH)+except ValueError as _exc:+ _existing = getattr(twist_base_adapter_registry, "_factory_paths", {}).get(MOSS_ADAPTER_NAME)+ _logger.warning(f"twist base adapter '{MOSS_ADAPTER_NAME}' is already registered as "+ f"{_existing or 'another path'!r}; {MOSS_ADAPTER_PATH!r} is NOT used "+ f"in this process ({_exc})")++# Velocity commands must never queue up (stale twists drive the rover past+# the point asked): clamp the LCM queue of command channels to one message.+from moss_dimos.lcm_latest import install as _install_latest_only # noqa: E402+_install_latest_only()++# That clamp is LCM-only, and zenoh's subscriber has no queue bound to set, so+# the coordinator drops its own stale twists below (moss_dimos.latest_only).+from moss_dimos.latest_only import LatestOnly # noqa: E402++LATEST_ONLY_STREAM = "twist_command"++# The shipped dimOS RPC clients address the coordinator by this name; keep it+# even though the class below is a subclass.+COORDINATOR_INSTANCE_NAME = "ControlCoordinator"++_base_joints = make_twist_base_joints("base")+++class MossControlCoordinator(ControlCoordinator):+ """Stock ControlCoordinator, deployed under this package's import path.++ One job of plumbing: make the forkserver worker import+ moss_dimos.blueprints (see this module's docstring), which is what+ registers the 'moss' twist base adapter in the process that actually+ instantiates it;++ plus one guard: twist_command is consumed latest-only (see below),++ and one window: `base_telemetry` republishes, every TELEMETRY_PERIOD_S,+ the firmware telemetry of any adapter that keeps one (the MOSS adapter:+ battery, mode, track outputs - see moss_adapter.telemetry_payload). The+ cockpit reads the battery there, so nothing but this process ever opens+ the motor board's port. An adapter without telemetry publishes nothing.+ """++ base_telemetry: Out[String]++ TELEMETRY_PERIOD_S = 0.5++ # Run the control loop in its own process: sharing a worker (and its GIL)+ # with a busy module - a camera, a point cloud - delays the execution of+ # twists even with the command queue clamped to one.+ dedicated_worker = True++ @rpc+ def start(self) -> None:+ super().start()+ if getattr(self, "_telemetry_running", False):+ return+ self._telemetry_running = True+ self.spawn(self._telemetry_loop())++ @rpc+ def stop(self) -> None:+ self._telemetry_running = False+ super().stop()++ async def _telemetry_loop(self) -> None:+ """2 Hz: one String JSON per adapter that has telemetry. Never raises:+ a bad line or a publish error is logged once per cause and the loop+ goes on (it must not take the control process down with it)."""+ last_error = None+ while getattr(self, "_telemetry_running", False):+ try:+ with self._hardware_lock:+ connected = list(self._hardware.values())+ now = time.time()+ for hw in connected:+ payload = telemetry_payload(getattr(hw, "adapter", None), now)+ if payload is not None:+ self.base_telemetry.publish(String(data=json.dumps(payload)))+ last_error = None+ except Exception as exc: # noqa: BLE001+ cause = f"{type(exc).__name__}: {exc}"+ if cause != last_error:+ _logger.warning(f"base_telemetry: {cause}")+ last_error = cause+ await asyncio.sleep(self.TELEMETRY_PERIOD_S)++ def _make_stream_cb(self, stream: str) -> Callable[[Any], None]:+ """Consume twist_command latest-only, whatever the transport.++ lcm_latest clamps the LCM queue to one message; zenoh's subscriber has+ no such bound and dimOS exposes no capacity for it, so the drop has to+ happen here. dimOS builds a fresh callback per subscription (hardware+ add/remove re-syncs them) - keep one drain thread and re-point it.+ """+ callback = super()._make_stream_cb(stream)+ if stream != LATEST_ONLY_STREAM:+ return callback+ latest = getattr(self, "_latest_twist", None)+ if latest is None:+ latest = self._latest_twist = LatestOnly(callback, name=stream)+ else:+ latest.rebind(callback)+ return latest+++def _moss_base(hw_id: str = "base",+ address: str | None = None) -> HardwareComponent:+ """MOSS tracked base (differential; vy ignored) - QT Py ESP32-S3 over USB CDC."""+ return HardwareComponent(+ hardware_id=hw_id,+ hardware_type=HardwareType.BASE,+ joints=make_twist_base_joints(hw_id),+ adapter_type=MOSS_ADAPTER_NAME,+ address=address, # serial port; adapter default: MOSS_PORT, the QT Py by-id, or a sentinel that fails loudly+ )+++def _coordinator_blueprint(base: HardwareComponent):+ """Coordinator driving one base at velocity."""+ return MossControlCoordinator.blueprint(+ hardware=[base],+ tasks=[+ TaskConfig(name="vel_base", type="velocity",+ joint_names=_base_joints, priority=10),+ ],+ instance_name=COORDINATOR_INSTANCE_NAME,+ )+++# ── devices, cameras, switches ──────────────────────────────────────────────++LIDAR_BAUDRATE = 460800+CAMERA_WIDTH, CAMERA_HEIGHT, CAMERA_FPS = 640, 480, 5+EXPLORER_MIN_FRONTIER_PERIMETER_M = 0.3 # doorway-sized frontiers stay visible+EXPLORER_GOAL_TIMEOUT_S = 45.0+++def moss_ports(env: dict | None = None) -> dict:+ """{"base": QT Py port, "lidar": C1 port}: env first, then the by-id glob+ under robot_profile.BYID_ROOT, then the sentinel.++ Raises ValueError when both resolve to the same device (realpath): the+ base adapter would open the lidar's tty at 115200, retune the line under+ RPLidarC1 (460800) and write its 'status' probe into the C1. The usual+ cause is a stale MOSS_PORT pointing at the CP2102N."""+ ports = {"base": robot_profile.moss_qtpy_port(env),+ "lidar": robot_profile.moss_lidar_port(env)}+ if (ports["base"] != MOSS_QTPY_MISSING and ports["lidar"] != MOSS_LIDAR_MISSING+ and os.path.realpath(ports["base"]) == os.path.realpath(ports["lidar"])):+ raise ValueError(+ f"MOSS: the base port and the lidar port are the same device "+ f"({ports['base']} -> {os.path.realpath(ports['base'])}); the motor board is the "+ "QT Py (*QT_Py*), the lidar the CP2102N - check MOSS_PORT / MOSS_LIDAR_PORT")+ return ports+++WRIST_WIDTH, WRIST_HEIGHT, WRIST_FPS = 640, 480, 5+WRIST_BY_ID_GLOBS = ("*KD-USB*-video-index0", "*Sonix*-video-index0") # the arm's UVC camera, as moss_preflight+V4L_BY_ID = "/dev/v4l/by-id"+++def wrist_camera_index(env: dict | None = None, by_id_dir: str | None = None) -> int | None:+ """/dev/videoN index of the wrist camera, or None when it is not there.+ MOSS_WRIST_DEV wins (an index or a /dev/videoN path); otherwise the first+ by-id match of the known names. The kernel numbers video devices in plug+ order, so the by-id name is the stable handle and the index is resolved+ when the blueprint is built."""+ import glob+ import re+ env = os.environ if env is None else env+ by_id_dir = V4L_BY_ID if by_id_dir is None else by_id_dir+ dev = str(env.get("MOSS_WRIST_DEV", "")).strip()+ if dev:+ if dev.isdigit():+ return int(dev)+ target = os.path.realpath(dev)+ else:+ target = None+ for pat in WRIST_BY_ID_GLOBS:+ hits = sorted(glob.glob(os.path.join(by_id_dir, pat)))+ if hits:+ target = os.path.realpath(hits[0])+ break+ if target is None:+ return None+ m = re.fullmatch(r"/dev/video(\d+)", target)+ return int(m.group(1)) if m else None+++_WRIST_CLASS = None+++def _wrist_module_class():+ """A CameraModule of its own class, so the blueprint remaps its outputs+ (color_image -> wrist_image, ...) without touching the RealSense. Built+ on first use (the camera stack is imported only then, so a coordinator+ worker never loads it) and exposed as `moss_dimos.blueprints.WristCamera`+ through the module __getattr__, so dimOS can pickle it by reference into+ its worker processes."""+ global _WRIST_CLASS+ if _WRIST_CLASS is None:+ from dimos.hardware.sensors.camera.module import CameraModule++ cls = type("WristCamera", (CameraModule,),+ {"__doc__": "The arm's UVC camera (dimOS Webcam hardware, cv2 capture).",+ "__module__": __name__, "__qualname__": "WristCamera"})+ _WRIST_CLASS = cls+ return _WRIST_CLASS+++def _wrist_part(index: int):+ from functools import partial+ from dimos.hardware.sensors.camera.webcam import Webcam++ WristCamera = _wrist_module_class()+ # transform=None: no base_link -> camera_link TF from this module (that frame+ # is the RealSense's); the frames are "wrist/..." through the hardware prefix+ return WristCamera.blueprint(+ hardware=partial(Webcam, camera_index=index, width=WRIST_WIDTH, height=WRIST_HEIGHT,+ fps=float(WRIST_FPS), frame_id_prefix="wrist"),+ frame_id="wrist_camera_link", transform=None, frequency=float(WRIST_FPS))+++def camera_mount_xyzq(profile: RobotProfile) -> tuple[float, ...]:+ """base_link -> camera_link for the RealSense module, as+ (x, y, z, qx, qy, qz, qw): the mechanical pose only (dimOS adds the+ optical rotation itself), a pure pitch about +y, positive = nose down."""+ x, y, z = profile.camera_xyz_base_m+ half = math.radians(profile.camera_pitch_deg) / 2.0+ return (x, y, z, 0.0, math.sin(half), 0.0, math.cos(half))+++def _announce(ports: dict, profile: RobotProfile) -> None:+ """The build-time lines: which devices, which body - and loudly when a+ device is missing."""+ if ports["base"] == MOSS_QTPY_MISSING:+ _logger.error("MOSS: no QT Py under /dev/serial/by-id (*QT_Py*) and MOSS_PORT unset - "+ f"the base gets {MOSS_QTPY_MISSING} and will not connect")+ if ports["lidar"] == MOSS_LIDAR_MISSING:+ _logger.error("MOSS: no CP2102N under /dev/serial/by-id (*CP2102N*) and MOSS_LIDAR_PORT unset - "+ f"the lidar gets {MOSS_LIDAR_MISSING} and will publish nothing")+ _logger.info(f"MOSS blueprint: base {ports['base']}, lidar {ports['lidar']}, profile {profile}")+++def _gamepad_part():+ """The pad, its stick scale and clamp sized to the base's envelope+ (MOSS_MPS_AT_LIMIT / MOSS_TRACK_M / MOSS_TURN_GAIN / MOSS_MAX_PCT, read+ now, in the process that builds the blueprint): see teleop_speeds and+ teleop_envelope."""+ from moss_dimos.gamepad import GamepadTeleop+ from moss_dimos.moss_adapter import MossGeometry, teleop_envelope, teleop_speeds++ g = MossGeometry.from_env()+ linear, angular = teleop_speeds(g)+ return GamepadTeleop.blueprint(linear_speed=linear, angular_speed=angular,+ envelope=teleop_envelope(g))+++def _common_parts(ports: dict, profile: RobotProfile) -> list:+ """Base, pad, lidar, odometry, costmap, camera, recorder, viewer: the+ modules moss-nav and moss-explore share."""+ from moss_dimos.costmap2d import MossCostMap+ from moss_dimos.lidar_odometry import LidarOdometry+ from moss_dimos.rplidar_c1 import RPLidarC1++ parts = [+ _coordinator_blueprint(_moss_base(address=ports["base"])),+ _gamepad_part(),+ RPLidarC1.blueprint(port=ports["lidar"], baudrate=LIDAR_BAUDRATE, frame_id="lidar_link"),+ # no IMU wired: kiss-icp on its constant-velocity prior+ LidarOdometry.blueprint(use_gyro_prior=False),+ MossCostMap.blueprint(),+ ]+ 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+ from dimos.msgs.geometry_msgs.Transform import Transform+ from dimos.msgs.geometry_msgs.Vector3 import Vector3++ x, y, z, qx, qy, qz, qw = camera_mount_xyzq(profile)+ cam_kwargs = dict(width=CAMERA_WIDTH, height=CAMERA_HEIGHT, fps=CAMERA_FPS,+ enable_depth=False, enable_pointcloud=False, enable_imu=False,+ base_transform=Transform(translation=Vector3(x, y, z),+ rotation=Quaternion(qx, qy, qz, qw)))+ serial = os.environ.get("MOSS_RS_SERIAL", "").strip()+ if serial:+ cam_kwargs["serial_number"] = serial+ parts.append(RealSenseCamera.blueprint(**cam_kwargs))+ if robot_profile.env_on("MOSS_WRIST", True):+ idx = wrist_camera_index()+ if idx is None:+ _logger.warning("MOSS blueprint: no wrist camera (%s under %s, or MOSS_WRIST_DEV): "+ "wrist_image will not be published", " / ".join(WRIST_BY_ID_GLOBS), V4L_BY_ID)+ else:+ _logger.info(f"MOSS blueprint: wrist camera /dev/video{idx}, {WRIST_WIDTH}x{WRIST_HEIGHT} @ {WRIST_FPS} fps")+ parts.append(_wrist_part(idx))+ if robot_profile.env_on("MOSS_RECORD", True):+ from moss_dimos.memory import MossRecorder+ parts.append(MossRecorder.blueprint(db_path=str(robot_profile.moss_recording_path())))+ if robot_profile.env_on("MOSS_RERUN", False):+ from dimos.core.global_config import global_config+ from dimos.visualization.vis_module import vis_module+ parts.append(vis_module(viewer_backend=global_config.viewer))+ return parts+++def wrist_instance(bp) -> str | None:+ """The wrist camera's instance name in a blueprint (dimOS lowercases module+ names unless one is given), or None when the module is not in it."""+ for atom in bp.blueprints:+ if getattr(atom.module, "__name__", "") == "WristCamera":+ return atom.name+ return None+++def _jpeg_camera(bp):+ """Colour frames cross the bus as JPEG (encoded once in the camera's+ process): the cockpit passes them through, the recorder stores JPEG.+ The wrist camera's outputs are remapped away from the RealSense's names+ (wrist_image, wrist_camera_info, wrist_tf) and wrist_image is JPEG too."""+ from dimos.core.transport import JpegLcmTransport+ from dimos.msgs.sensor_msgs.Image import Image+ transports = {}+ if robot_profile.env_on("MOSS_CAMERA", True):+ transports[("color_image", Image)] = JpegLcmTransport.spec("/color_image", Image)+ wrist = wrist_instance(bp)+ if robot_profile.env_on("MOSS_WRIST", True) and wrist is not None:+ bp = bp.remappings([(wrist, "color_image", "wrist_image"),+ (wrist, "camera_info", "wrist_camera_info"),+ (wrist, "tf", "wrist_tf")])+ transports[("wrist_image", Image)] = JpegLcmTransport.spec("/wrist_image", Image)+ return bp.transports(transports) if transports else bp+++# ── the four blueprints, built on first access ──────────────────────────────++def build_moss_base():+ """The coordinator and the tracked base, twist_command on cmd_vel: any+ publisher of cmd_vel drives the base (the adapter's deadman still holds).+ The port is the adapter's default: MOSS_PORT, the QT Py by-id, or the+ sentinel."""+ return _coordinator_blueprint(_moss_base()).remappings(+ [(MossControlCoordinator, "twist_command", "cmd_vel")])+++def build_moss_gamepad():+ """Base + pad, direct drive: the pad IS the commander (tele_cmd_vel ->+ cmd_vel), no nav, no MovementManager. tools/moss-teleop.sh runs it."""+ from dimos.core.coordination.blueprints import autoconnect+ from moss_dimos.gamepad import GamepadTeleop++ return autoconnect(+ _coordinator_blueprint(_moss_base()),+ _gamepad_part(),+ ).remappings([+ (MossControlCoordinator, "twist_command", "cmd_vel"),+ (GamepadTeleop, "tele_cmd_vel", "cmd_vel"),+ ])+++def build_moss_nav():+ """Mapping + pad teleop. No planner, no explorer, no MovementManager: the+ pad drives the coordinator directly, like moss-gamepad."""+ from dimos.core.coordination.blueprints import autoconnect+ from moss_dimos.gamepad import GamepadTeleop++ profile = robot_profile.current()+ ports = moss_ports()+ _announce(ports, profile)+ return _jpeg_camera(autoconnect(*_common_parts(ports, profile))).remappings([+ (MossControlCoordinator, "twist_command", "cmd_vel"),+ (GamepadTeleop, "tele_cmd_vel", "cmd_vel"),+ ])+++def build_moss_explore():+ """moss-nav's modules plus the planner, the arbiter and the explorer. The+ pad stays on tele_cmd_vel: MovementManager lets it win and cancels any+ navigation. Exploration waits for `explore_cmd`."""+ from dimos.core.coordination.blueprints import autoconnect+ from dimos.msgs.geometry_msgs.Twist import Twist+ from dimos.msgs.geometry_msgs.Vector3 import Vector3+ from dimos.navigation.movement_manager.movement_manager import MovementManager+ from moss_dimos.explorer2 import Explorer2+ from moss_dimos.recovering_planner import RecoveringPlanner++ profile = robot_profile.current()+ ports = moss_ports()+ _announce(ports, profile)+ lethal, pivot = robot_profile.derived(profile)+ parts = _common_parts(ports, profile) + [+ RecoveringPlanner.blueprint(robot_width=profile.robot_width_m,+ robot_rotation_diameter=profile.pivot_diameter_m),+ # pass-through scaling: the pad's own envelope is the guard+ MovementManager.blueprint(+ tele_cmd_vel_scaling=Twist(Vector3(1.0, 1.0, 0.0), Vector3(0.0, 0.0, 1.0))),+ Explorer2.blueprint(min_frontier_perimeter=EXPLORER_MIN_FRONTIER_PERIMETER_M,+ goal_timeout=EXPLORER_GOAL_TIMEOUT_S,+ lethal_clearance_m=lethal, pivot_clearance_m=pivot),+ ]+ return _jpeg_camera(autoconnect(*parts)).remappings([+ (MossControlCoordinator, "twist_command", "cmd_vel"),+ ])+++# pyproject entry point target -> builder. Keys must equal the targets.+_BUILDERS = {"moss_base_blueprint": build_moss_base,+ "moss_gamepad_blueprint": build_moss_gamepad,+ "moss_nav_blueprint": build_moss_nav,+ "moss_explore_blueprint": build_moss_explore}+++def __getattr__(name: str):+ """Entry points load `moss_<x>_blueprint` as attributes: build on first+ access, then cache. `WristCamera` is the dynamic camera class (built+ once, so pickling by reference finds the same object)."""+ if name == "WristCamera":+ return _wrist_module_class()+ builder = _BUILDERS.get(name)+ if builder is None:+ raise AttributeError(f"module {__name__!r} has no attribute {name!r}")+ bp = builder()+ globals()[name] = bp+ return bpdiff --git a/moss_dimos/c1_serial.py b/moss_dimos/c1_serial.pynew file mode 100644index 0000000..2a05593--- /dev/null+++ b/moss_dimos/c1_serial.py@@ -0,0 +1,210 @@+"""Minimal RPLIDAR C1 reader over pyserial: the standard SLAMTEC scan, no third-party lib.++The C1 answers the classic SLAMTEC protocol: SCAN (A5 20) -> 7-byte descriptor+(A5 5A 05 00 00 40 81) -> endless 5-byte samples. rplidar-roboticia 0.9.5+rejects the C1's descriptor ("Descriptor length mismatch"), while the raw+protocol below works as is. Measured on the bench: ~3.8 k samples/s at+460800 baud, 10 Hz scans.++Sample (5 bytes): b0: S (bit0), !S (bit1), quality (bits 2-7)+ b1: C (bit0, always 1), angle_q6 low 7 bits+ b2: angle_q6 high 8 bits -> angle_deg = angle_q6 / 64+ b3-b4: distance_q2 (u16 LE) -> dist_mm = distance_q2 / 4+"""+from __future__ import annotations++import time+from collections.abc import Iterator++import serial++SCAN = b"\xa5\x20"+STOP = b"\xa5\x25"+RESET = b"\xa5\x40"+GET_HEALTH = b"\xa5\x52"+GET_INFO = b"\xa5\x50"+SCAN_DESCRIPTOR = b"\xa5\x5a\x05\x00\x00\x40\x81"+SPINUP_GRACE_S = 10.0 # motor spin-up after a STOP before the first sample+++class C1Scanner:+ def __init__(self, port: str, baudrate: int = 460800, timeout: float = 2.0):+ self.port, self.baudrate, self.timeout = port, baudrate, timeout+ self._ser: serial.Serial | None = None++ # ── lifecycle ──────────────────────────────────────────────────────+ def open(self) -> None:+ self._ser = serial.Serial(self.port, self.baudrate, timeout=self.timeout)+ # The modem lines matter on the C1's CP2102 bridge: the C1 answers+ # with exactly ONE of DTR/RTS asserted and stays mute with both low or+ # both high. The cp210x driver keeps the last state across opens, so a+ # process killed at the wrong moment leaves the mute combination in+ # place and every later open inherits it (a power cycle does not+ # clear it). DTR low + RTS high is the known-good state; set BOTH,+ # on every open.+ self._ser.dtr = False+ self._ser.rts = True+ self.stop()+ self._wake()++ def _wake(self) -> None:+ """Wake a C1 that stays numb after a hot unplug: its link can answer+ nothing until it receives a flood of requests. Probe GET_INFO; on+ silence, toggle RTS (the rising edge resets the C1), then send bursts+ of GET_INFO for up to 6 s.++ 6 s: a numb C1 has always woken within that; waiting longer only+ delays the verdict."""+ try:+ self.info()+ return+ except Exception: # noqa: BLE001 - numb: go wake it up+ pass+ self._ser.rts = False+ time.sleep(0.3)+ self._ser.rts = True # rising edge: the C1 reboots (banner) and listens again+ time.sleep(2.5)+ self._ser.reset_input_buffer()+ try:+ self.info()+ return+ except Exception: # noqa: BLE001 - still nothing: the flood, then+ pass+ deadline = time.monotonic() + 6.0+ while time.monotonic() < deadline:+ burst_until = time.monotonic() + 1.0+ while time.monotonic() < burst_until:+ self._ser.write(bytes([0xA5, 0x50]) * 50)+ time.sleep(0.1)+ self._ser.reset_input_buffer()+ try:+ self.info()+ return+ except Exception: # noqa: BLE001 - still numb: keep insisting+ pass+ raise RuntimeError("lidar silent even after the wake-up burst")++ def close(self) -> None:+ if self._ser is not None:+ try:+ self.stop()+ finally:+ self._ser.close()+ self._ser = None++ def __enter__(self) -> C1Scanner:+ self.open()+ return self++ def __exit__(self, *exc: object) -> None:+ self.close()++ def stop(self) -> None:+ assert self._ser is not None+ self._ser.write(STOP)+ time.sleep(0.05)+ self._ser.reset_input_buffer()++ def reset(self) -> None:+ assert self._ser is not None+ self._ser.write(RESET)+ time.sleep(2.0)+ self._ser.reset_input_buffer()++ # ── queries ────────────────────────────────────────────────────────+ def _query(self, cmd: bytes, payload_len: int) -> bytes:+ assert self._ser is not None+ self._ser.reset_input_buffer()+ self._ser.write(cmd)+ head = self._ser.read(7)+ if len(head) != 7 or head[:2] != b"\xa5\x5a":+ raise RuntimeError(f"no descriptor for {cmd.hex()}: {head.hex()}")+ return self._ser.read(payload_len)++ def health(self) -> tuple[str, int]:+ p = self._query(GET_HEALTH, 3)+ status = {0: "Good", 1: "Warning", 2: "Error"}.get(p[0], str(p[0]))+ return status, p[1] | (p[2] << 8)++ def info(self) -> dict[str, object]:+ p = self._query(GET_INFO, 20)+ return {"model": p[0], "firmware": (p[2], p[1]), "hardware": p[3],+ "serial": p[4:20].hex().upper()}++ # ── scanning ───────────────────────────────────────────────────────+ def samples(self) -> Iterator[tuple[bool, int, float, float]]:+ """Yield (new_turn, quality, angle_deg, dist_m) forever; dist 0.0 = no return."""+ assert self._ser is not None+ s = self._ser+ s.reset_input_buffer()+ s.write(SCAN)+ head = s.read(7)+ if head != SCAN_DESCRIPTOR:+ raise RuntimeError(f"unexpected scan descriptor: {head.hex()}")+ buf = b""+ # After a STOP the C1 spins its motor down; on SCAN it answers the+ # descriptor at once but streams samples only once the motor is back+ # up - a few seconds. Give the first sample that grace, then the+ # normal timeout applies.+ first_deadline = time.monotonic() + SPINUP_GRACE_S+ while True:+ chunk = s.read(max(5, s.in_waiting or 0))+ if not chunk:+ if not buf and time.monotonic() < first_deadline:+ continue+ raise RuntimeError("scan stream timed out")+ buf += chunk+ i = 0+ while len(buf) - i >= 5:+ b0, b1 = buf[i], buf[i + 1]+ start, nstart, check = b0 & 1, (b0 >> 1) & 1, b1 & 1+ if start == nstart or check != 1: # lost sync: slide one byte+ i += 1+ continue+ quality = b0 >> 2+ angle = ((b1 >> 1) | (buf[i + 2] << 7)) / 64.0+ dist = (buf[i + 3] | (buf[i + 4] << 8)) / 4.0 / 1000.0+ yield bool(start), quality, angle, dist+ i += 5+ buf = buf[i:]++ def scans(self, min_len: int = 30) -> Iterator[list[tuple[int, float, float]]]:+ """Yield one list of (quality, angle_deg, dist_m) per revolution."""+ current: list[tuple[int, float, float]] = []+ for new_turn, q, ang, d in self.samples():+ if new_turn and len(current) >= min_len: # the S flag marks a revolution+ yield current+ current = []+ current.append((q, ang, d))+++class C1Lidar:+ """Drop-in for the subset of the rplidar-roboticia API that rplidar_c1.py+ uses: opens the port in __init__ (so a missing device raises right there),+ get_info/get_health, iter_scans() yielding (quality, angle_deg, distance_MM)+ per revolution, stop/stop_motor/disconnect. The C1 stops its motor on STOP,+ so stop_motor is a no-op kept for interface parity."""++ def __init__(self, port: str, baudrate: int = 460800, timeout: float = 2.0,+ logger: object | None = None):+ self._s = C1Scanner(port, baudrate=baudrate, timeout=timeout)+ self._s.open()++ def get_info(self) -> dict[str, object]:+ return self._s.info()++ def get_health(self) -> tuple[str, int]:+ return self._s.health()++ def iter_scans(self, min_len: int = 30) -> Iterator[list[tuple[int, float, float]]]:+ for scan in self._s.scans(min_len=min_len):+ yield [(q, ang, d * 1000.0) for (q, ang, d) in scan]++ def stop(self) -> None:+ self._s.stop()++ def stop_motor(self) -> None:+ return None++ def disconnect(self) -> None:+ self._s.close()diff --git a/moss_dimos/costmap2d.py b/moss_dimos/costmap2d.pynew file mode 100644index 0000000..5e52b79--- /dev/null+++ b/moss_dimos/costmap2d.py@@ -0,0 +1,355 @@+"""A 2D costmap for MOSS that learns AND unlearns, from the lidar.++dimOS's CostMapper turns a 3D voxel map (a set that never forgets) into a+terrain-slope map meant for a walking robot, and its erosion drops any cell+whose four neighbours are unobserved, which erases thin obstacles such as+table legs. This module replaces it for a rover with a single-plane lidar:++ * the map must REINFORCE, but with a ceiling (HIT_CAP), or a ramp the+ rover struggled on becomes a wall after a day;+ * it must UNLEARN what it sees absent again: every lidar ray that passes+ through a cell is a miss, down to FREE_FLOOR, so a chair that was moved+ away fades in a few revolutions;+ * reinforcement needs new viewpoints: from one spot a cell gets no more+ certain than OCCUPIED_AT - 1, so a passer-by hammered from one parked spot+ never becomes a wall; a second viewpoint NEW_VIEWPOINT_M away makes it an+ obstacle, and a leg seen from twenty positions is solid (up to HIT_CAP);+ * what the body drove over (body_clear) is cleared: that one is a physical+ certainty, not a sensor reading.++The lidar is the only sensor that writes here. lidar_odometry publishes each+revolution in `world` with every lidar return at exactly the profile's+lidar_height_m; `lidar_returns` keeps those points and drops anything else+the cloud may carry.++Mission gate: the map writes only once the mission has started. A rover parked+with a spinning lidar thickens the obstacles around its one viewpoint and+records whoever walks past while it waits. The gate opens on the first of two+events: the rover moving MISSION_START_M away from its first pose, or an+`explore_cmd` (the order to explore, which comes just before the first+motion). It then stays open for the rest of the run: the end of an+exploration (`explore_done`, `stop_explore_cmd`) stops the explorer, not the+map, because the lidar map is MOSS's only obstacle source and a cockpit goal+or STOP sends `stop_explore_cmd` mid-run.++Numbers (HIT_CAP, FREE_FLOOR, OCCUPIED_AT) are a starting point to be tested.+"""++from __future__ import annotations++import math+import time+from typing import Any++import numpy as np++from dimos.core.core import rpc+from dimos.core.module import Module+from dimos.core.stream import In, Out+from dimos.msgs.geometry_msgs.Pose import Pose+from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped+from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid+from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2+from dimos.utils.logging_config import setup_logger+from dimos_lcm.std_msgs import Bool++from moss_dimos.robot_profile import current as _current_profile++logger = setup_logger()++RESOLUTION_M = 0.05+GRID_SPAN_M = 24.0 # world-fixed square, centred on the first pose+HIT_CAP = 10 # ceiling: no cell gets more certain than this+FREE_FLOOR = -3 # floor: no cell gets more "free" than this+OCCUPIED_AT = 2 # two hits from two places = an obstacle+NEW_VIEWPOINT_M = 0.10 # a hit counts only if the rover moved this much since the cell's last hit+LIDAR_Z_TOL_M = 1e-6 # lidar returns sit at EXACTLY the profile's lidar_height_m: the tag survives the float32 round trip to 5 nm (see lidar_returns)+PUBLISH_EVERY = 5 # one grid every 5 revolutions (~2 Hz at 10 Hz). A 24 m grid is ~230 kB and every+ # subscriber (planner, explorer, recorder, cockpit) decodes each one; the map+ # QUALITY does not depend on this rate (the odometry gives it).+RAY_MAX_M = 4.0 # rays are carved (misses) up to this range only: 12 m rays in 2.5 cm steps cost ~230 ms per revolution+RAY_EVERY = 2 # carve on every other revolution; hits are taken on all+MISSION_START_M = 0.05 # the map opens when the rover has moved this far from its first pose (or on explore_cmd).+ # Measuring the displacement needs no clock: the wait accumulates nothing however+ # long it lasts, and the first useful revolution leaves with the rover.+++class ScoredGrid:+ """One int8 score layer over a fixed world grid, plus where each cell was last hit from."""++ def __init__(self, resolution: float = RESOLUTION_M, span_m: float = GRID_SPAN_M,+ centre: tuple[float, float] = (0.0, 0.0)) -> None:+ self.res = resolution+ self.n = int(round(span_m / resolution))+ self.ox = centre[0] - span_m / 2.0+ self.oy = centre[1] - span_m / 2.0+ self.lidar = np.zeros((self.n, self.n), dtype=np.int8) # what the lidar saw in its scan plane+ self.seen = np.zeros((self.n, self.n), dtype=bool)+ self._last_hit_xy = np.full((self.n, self.n, 2), np.nan, dtype=np.float32)++ # ---- coordinates -------------------------------------------------------+ def cell(self, x: np.ndarray, y: np.ndarray) -> tuple[np.ndarray, np.ndarray]:+ gx = np.floor((np.asarray(x) - self.ox) / self.res).astype(np.int64)+ gy = np.floor((np.asarray(y) - self.oy) / self.res).astype(np.int64)+ ok = (gx >= 0) & (gx < self.n) & (gy >= 0) & (gy < self.n)+ return gx[ok], gy[ok]++ # ---- learning ----------------------------------------------------------+ def _hit(self, layer: np.ndarray, xs: np.ndarray, ys: np.ndarray, from_xy: tuple[float, float]) -> int:+ gx, gy = self.cell(xs, ys)+ if len(gx) == 0:+ return 0+ flat = np.unique(gy * self.n + gx)+ gx, gy = flat % self.n, flat // self.n+ last = self._last_hit_xy[gy, gx]+ moved = np.isnan(last[:, 0]) | (np.hypot(last[:, 0] - from_xy[0], last[:, 1] - from_xy[1]) >= NEW_VIEWPOINT_M)+ cur = layer[gy, gx].astype(np.int16)+ # An obstacle is REAL when it was seen from two viewpoints: a+ # passer-by hammered from one parked spot must never become a wall.+ # From one viewpoint a cell rises to OCCUPIED_AT - 1 and no+ # further; the second viewpoint (0.10 m of motion) makes it a wall.+ # Driving toward anything real crosses viewpoints within a metre, so+ # legs and furniture still map on approach.+ # The viewpoint rule bounds the INCREASE, never the value: a+ # same-viewpoint hit adds nothing and takes nothing away. Clamping the+ # value instead would let one revolution from a parked rover push a+ # wall built from twenty viewpoints back below OCCUPIED_AT, and+ # occupancy() would publish free floor where the wall stands.+ new = np.where(moved, np.minimum(cur + 1, HIT_CAP), cur).astype(np.int8)+ layer[gy, gx] = new+ self._last_hit_xy[gy[moved], gx[moved]] = from_xy+ self.seen[gy, gx] = True+ return int(moved.sum())++ def _miss(self, layer: np.ndarray, gx: np.ndarray, gy: np.ndarray) -> None:+ if len(gx) == 0:+ return+ cur = layer[gy, gx].astype(np.int16)+ new = np.maximum(cur - 1, FREE_FLOOR).astype(np.int8)+ layer[gy, gx] = new+ self.seen[gy, gx] = True++ _revs: int = 0++ def lidar_revolution(self, hits_xy: np.ndarray, from_xy: tuple[float, float]) -> None:+ """World-frame lidar hits of one revolution, seen from `from_xy`:+ the cells along each ray (up to just short of the hit, RAY_MAX_M at+ most, every RAY_EVERY-th revolution) take a miss, the hit cells a hit."""+ if len(hits_xy) == 0:+ return+ self._revs += 1+ if self._revs % RAY_EVERY == 0:+ cells = self._ray_cells(from_xy, hits_xy)+ if cells is not None:+ self._miss(self.lidar, cells[0], cells[1])+ self._hit(self.lidar, hits_xy[:, 0], hits_xy[:, 1], from_xy)++ def _ray_cells(self, from_xy: tuple[float, float], hits_xy: np.ndarray):+ """Cells crossed by the rays from `from_xy` to each hit (capped at+ RAY_MAX_M), stopping one cell short of the hit; one sample per cell."""+ dx, dy = hits_xy[:, 0] - from_xy[0], hits_xy[:, 1] - from_xy[1]+ r = np.hypot(dx, dy)+ keep = r > self.res+ if not keep.any():+ return None+ dx, dy, r = dx[keep], dy[keep], r[keep]+ end = np.minimum(r - self.res, RAY_MAX_M)+ n_steps = np.floor(end / self.res).astype(int)+ nmax = int(n_steps.max())+ if nmax < 1:+ return None+ # (rays x steps) sample grid in one shot, masked past each ray's end+ d = (np.arange(1, nmax + 1) * self.res)[None, :]+ ux, uy = (dx / r)[:, None], (dy / r)[:, None]+ valid = d <= end[:, None]+ xs = (from_xy[0] + ux * d)[valid]+ ys = (from_xy[1] + uy * d)[valid]+ gx, gy = self.cell(xs, ys)+ if len(gx) == 0:+ return None+ flat = np.unique(gy * self.n + gx)+ return flat % self.n, flat // self.n++ def body_clear(self, pose: tuple) -> None:+ """The body IS here: every cell under the profile's body_clear_m+ footprint (half extents around base_link) is certainly free, and seen.+ Without it, cells written before the rover drove over them stay and+ can wall it in. Keep the footprint exact, never wider, so a wall+ right against the body survives."""+ x, y, yaw = pose+ c, s = np.cos(yaw), np.sin(yaw)+ hx, hy = _current_profile().body_clear_m+ lx = np.arange(-hx, hx + 1e-9, self.res / 2)+ ly = np.arange(-hy, hy + 1e-9, self.res / 2)+ bx, by = np.meshgrid(lx, ly)+ gx, gy = self.cell((x + c * bx - s * by).ravel(), (y + s * bx + c * by).ravel())+ if len(gx):+ self.lidar[gy, gx] = FREE_FLOOR+ self.seen[gy, gx] = True++ # ---- output ------------------------------------------------------------+ def occupancy(self) -> np.ndarray:+ """int8 grid: 100 occupied, 0 free (observed), -1 unknown."""+ out = np.full((self.n, self.n), -1, dtype=np.int8)+ out[self.seen] = 0+ out[self.lidar >= OCCUPIED_AT] = 100+ return out++ def value_at(self, x: float, y: float) -> int:+ gx, gy = self.cell(np.array([x]), np.array([y]))+ return int(self.occupancy()[gy[0], gx[0]]) if len(gx) else -1++ def cropped(self, margin_cells: int = 20) -> tuple[np.ndarray, float, float] | None:+ """The occupancy grid cropped to the observed area (+ margin): (grid, origin_x, origin_y)."""+ if not self.seen.any():+ return None+ ys, xs = np.nonzero(self.seen)+ y0, y1 = max(0, ys.min() - margin_cells), min(self.n, ys.max() + margin_cells + 1)+ x0, x1 = max(0, xs.min() - margin_cells), min(self.n, xs.max() + margin_cells + 1)+ return self.occupancy()[y0:y1, x0:x1], self.ox + x0 * self.res, self.oy + y0 * self.res+++def lidar_returns(pts: np.ndarray) -> np.ndarray:+ """Which points of the `lidar` cloud are lidar returns.++ lidar_odometry builds each revolution flat (z = 0) and lifts it by the+ profile's lidar_height_m, so every lidar return carries that height to+ the last bit of a float32. Any other point the cloud carries (a depth+ camera's, at its true height) can sit within millimetres of the scan+ plane, so the tag is read as one exact value (LIDAR_Z_TOL_M), never as a+ band.+ """+ return np.abs(pts[:, 2] - _current_profile().lidar_height_m) < LIDAR_Z_TOL_M+++class MossCostMap(Module):+ """Replaces dimOS's CostMapper on MOSS.++ Ins: `lidar` (the world cloud from lidar_odometry, lidar returns tagged+ with z = profile.lidar_height_m exactly), `odom` (base pose in world),+ `explore_cmd` (the order that opens the map), `explore_done` and+ `stop_explore_cmd` (logged; the map keeps writing).+ Out: `global_costmap` (the stream name the planner and the explorer+ listen to)."""++ lidar: In[PointCloud2]+ odom: In[PoseStamped]+ explore_done: In[Bool] # explorer2 found no reachable frontier left: logged, the map keeps writing+ stop_explore_cmd: In[Bool] # exploration stopped on request (explore_ctl.py stop, the cockpit): logged, the map keeps writing+ explore_cmd: In[Bool] # the order to explore: opens the map before the first motion+ global_costmap: Out[OccupancyGrid]++ def __init__(self, world_frame: str = "world", **kwargs: Any) -> None:+ super().__init__(**kwargs)+ self.world_frame = world_frame+ self._grid: ScoredGrid | None = None+ self._pose_xy: tuple[float, float] | None = None+ self._pose_yaw = 0.0+ self._revolutions = 0+ self._last_clear: tuple | None = None # (x, y, yaw) of the last body_clear+ # The pre-mission gate, closed at construction: a rover parked with a+ # spinning lidar would write the room it waits in, from one spot. It+ # opens on MISSION_START_M of displacement or on explore_cmd, and stays+ # open for the rest of the run.+ self._mission_frozen = True+ self._boot_xy: tuple[float, float] | None = None # the departure reference: the first pose++ @rpc+ def start(self) -> None:+ super().start()+ logger.info(f"costmap up: {RESOLUTION_M} m cells, hit cap {HIT_CAP}, free floor {FREE_FLOOR}, occupied at {OCCUPIED_AT}")++ async def handle_odom(self, msg: PoseStamped) -> None:+ self._pose_xy = (float(msg.position.x), float(msg.position.y))+ q = msg.orientation+ self._pose_yaw = float(np.arctan2(2 * (q.w * q.z + q.x * q.y), 1 - 2 * (q.y * q.y + q.z * q.z)))+ if self._grid is None:+ self._grid = ScoredGrid(centre=self._pose_xy)+ logger.info(f"costmap: grid centred on ({self._pose_xy[0]:+.2f}, {self._pose_xy[1]:+.2f})")+ self._check_departure()+ if self._write_frozen():+ return # before the mission: not even body_clear+ yaw = self._pose_yaw+ lc = self._last_clear+ if (lc is None+ or (self._pose_xy[0] - lc[0]) ** 2 + (self._pose_xy[1] - lc[1]) ** 2 > 0.03 ** 2+ or abs((yaw - lc[2] + np.pi) % (2 * np.pi) - np.pi) > np.radians(10.0)):+ self._last_clear = (self._pose_xy[0], self._pose_xy[1], yaw)+ self._grid.body_clear(self._last_clear)++ def _write_frozen(self) -> bool:+ """The ONE write gate: a hit, a miss or a body_clear needs the mission+ started (MISSION_START_M of displacement, or explore_cmd)."""+ return self._mission_frozen++ def _check_departure(self) -> None:+ """Open the gate once the rover has left its first pose.++ Cheap by design: two subtractions on a pose handle_odom already has,+ and only until it fires once.+ """+ if not self._mission_frozen or self._pose_xy is None:+ return+ if self._boot_xy is None:+ self._boot_xy = self._pose_xy+ return+ dx = self._pose_xy[0] - self._boot_xy[0]+ dy = self._pose_xy[1] - self._boot_xy[1]+ if dx * dx + dy * dy < MISSION_START_M * MISSION_START_M:+ return+ self._mission_frozen = False+ logger.info(f"costmap: the rover left its boot pose ({math.hypot(dx, dy) * 100:.0f} cm) - "+ "map writing OPEN for the mission")++ async def handle_explore_done(self, msg: Bool) -> None:+ """explorer2 ran out of reachable frontier. The explorer stops; the map+ keeps writing (the lidar map is the only obstacle source)."""+ if bool(getattr(msg, "data", False)):+ logger.info("costmap: exploration complete - map writing continues")++ async def handle_stop_explore_cmd(self, msg: Bool) -> None:+ """Exploration stopped on request (explore_ctl.py stop, the cockpit's+ STOP or a goal). The explorer stops; the map keeps writing. A map not+ opened yet still opens later, on displacement or a new explore_cmd."""+ if bool(getattr(msg, "data", False)):+ logger.info("costmap: exploration stopped - map writing continues")++ async def handle_explore_cmd(self, msg: Bool) -> None:+ """The order to explore opens the map, without waiting for motion.++ Displacement alone deadlocks a fresh map: no written cell -> no+ frontier -> no goal -> no motion -> the 5 cm never happen. The order+ comes just before the rover departs, which is the moment the gate+ waits for; the displacement check stays as the opener for runs+ driven without an explore order (gamepad, cockpit goals)."""+ if not bool(getattr(msg, "data", False)):+ return+ if self._mission_frozen:+ self._mission_frozen = False+ logger.info("costmap: explore_cmd received - map writing OPEN for the mission")++ async def handle_lidar(self, msg: PointCloud2) -> None:+ if self._grid is None or self._pose_xy is None or self._write_frozen():+ return+ pts = np.asarray(msg.as_numpy()[0], dtype=np.float64)+ if len(pts) == 0:+ return+ hits = pts[lidar_returns(pts)]+ if len(hits) == 0:+ return # no lidar return on this cloud: not a revolution+ self._grid.lidar_revolution(hits[:, :2], self._pose_xy)+ self._revolutions += 1+ if self._revolutions % PUBLISH_EVERY == 0:+ self._publish()++ def _publish(self) -> None:+ assert self._grid is not None+ crop = self._grid.cropped()+ if crop is None:+ return+ grid, ox, oy = crop+ origin = Pose()+ origin.position.x, origin.position.y, origin.position.z = ox, oy, 0.0+ origin.orientation.w = 1.0+ self.global_costmap.publish(OccupancyGrid(grid=grid, resolution=RESOLUTION_M, origin=origin,+ frame_id=self.world_frame, ts=time.time()))diff --git a/moss_dimos/explorer2.py b/moss_dimos/explorer2.pynew file mode 100644index 0000000..c2123d7--- /dev/null+++ b/moss_dimos/explorer2.py@@ -0,0 +1,1353 @@+"""Frontier exploration as a pure, replayable function.++Shape asked for upstream in dimensionalOS/dimos#1255 (design notes+in this docstring)++ next_target(costmap, pose, state) -> PoseStamped | None++`state` is the only memory (a plain dataclass), the function touches no LCM,+no clock it was not given and no robot, so a recording replays through it+unchanged. `None` is the ONLY way exploration ends.++Strategy: information gain per path cost, taken from dimensionalOS/dimos+PR #2830. What is reused verbatim from that PR, and what is not:++ reused the objective shape, multiplicative instead of the old weighted sum:+ score = info_gain / (1 + path_cost)+ score *= 1 + 0.5 * heading_alignment (its only numeric weight)+ score *= revisit_fade (linear there, inside a radius)+ reused info gain = frontier cluster cell count, normalised by+ min_frontier_perimeter / resolution * 10+ reused an unreachable cluster scores -inf and is dropped, never published+ reused unknown space is priced near-lethal so a route may cut a corner+ through it but never prefers it (its unknown_traversal_penalty=0.95)+ dropped the old 0.3 info + 0.3 distance-from-explored-goals + 0.2+ lookahead-distance + 0.15 obstacle-distance + 0.05 momentum sum.+ The 0.3 on "far from explored goals" is the map-jumping that+ #1255 was opened about; the lookahead term peaked at 5 m and taxed+ every nearby frontier.+ dropped its info_gain_threshold / num_no_gain_attempts self-stop, and the+ loop's "10 consecutive failures" give-up. Both are timers, and a+ timer can end a run with valid clusters still on the map.+ changed path cost is ONE Dijkstra from the robot over the whole traversable+ grid instead of one A* per cluster: same currency, N clusters for+ the price of one search.+ changed the target is a free, reachable cell standing off the cluster, not+ the cluster centroid in unknown space. The lidar (RPLIDAR C1)+ reaches 12 m; the robot does not need to enter the unknown to see+ it, and a goal it can actually stand on is a goal it can actually+ reach (goals placed in the unknown were measured reaching 1 in 29).+ changed a cluster is offered when there is somewhere the body can STAND+ within the standoff and look at it, not only when the frontier+ cell itself touches the region the body can reach: standing in a+ doorway maps the room behind it (see _clusters).+ changed information gain is the unknown AREA a revolution from the viewpoint+ would actually reveal - unknown cells in LINE OF SIGHT of it, within+ Tuning.info_radius_m - not the length of the frontier. Dividing a+ boundary length by path cost makes the robot take the nearest door+ every time; measured offline on a recorded indoor map, that myopia+ cost +71 % travel.+ changed the revisit fade is quadratic in the distance (what a second look+ can add is an AREA) and spares the goal just attempted, which is+ unfinished business rather than a revisit (see next_target).+ added a cluster reachable only across unmapped ground is a PROBE and is+ ranked behind every safe frontier (Tuning.probe_penalty).+ added a viewpoint the robot has already decided from is never handed out+ again (Tuning.observed_radius_m): driving to a spot it is already at+ reveals nothing. That is geometry, not a counter, and it is what+ keeps a frontier no viewpoint can resolve from being retargeted for+ ever (measured: 300 goals and 184 m of travel for the last 0.2 m2).++Field additions:++ failed-target memory+ A 0.6 m exclusion that NEVER causes extinction and holds no clock: a+ failed spot stays excluded until the WORLD reopens it - the map around+ the goal changed, or the robot looks from a viewpoint a metre from where+ it failed (triggers, never arbitrary timers). When every cluster is+ excluded the function returns a WAIT directive, not None. Starvation+ escape: a motionless robot cannot change the world, so after+ WAIT_REOPEN_POLLS consecutive WAIT answers the OLDEST failed entry is+ reopened - the world stayed silent, so the oldest question is asked+ again. A count of asks, not a wall clock.+ born-cornered detection+ No reachable frontier AND a reachable free area under 0.5 m2 -> one+ back-off directive, before anything else. The same one back-off answers+ the other way a robot can be shut in: no reachable frontier while the+ MAP still holds frontier clusters. Pinched is not finished, and the two+ are never reported with the same words.+ prefer-forward+ On the robot's real heading: in discovery mode the camera looks ahead,+ so a target behind costs a turn and a blind side.+ blocked cells+ No code here: whatever the map publishes as occupied (100) is inflated+ like a wall, so no target lands within a body radius of it.++The three directives all come back as a pose (the signature stays the upstream one);+which one it is reads off `target.directive`.++The decisions are checked offline by benches/bench_explorer2_cold.py.+"""++from __future__ import annotations++import math+import time+from dataclasses import dataclass, field, replace+from typing import Any++import numpy as np+from scipy import ndimage+from scipy.sparse import csr_matrix+from scipy.sparse.csgraph import dijkstra++# --- types -----------------------------------------------------------------+#+# The pure function must import with no dimOS installed (its bench and any+# offline replay run wherever numpy and scipy do). dimOS's own messages are+# used when they are there; otherwise a minimal stand-in with the same+# attribute names, which is all this module and its callers ever touch.++try: # pragma: no cover - exercised by whichever machine runs this+ from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped++ HAVE_DIMOS = True+except ImportError: # no dimOS: replay/offline+ HAVE_DIMOS = False++ class _XYZ:+ __slots__ = ("x", "y", "z")++ def __init__(self, x: float = 0.0, y: float = 0.0, z: float = 0.0) -> None:+ self.x, self.y, self.z = float(x), float(y), float(z)++ class _Quat:+ __slots__ = ("x", "y", "z", "w")++ def __init__(self, x: float = 0.0, y: float = 0.0, z: float = 0.0, w: float = 1.0) -> None:+ self.x, self.y, self.z, self.w = float(x), float(y), float(z), float(w)++ class PoseStamped: # type: ignore[no-redef]+ """Only what next_target reads and writes: position, orientation, frame, ts."""++ def __init__(self, ts: float | None = None, frame_id: str = "") -> None:+ self.position = _XYZ()+ self.orientation = _Quat()+ self.frame_id = frame_id+ self.ts = time.time() if ts is None else float(ts)++ def __repr__(self) -> str:+ return (f"PoseStamped(x={self.position.x:.3f}, y={self.position.y:.3f}, "+ f"frame_id={self.frame_id!r})")+++# --- grid conventions ------------------------------------------------------++FREE = 0+UNKNOWN = -1+OCCUPIED = 100+OCCUPANCY_THRESHOLD = 99 # dimOS WavefrontConfig.occupancy_threshold; costmap2d+ # publishes 100 for obstacles++_EIGHT = np.ones((3, 3), dtype=bool)+_SQRT2 = math.sqrt(2.0)+# (dy, dx, step length in cells)+_NEIGHBOURS = ((-1, -1, _SQRT2), (-1, 0, 1.0), (-1, 1, _SQRT2),+ (0, -1, 1.0), (0, 1, 1.0),+ (1, -1, _SQRT2), (1, 0, 1.0), (1, 1, _SQRT2))+++# --- directives ------------------------------------------------------------++DIRECTIVE_FRONTIER = "frontier" # drive here (goal_request)+DIRECTIVE_WAIT = "wait" # stay, re-evaluate in `wait_s` - NOT the end (failed-target memory)+DIRECTIVE_BACK_OFF = "back_off" # cornered or pinched: reverse once, then re-evaluate++# How many of a cluster's nearest free viewpoints are tested for line of sight+# to it before the nearest one is taken anyway. Bounded on purpose: in the+# middle of a wide room every cell of the standoff window is a candidate, and+# an unbounded search would turn one decision into thousands of ray walks.+_LOS_CANDIDATES = 32++# The anti-revisit fade. Not a Tuning field: Tuning holds physical quantities,+# and this is the SHAPE of a preference, fixed by an argument rather than+# measured on the robot - what a second look at the same spot can add is the+# AREA the first revolution did not cover, and an area goes as the square of+# how far the rover moved.+_REVISIT_FADE_POWER = 2+++# --- tuning ----------------------------------------------------------------++@dataclass(frozen=True)+class Tuning:+ """Every knob, in physical units. Frozen so a replay cannot drift."""++ # Clearance. Same two radii the planner uses, so the explorer never picks a+ # goal the planner will refuse (moss_dimos/recovering_planner.py,+ # clearances(): lethal = width/2 + control margin, pivot = pivot+ # diameter / 2). The defaults describe a 0.50 m wide reference body with a+ # 0.78 m pivot diameter; the blueprint passes the robot's own values from+ # robot_profile.derived(). Checked against the planner at start-up.+ lethal_clearance_m: float = 0.30+ pivot_clearance_m: float = 0.39+ # Cost of driving one metre through the band the body fits in but cannot+ # pivot in, on top of the metre itself. Same 4th-power ramp shape as the+ # planner's gradient (recovering_planner.PIVOT_RAMP_EXPONENT): near zero at+ # the turning circle, all of it against the wall.+ pivot_cost_factor: float = 1.0+ pivot_ramp_exponent: int = 4+ # A metre of unknown counts as this many metres. dimOS's A* prices an+ # unknown cell at threshold * 0.95 against a free cell's 0 (PR #2830's+ # unknown_traversal_penalty), i.e. "only if there is nothing else"; in a+ # metres currency 4x is the same intent without forbidding it outright.+ unknown_cost_factor: float = 4.0++ # Frontiers. 0.3 m of perimeter is the blueprint's min_frontier_perimeter.+ min_frontier_perimeter_m: float = 0.30+ # A cluster the rover can only reach by betting on unmapped ground scores+ # this fraction of what a walk over seen floor scores. Not a ban: a probe+ # is how the next room gets found. Just last in the queue.+ probe_penalty: float = 0.25+ # Information gain is the UNKNOWN AREA the viewpoint can SEE within this+ # radius - _visible_unknown, one ray-cast revolution - not the length of the+ # frontier. PR #2830 uses the cluster cell count, which is the boundary's+ # length: a doorway into an unmapped room and a doorway into a cupboard have+ # the same one, and a strategy that divides it by path cost then always+ # takes the nearest door. Measured offline on a recorded indoor map, that+ # myopia cost +71 % travel against the weighted-sum scoring, whose 4 m+ # "lookahead distance" happened to compensate for it. Area answers the question+ # actually being asked - how much map is behind this door - and line of+ # sight is what keeps the answer honest at a wall (see _visible_unknown).+ info_radius_m: float = 2.0+ # How far from the cluster the rover is asked to stand. It only has to SEE+ # the unknown (RPLIDAR C1, 12 m), not enter it.+ standoff_max_m: float = 1.00++ # Scoring.+ forward_bonus: float = 0.5 # PR #2830's momentum weight, on the real heading+ revisit_radius_m: float = 1.0 # within 1 m of a goal already published this+ # run, the score fades as the square of the+ # distance (_REVISIT_FADE_POWER)+ # Hard version of the same idea, and the only thing besides "no frontier"+ # that can end a run. A place the rover has ALREADY decided from - i.e.+ # already taken a full lidar revolution at - cannot be made to give up more+ # by going back to it: the rover would not move. That is a geometric fact,+ # not an attempt counter, and it is what the upstream "nothing reachable AND worth+ # it" means. Without it a frontier that no viewpoint can resolve (a shadow+ # the 2D scan plane never enters) is retargeted forever: measured offline,+ # 300 goals and 184 m of travel for the last 0.2 m2 of map.+ #+ # It is a rule about VIEWPOINTS, and only about viewpoints. A cluster whose+ # nearest viewpoint is spent still has the rest of its standoff window to be+ # looked at from; what retires the cluster itself is stated in _clusters.+ observed_radius_m: float = 0.30 # the follower's arrival tolerance (0.25 m) + a cell++ # Failed-target memory: a 0.6 m radius around a failed goal. There is no+ # hold time (that would be an arbitrary clock): an exclusion lifts on a+ # TRIGGER - the unknown-cell signature around the goal changed, or the+ # robot stands this far from where it failed (a new viewpoint on the+ # same spot).+ failed_goal_radius_m: float = 0.6+ failed_goal_moved_m: float = 1.0++ # The body is physically where it is: the cells under it are passable even+ # when the inflation says otherwise, or a rover that has driven into a pinch+ # can never be planned out of it. Same 0.25 m (half a 0.50 m body) that+ # dimOS's GlobalPlanner._clear_robot_footprint uses before every A*.+ footprint_clear_m: float = 0.25++ # Born-cornered. back_off_m places the back-off pose the directive carries;+ # the motion itself is the planner's contact escape (Explorer2.bump).+ cornered_area_m2: float = 0.5+ back_off_m: float = 0.22+++DEFAULT_TUNING = Tuning()++# Starvation escape: consecutive WAIT answers before the oldest failed entry is+# reopened. Counts ASKS (the loop polls once per WAIT_POLL_S), not wall time -+# a replay driving next_target by hand starves and recovers identically.+WAIT_REOPEN_POLLS = 12+++# --- state -----------------------------------------------------------------++@dataclass+class ExploreState:+ """The only memory of the exploration. Plain data: pickles, replays, diffs.++ `next_target` writes ONLY these fields, and only these:+ visited appended with every frontier target it hands out+ observed appended with the pose it was called from (deduplicated);+ every entry is a place the rover really stood, and only+ these retire a frontier+ failed entries whose reopening trigger fired are pruned (the+ loop appends new ones)+ heading refreshed from the pose it was given+ back_off_issued set when a back-off goes out, cleared when a frontier does+ last_directive what it just returned ("" if None)+ targets_issued count of frontier targets handed out+ Nothing else in the process is touched: no globals, no costmap writes.+ """++ visited: list[tuple[float, float]] = field(default_factory=list)+ # Where the rover has actually stood and decided - one full lidar revolution+ # each. Appended by next_target itself from the pose it is given, so it stays+ # true without the loop having to report anything.+ observed: list[tuple[float, float]] = field(default_factory=list)+ # (goal_x, goal_y, robot_x_at_failure, robot_y_at_failure, unknown_sig)+ failed: list[tuple[float, float, float, float, int]] = field(default_factory=list)+ heading: float = 0.0+ back_off_issued: bool = False+ last_directive: str = ""+ targets_issued: int = 0+ wait_streak: int = 0 # consecutive WAITs; WAIT_REOPEN_POLLS triggers the starvation escape++ def note_failed(self, x: float, y: float, robot_xy: tuple[float, float],+ costmap: Any, radius_m: float = DEFAULT_TUNING.failed_goal_radius_m) -> None:+ """The planner gave up on this goal. Called by the loop, not by next_target.++ Stores where the rover STOOD when it failed and the unknown-cell+ signature around the goal: the two things whose change reopens it."""+ self.failed.append((float(x), float(y), float(robot_xy[0]), float(robot_xy[1]),+ unknown_signature(costmap, x, y, radius_m)))++ def copy(self) -> "ExploreState":+ return replace(self, visited=list(self.visited), failed=list(self.failed),+ observed=list(self.observed))+++def unknown_signature(costmap: Any, x: float, y: float, radius_m: float) -> int:+ """Number of UNKNOWN cells within radius_m of (x, y) on this costmap.++ The reopening trigger of a failed-goal exclusion: cells once observed stay+ observed, so this count only moves when genuinely new information lands+ around the failed goal - sensor noise flipping free<->occupied does not+ touch it. Computed against world coordinates, so a moving crop origin+ does not shift it; a goal that falls OFF the published crop reads 0,+ which at worst reopens it early (one retry, one re-exclusion)."""+ grid = np.asarray(costmap.grid)+ res = float(costmap.resolution)+ ox = float(costmap.origin.position.x)+ oy = float(costmap.origin.position.y)+ h, w = grid.shape+ r = max(1, int(round(radius_m / res)))+ cx = int(math.floor((x - ox) / res))+ cy = int(math.floor((y - oy) / res))+ x0, x1 = max(0, cx - r), min(w, cx + r + 1)+ y0, y1 = max(0, cy - r), min(h, cy + r + 1)+ if x0 >= x1 or y0 >= y1:+ return 0+ return int((grid[y0:y1, x0:x1] == -1).sum())+++# --- geometry helpers ------------------------------------------------------++def _yaw_of(pose: Any, fallback: float) -> float:+ """Heading in the costmap frame, from the pose quaternion."""+ q = getattr(pose, "orientation", None)+ if q is None:+ return fallback+ x, y, z, w = float(q.x), float(q.y), float(q.z), float(q.w)+ if x * x + y * y + z * z + w * w < 1e-9:+ return fallback+ return math.atan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y * y + z * z))+++def _target_pose(x: float, y: float, *, frame_id: str, ts: float,+ directive: str, **extra: Any) -> PoseStamped:+ p = PoseStamped(ts=ts, frame_id=frame_id)+ p.position.x, p.position.y, p.position.z = float(x), float(y), 0.0+ p.orientation.x = p.orientation.y = p.orientation.z = 0.0+ p.orientation.w = 1.0+ p.directive = directive # type: ignore[attr-defined]+ for key, value in extra.items():+ setattr(p, key, value)+ return p+++def _disc(shape: tuple[int, int], centre: tuple[int, int], radius_cells: float) -> np.ndarray:+ """Boolean disc of `radius_cells` around `centre`, clipped to the grid."""+ cy, cx = centre+ r = int(math.ceil(radius_cells))+ h, w = shape+ y0, y1 = max(0, cy - r), min(h, cy + r + 1)+ x0, x1 = max(0, cx - r), min(w, cx + r + 1)+ out = np.zeros(shape, dtype=bool)+ yy = np.arange(y0, y1)[:, None] - cy+ xx = np.arange(x0, x1)[None, :] - cx+ out[y0:y1, x0:x1] = (yy ** 2 + xx ** 2) <= radius_cells ** 2+ return out+++def _nearest_true(mask: np.ndarray, yx: tuple[int, int]) -> tuple[int, int] | None:+ """Cell of `mask` closest to `yx`, Euclidean. None if the mask is empty."""+ if not mask.any():+ return None+ y, x = yx+ if mask[y, x]:+ return y, x+ _, idx = ndimage.distance_transform_edt(~mask, return_indices=True)+ return int(idx[0][y, x]), int(idx[1][y, x])+++# --- what a viewpoint can see ----------------------------------------------+#+# Two questions, one primitive: does a straight line between two cells cross an+# obstacle. "How much unknown would a lidar revolution here reveal" is that+# question asked once per ray; "was this frontier ever in view from there" is it+# asked once. Both are answered on the RAW occupancy - unknown never blocks a+# ray, because unknown is exactly what the rover is going to find out.++_RAYS: dict[int, tuple[np.ndarray, np.ndarray]] = {}+++def _rays(radius_cells: int) -> tuple[np.ndarray, np.ndarray]:+ """(dy, dx) of one revolution: enough rays that no cell of the disc is missed.++ One ray per cell of the outer circumference (2 pi r), sampled every cell+ outwards. Cached per radius, which is a constant of the tuning, so the whole+ cost is paid once per process.+ """+ key = int(radius_cells)+ cached = _RAYS.get(key)+ if cached is None:+ n_rays = max(8, int(math.ceil(2.0 * math.pi * key)))+ angles = np.linspace(0.0, 2.0 * math.pi, n_rays, endpoint=False)+ steps = np.arange(1, key + 1)+ dy = np.rint(np.sin(angles)[:, None] * steps[None, :]).astype(np.int32)+ dx = np.rint(np.cos(angles)[:, None] * steps[None, :]).astype(np.int32)+ _RAYS[key] = cached = (dy, dx)+ return cached+++def _visible_unknown(unknown: np.ndarray, occupied: np.ndarray,+ centre: tuple[int, int], radius_cells: int) -> float:+ """Unknown cells in LINE OF SIGHT of `centre`, as a fraction of its disc.++ This is the information gain, and it is the answer to the question actually+ being asked: how much of what I do not know would ONE revolution from that+ spot show me. A box filter over the unknown mask counts the unknown on the+ far side of the wall the robot would be standing against, and indoors that+ is most of it: on a recorded map the box gave a frontier against a wall+ 0.514 and the ray-cast gives it 0.327, against 0.70-0.78 for the frontiers+ that open onto unmapped rooms.+ """+ dy, dx = _rays(radius_cells)+ h, w = unknown.shape+ cy, cx = centre+ ys, xs = cy + dy, cx + dx+ inside = (ys >= 0) & (ys < h) & (xs >= 0) & (xs < w)+ ysc = np.clip(ys, 0, h - 1)+ xsc = np.clip(xs, 0, w - 1)+ blocked = occupied[ysc, xsc] | ~inside+ steps = blocked.shape[1]+ first = np.where(blocked.any(axis=1), blocked.argmax(axis=1), steps)+ open_run = np.arange(steps)[None, :] < first[:, None]+ hit = open_run & inside & unknown[ysc, xsc]+ if not hit.any():+ return 0.0+ # rays overlap near the centre: count CELLS, not ray samples+ seen = np.zeros((2 * radius_cells + 1, 2 * radius_cells + 1), dtype=bool)+ seen[dy[hit] + radius_cells, dx[hit] + radius_cells] = True+ return float(seen.sum()) / (math.pi * radius_cells * radius_cells)+++def _line_of_sight(occupied: np.ndarray, a: tuple[int, int], b: tuple[int, int]) -> bool:+ """True when no obstacle cell sits strictly between the two cells."""+ (y0, x0), (y1, x1) = a, b+ n = int(max(abs(y1 - y0), abs(x1 - x0)))+ if n <= 1:+ return True+ t = np.arange(1, n) / n+ ys = np.rint(y0 + (y1 - y0) * t).astype(np.int64)+ xs = np.rint(x0 + (x1 - x0) * t).astype(np.int64)+ return not bool(occupied[ys, xs].any())+++def _cells_of(points: list[tuple[float, float]], shape: tuple[int, int], res: float,+ origin: tuple[float, float]) -> np.ndarray:+ """World points -> an (n, 2) array of (row, col), clipped to the grid."""+ if not points:+ return np.zeros((0, 2), dtype=np.int64)+ ox, oy = origin+ h, w = shape+ ys = np.clip(np.floor((np.array([p[1] for p in points]) - oy) / res), 0, h - 1)+ xs = np.clip(np.floor((np.array([p[0] for p in points]) - ox) / res), 0, w - 1)+ return np.stack([ys, xs], axis=1).astype(np.int64)+++def _decided_from(observed: list[tuple[float, float]], shape: tuple[int, int], res: float,+ origin: tuple[float, float], radius_m: float) -> np.ndarray:+ """Cells within `radius_m` of a pose the rover has already decided from.++ A viewpoint inside this mask has had its lidar revolution: sending the rover+ back to it cannot show it anything, whatever the frontier it would be aimed+ at. That is the only thing the rule below is allowed to conclude.+ """+ mask = np.zeros(shape, dtype=bool)+ cells = _cells_of(observed, shape, res, origin)+ if not cells.size:+ return mask+ mask[cells[:, 0], cells[:, 1]] = True+ r = int(math.ceil(radius_m / res))+ if r <= 0:+ return mask+ yy, xx = np.ogrid[-r:r + 1, -r:r + 1]+ return ndimage.binary_dilation(mask, structure=(yy * yy + xx * xx <= r * r))+++# --- the map, read once ----------------------------------------------------++@dataclass+class _Survey:+ """Everything next_target needs to know about this costmap, computed once."""++ free: np.ndarray+ unknown: np.ndarray+ occupied: np.ndarray # obstacle cells, as published+ lethal: np.ndarray # body does not fit (obstacles, inflated)+ reachable: np.ndarray # free or unknown, body fits, connected to the robot+ goal_ok: np.ndarray # free and reachable: where a goal may sit+ walkable: np.ndarray # ... of which this part needs no bet on unmapped ground+ cell_cost: np.ndarray # cost multiplier per cell (1.0 = a plain free metre)+ seed: tuple[int, int] | None # the cell the robot plans from+ reachable_free_m2: float+++def _survey(grid: np.ndarray, res: float, robot_yx: tuple[int, int], tuning: Tuning) -> _Survey:+ occupied = grid >= OCCUPANCY_THRESHOLD+ free = grid == FREE+ unknown = grid == UNKNOWN++ # Metres to the nearest obstacle. Every cell published as 100 inflates+ # exactly like a wall, so no target can land within a body radius of it.+ distance_m = ndimage.distance_transform_edt(~occupied) * res+ # Strictly inside the radius, with the same float tolerance as the planner:+ # a cell sitting exactly ON the radius is the centre of a door the body+ # fits through, and rounding it away walls off doors the body can pass.+ lethal = distance_m + 1e-6 < tuning.lethal_clearance_m++ # 1. Where the rover physically is: the free+unknown region it sits in,+ # NOT inflated. Snapping across a wall would report a cornered robot+ # as being in the room next door.+ physical = free | unknown+ ry, rx = robot_yx+ if not physical[ry, rx]:+ snapped = _nearest_true(physical, (ry, rx))+ if snapped is None:+ empty = np.zeros_like(free)+ return _Survey(free, unknown, occupied, lethal, empty, empty, empty,+ np.ones(grid.shape), None, 0.0)+ ry, rx = snapped+ comp_labels, _ = ndimage.label(physical, structure=_EIGHT)+ component = comp_labels == comp_labels[ry, rx]++ # 2. Where the BODY can go inside that region, starting from under the+ # body itself. The footprint is cleared exactly as the planner clears it+ # before every A*: without that, a rover standing in a pinch has no+ # traversable cell of its own, and picking the nearest one by Euclidean+ # distance walks straight through the wall it is pinched against - the+ # explorer then certifies goals the planner answers "no path" to+ # (measured offline: 78 of 80 goals blocked, 7 m of travel).+ lethal = lethal.copy()+ footprint = _disc(lethal.shape, (ry, rx), tuning.footprint_clear_m / res)+ lethal[footprint] = False+ traversable = component & ~lethal+ if not traversable[ry, rx]:+ # The body is not even on free/unknown ground it fits in: nothing this+ # function can plan. That is the born-cornered signature, judged on the+ # pocket's own free area, not on the inflated one.+ empty = np.zeros_like(free)+ return _Survey(free, unknown, occupied, lethal, empty, empty, empty,+ np.ones(grid.shape), None,+ float((component & free).sum()) * res * res)+ seed = (ry, rx)+ trav_labels, _ = ndimage.label(traversable, structure=_EIGHT)+ reachable = trav_labels == trav_labels[seed]++ # Two reachability sets, and the difference between them is the whole+ # question "can I walk there, or am I hoping?":+ #+ # reachable free OR unknown, body fits, connected to the rover. This+ # is dimOS's own set (detect_frontiers floods free|unknown)+ # and it is what makes a frontier behind an unmapped pocket+ # visible at all - which it should be, that is exploration.+ # walkable the same, over floor the rover has actually SEEN. Every+ # cell the lidar marked free is joined to the rover by the+ # ray that marked it, so this set is what the rover can+ # reach without betting on unmapped ground.+ #+ # A cluster whose standing point is in `walkable` is a safe errand. One that+ # can only be got to across unmapped ground is a PROBE: still offered with a+ # proper free standing point, because refusing to try unmapped routes is+ # refusing to explore, but scored down (Tuning.probe_penalty) so every safe+ # frontier goes first. Measured offline: with probes ranked equal to safe+ # goals, one start pose spent 33 goals, 40 m and 183 direction reversals+ # driving into door frames.+ walk_labels, _ = ndimage.label((free | footprint) & ~lethal, structure=_EIGHT)+ walkable = walk_labels == walk_labels[seed]++ goal_ok = reachable & free+ reachable_free_m2 = float((walkable & free).sum()) * res * res++ # Cost per cell, in "metres per metre". Free and clear = 1.0.+ ramp = np.clip((tuning.pivot_clearance_m - distance_m)+ / max(tuning.pivot_clearance_m - tuning.lethal_clearance_m, 1e-6), 0.0, 1.0)+ cell_cost = (1.0 + tuning.pivot_cost_factor * ramp ** tuning.pivot_ramp_exponent)+ cell_cost = np.where(unknown, cell_cost * tuning.unknown_cost_factor, cell_cost)+ return _Survey(free, unknown, occupied, lethal, reachable, goal_ok, walkable,+ cell_cost.astype(np.float64), seed, reachable_free_m2)+++def _path_cost(reachable: np.ndarray, cell_cost: np.ndarray, seed: tuple[int, int],+ res: float) -> np.ndarray:+ """Geodesic cost in metres from `seed` to every reachable cell.++ One Dijkstra over the 8-connected grid, edge weight = mean cell cost x step+ length. PR #2830 runs one A* per cluster for the same number; a single+ multi-target search costs the same as its first one, which is what let the+ 16 s wavefront search go away for good.+ """+ h, w = reachable.shape+ out = np.full((h, w), np.inf)+ ys, xs = np.nonzero(reachable)+ n = int(ys.size)+ if n == 0:+ return out+ index = np.full((h, w), -1, dtype=np.int64)+ index[ys, xs] = np.arange(n)++ rows, cols, vals = [], [], []+ for dy, dx, length in _NEIGHBOURS:+ y0, y1 = max(0, -dy), h - max(0, dy)+ x0, x1 = max(0, -dx), w - max(0, dx)+ if y1 <= y0 or x1 <= x0:+ continue+ a = index[y0:y1, x0:x1]+ b = index[y0 + dy:y1 + dy, x0 + dx:x1 + dx]+ both = (a >= 0) & (b >= 0)+ if not both.any():+ continue+ ca = cell_cost[y0:y1, x0:x1][both]+ cb = cell_cost[y0 + dy:y1 + dy, x0 + dx:x1 + dx][both]+ rows.append(a[both])+ cols.append(b[both])+ vals.append(0.5 * (ca + cb) * length * res)++ if not rows:+ out[ys[0], xs[0]] = 0.0+ return out+ graph = csr_matrix((np.concatenate(vals),+ (np.concatenate(rows), np.concatenate(cols))), shape=(n, n))+ distances = dijkstra(graph, directed=True, indices=int(index[seed]))+ out[ys, xs] = distances+ return out+++# --- frontier clusters -----------------------------------------------------++@dataclass+class Cluster:+ """One frontier, with the place the rover would look at it from."""++ size: int # frontier cells (PR #2830's own gain proxy)+ gain: float # unknown cells in line of sight of the viewpoint,+ # as a fraction of its disc: the gain actually used+ centroid_xy: tuple[float, float]+ look_at_xy: tuple[float, float] # the cluster cell nearest the centroid+ goal_xy: tuple[float, float] # where to stand+ goal_yx: tuple[int, int]+ on_frontier: bool # True when no free standing spot was found+ probe: bool # getting there means betting on unmapped ground+ retired: bool = False # every viewpoint of it has already been used+ in_sight: bool = True # the viewpoint can actually SEE the frontier+++def _frontier_mask(survey: _Survey) -> np.ndarray:+ """dimOS's own frontier definition, kept as it is: an UNKNOWN cell with a+ FREE 8-neighbour and no occupied 8-neighbour."""+ near_occupied = ndimage.binary_dilation(survey.occupied, structure=_EIGHT)+ near_free = ndimage.binary_dilation(survey.free, structure=_EIGHT)+ return survey.unknown & near_free & ~near_occupied+++def _count_clusters(frontier: np.ndarray, min_cells: int) -> int:+ """How many frontier clusters the MAP holds, reachable or not.++ The difference between this number and the number of clusters the robot can+ plan to is the whole difference between "there is nothing left to see" and+ "I am pinched": see next_target. Confusing the two ends a run early, with+ clusters still on the map behind a pinch the body cannot pass.+ """+ if not frontier.any():+ return 0+ labels, count = ndimage.label(frontier, structure=_EIGHT)+ if count == 0:+ return 0+ sizes = np.bincount(labels.ravel())[1:]+ return int((sizes >= min_cells).sum())+++def _clusters(survey: _Survey, frontier_all: np.ndarray, grid_shape: tuple[int, int],+ res: float, origin: tuple[float, float], tuning: Tuning,+ decided_from: np.ndarray, observed_yx: np.ndarray) -> list[Cluster]:+ """The reachable frontier clusters, each with the viewpoint to look at it from.++ The body radius deliberately does NOT filter the frontier cells. A frontier+ is a measurement of unknown area, not a place to drive; narrowing it would+ throw away information and bring back the false extinctions. Where the rover+ stands to look at it is a separate question, answered below with+ `survey.goal_ok`, which is body-aware.++ `decided_from` are the cells the rover has already taken a lidar revolution+ at, and `observed_yx` those revolutions' centres. They are not allowed as+ viewpoints - driving to a spot the rover is effectively already at reveals+ nothing, which is geometry and not a counter.++ A cluster is RETIRED when the frontier itself has already been in CLEAR+ VIEW - line of sight, within standoff range - from a spot the rover stood+ at. If a full revolution with nothing in the way left those cells unknown,+ no second viewpoint at that scale is going to resolve them: that is the+ shadow the 2D scan plane never enters, and retargeting it forever is the+ 300-goals-and-184-m pathology the rule exists for. Retiring a cluster as+ soon as its NEAREST viewpoint had been used, whether or not anything of the+ frontier could be seen from there, would let a frontier round the corner+ from a spot the robot happened to stand at die silently.++ A cluster is offered when there is somewhere the BODY CAN STAND within+ standoff_max_m of it. That is the real question, and it is not the same as+ asking whether the frontier cell itself touches the region the body can+ reach: a robot in a hallway sees into every room through the doorways, and+ the inflated map may price those doorways a centimetre too narrow for the+ body. The lidar reaches 12 m: standing in a doorway maps the room, and+ that is what the robot has to do until a doorway is measured wide enough+ to drive through.+ """+ frontier = frontier_all+ if not frontier.any():+ return []++ labels, count = ndimage.label(frontier, structure=_EIGHT)+ if count == 0:+ return []+ min_cells = max(1, int(tuning.min_frontier_perimeter_m / res))+ boxes = ndimage.find_objects(labels)+ ox, oy = origin+ h, w = grid_shape+ pad = int(tuning.standoff_max_m / res) + 2+ standoff_cells = tuning.standoff_max_m / res+ info_cells = max(1, int(round(tuning.info_radius_m / res)))+ # "close enough for a revolution to have resolved it": one standoff, plus+ # the tolerance on where the rover actually came to a stop.+ seen_cells = (tuning.standoff_max_m + tuning.observed_radius_m) / res+ out: list[Cluster] = []+ for i, box in enumerate(boxes, start=1):+ if box is None:+ continue+ sub = labels[box] == i+ size = int(sub.sum())+ if size < min_cells:+ continue+ ys = np.nonzero(sub)[0] + box[0].start+ xs = np.nonzero(sub)[1] + box[1].start+ cy, cx = float(ys.mean()), float(xs.mean())++ # The point to LOOK AT: the cluster cell nearest the centroid, not the+ # centroid itself. A frontier that wraps around the known region (a+ # ring) has its centroid in the middle of the room, where there is+ # nothing to see; dimOS publishes that centroid as the goal and the+ # rover drives 5 cm. Snapping onto the cluster keeps the target on the+ # edge of what is known, which is the whole point of a frontier.+ rep = int(np.argmin((ys - cy) ** 2 + (xs - cx) ** 2))+ ry_c, rx_c = int(ys[rep]), int(xs[rep])++ # Has this frontier already been looked straight at, from close enough?+ seen_clearly = False+ if observed_yx.size:+ d2 = ((observed_yx[:, 0] - ry_c) ** 2 + (observed_yx[:, 1] - rx_c) ** 2)+ close = np.nonzero(d2 <= seen_cells * seen_cells)[0]+ for i in close[np.argsort(d2[close])]:+ if _line_of_sight(survey.occupied,+ (int(observed_yx[i, 0]), int(observed_yx[i, 1])), (ry_c, rx_c)):+ seen_clearly = True+ break++ # The place to STAND: a free, reachable, body-fits cell within+ # standoff_max_m of that look-at point, searched only in a window around+ # it so a free cell on the far side of a wall cannot be picked by+ # Euclidean luck. Of those candidates: never one the rover has already+ # decided from, and, among the rest, the nearest one that can actually+ # SEE the frontier - a viewpoint round the corner from the thing it is+ # sent to look at is a wasted goal.+ wy0, wy1 = max(0, ry_c - pad), min(h, ry_c + pad + 1)+ wx0, wx1 = max(0, rx_c - pad), min(w, rx_c + pad + 1)+ window = survey.goal_ok[wy0:wy1, wx0:wx1]+ gy = gx = None+ retired = in_sight = False+ if window.any():+ wys, wxs = np.nonzero(window)+ ys_abs, xs_abs = wys + wy0, wxs + wx0+ d2 = (ys_abs - ry_c) ** 2 + (xs_abs - rx_c) ** 2+ near = d2 <= standoff_cells * standoff_cells+ if near.any():+ ys_abs, xs_abs = ys_abs[near], xs_abs[near]+ order = np.argsort(d2[near], kind="stable")+ free_slots = [i for i in order if not decided_from[ys_abs[i], xs_abs[i]]]+ if not free_slots:+ # Not one viewpoint left that the rover is not already at.+ retired = True+ gy, gx = int(ys_abs[order[0]]), int(xs_abs[order[0]])+ else:+ pick = free_slots[0]+ for i in free_slots[:_LOS_CANDIDATES]:+ if _line_of_sight(survey.occupied, (ys_abs[i], xs_abs[i]), (ry_c, rx_c)):+ pick, in_sight = i, True+ break+ gy, gx = int(ys_abs[pick]), int(xs_abs[pick])+ # Retired when there is nothing more this rover can learn+ # about it from where it can stand: it has already been+ # looked straight at, or nowhere within the standoff can+ # see it at all. Both are read off THIS map, so both undo+ # themselves as soon as the map near it changes.+ retired = seen_clearly or not in_sight+ on_frontier = gy is None+ if on_frontier:+ if not survey.reachable[ry_c, rx_c]:+ # Nowhere to stand within standoff, and the frontier itself is+ # not in the region the body can reach either. This one is out+ # of reach from where the rover stands - counted by the caller+ # (a map that still holds frontiers is not an explored map),+ # never published, because the planner would refuse it.+ continue+ # Nowhere to stand within reach, but the frontier is inside the+ # region the body can reach: fall back to dimOS's own behaviour and+ # aim at the frontier cell itself, in unknown space. Harder to+ # reach, but dropping the cluster would be a silent extinction.+ gy, gx = ry_c, rx_c+ in_sight = True+ retired = seen_clearly+ # Cell CENTRES, not dimOS's grid_to_world corner. A corner is shared by+ # four cells and floor((corner - origin) / res) can land on any of them+ # (measured: floor(3.7 / 0.05) == 73, not 74), so a goal published on a+ # corner came back as the neighbouring cell - which was inside the+ # inflation, and the planner answered "no path" to a goal the explorer+ # had just certified as clear. A centre is half a cell from any edge.+ out.append(Cluster(+ size=size,+ gain=_visible_unknown(survey.unknown, survey.occupied, (gy, gx), info_cells),+ centroid_xy=(ox + (cx + 0.5) * res, oy + (cy + 0.5) * res),+ look_at_xy=(ox + (rx_c + 0.5) * res, oy + (ry_c + 0.5) * res),+ goal_xy=(ox + (gx + 0.5) * res, oy + (gy + 0.5) * res),+ goal_yx=(gy, gx),+ on_frontier=on_frontier,+ probe=on_frontier or not survey.walkable[gy, gx],+ retired=retired,+ in_sight=in_sight,+ ))+ return out+++# --- the pure function -----------------------------------------------------++def next_target(costmap: Any, pose: Any, state: ExploreState, *,+ now: float | None = None, tuning: Tuning = DEFAULT_TUNING) -> PoseStamped | None:+ """The next thing to do, or None when exploration is over.++ Args:+ costmap: anything with `.grid` (int8 HxW, 100/0/-1), `.resolution`,+ `.origin.position.x/.y` and optionally `.frame_id`/`.ts`. A dimOS+ OccupancyGrid is one; so is the stand-in the bench builds.+ Pass the RAW costmap - this function does its own inflation, and+ the base explorer's `simple_inflate(costmap, 0.25)` would double it.+ pose: the robot pose, `.position.x/.y` and `.orientation` (quaternion).+ state: the memory. Updated in place; see ExploreState.+ now: only stamps the returned pose when the costmap carries no ts.+ Defaults to time.monotonic(); nothing behavioural reads a clock.++ Returns:+ A pose carrying `.directive`:+ "frontier" drive there (this is a goal_request)+ "wait" stay put, re-evaluate in `.wait_s` seconds; a cluster is+ temporarily excluded, NOT absent+ "back_off" reverse `.back_off_m`; born cornered or pinched+ or None, which happens on exactly one condition: nothing reachable is+ left to look at and no exclusion is holding anything back - either the+ map holds no frontier cluster at all, or every one it holds has already+ been looked straight at from where the rover stood, or nowhere the body+ can stand can see it. No counters, no attempt limits, no timers; a rover+ that is merely shut in gets a back-off, not a None.+ """+ now = time.monotonic() if now is None else float(now)+ grid = np.asarray(costmap.grid)+ res = float(costmap.resolution)+ ox = float(costmap.origin.position.x)+ oy = float(costmap.origin.position.y)+ frame_id = getattr(costmap, "frame_id", "world")+ ts = float(getattr(costmap, "ts", now))++ rx, ry_world = float(pose.position.x), float(pose.position.y)+ heading = _yaw_of(pose, state.heading)+ state.heading = heading+ # This pose has just had a lidar revolution. Record it once per half-radius+ # so the list stays short over a long run.+ half = tuning.observed_radius_m / 2.0+ if all((rx - ox_) ** 2 + (ry_world - oy_) ** 2 >= half * half for ox_, oy_ in state.observed):+ state.observed.append((rx, ry_world))++ # An exclusion whose reopening trigger fired is gone: the map around the+ # goal changed (unknown-cell signature moved), or the rover now stands a+ # viewpoint away from where it failed. No clock anywhere. An exclusion is+ # never counted as "no frontier": that would end a run while clusters are+ # only waiting.+ moved2 = tuning.failed_goal_moved_m ** 2+ state.failed = [+ f for f in state.failed+ if (rx - f[2]) ** 2 + (ry_world - f[3]) ** 2 < moved2+ and unknown_signature(costmap, f[0], f[1], tuning.failed_goal_radius_m) == f[4]+ ]++ h, w = grid.shape+ gx0 = int(np.clip(math.floor((rx - ox) / res), 0, w - 1))+ gy0 = int(np.clip(math.floor((ry_world - oy) / res), 0, h - 1))++ survey = _survey(grid, res, (gy0, gx0), tuning)+ # A robot that starts walled in (often a map artefact around the start+ # pose) looks like a planner failure: say it loud the moment the map says+ # so, on every decision.+ if survey.reachable_free_m2 < 3.0:+ import logging+ logging.getLogger(__name__).warning(+ "WALLED-IN START? only %.2f m2 of reachable floor around the rover "+ "- check the map around the start pose before blaming the planner",+ survey.reachable_free_m2)+ frontier_all = _frontier_mask(survey)+ min_cells = max(1, int(tuning.min_frontier_perimeter_m / res))+ on_the_map = _count_clusters(frontier_all, min_cells)+ # A viewpoint is spent by STANDING there, and by nothing else. Counting a+ # goal that was merely issued would make this an attempt counter wearing a+ # geometric hat, and it shows: with issued goals spent too, offline runs+ # ended having given up on a frontier whose viewpoint the planner could+ # still reach from where the robot was standing when it stopped (3 of 4+ # starts). What stops a goal being re-issued for ever is the loop's+ # exclusion on a drive that made no progress - trigger-reopened, and one+ # that can only ever produce a WAIT.+ decided_from = _decided_from(state.observed, (h, w), res, (ox, oy),+ tuning.observed_radius_m)+ observed_yx = _cells_of(state.observed, (h, w), res, (ox, oy))+ clusters = ([] if survey.seed is None+ else _clusters(survey, frontier_all, (h, w), res, (ox, oy), tuning,+ decided_from, observed_yx))++ # --- nothing reachable: pinched, cornered, or actually finished ---------+ #+ # These are three different facts and must not be reported as the same+ # "no reachable frontier left":+ #+ # finished the MAP holds no frontier cluster at all. Nothing left to+ # see, and that is the one honest end of a run.+ # cornered no frontier and under cornered_area_m2 of floor: the robot is+ # wedged in a pocket (born cornered).+ # pinched the map still holds frontier clusters, but not one of them+ # touches the region the BODY can reach from where it stands.+ # That is not "explored", it is "shut in" - typically behind a+ # doorway the inflated map prices a little narrower than the+ # body needs, although the robot drove through it earlier.+ #+ # Cornered and pinched get the same answer: reverse once and look again.+ # Once per pocket - the flag is cleared only when a frontier goal goes+ # out - so this can never become a spin.+ if not clusters:+ if on_the_map > 0 or survey.reachable_free_m2 < tuning.cornered_area_m2:+ if not state.back_off_issued:+ state.back_off_issued = True+ state.last_directive = DIRECTIVE_BACK_OFF+ return _target_pose(rx - tuning.back_off_m * math.cos(heading),+ ry_world - tuning.back_off_m * math.sin(heading),+ frame_id=frame_id, ts=ts, directive=DIRECTIVE_BACK_OFF,+ back_off_m=tuning.back_off_m,+ reachable_free_m2=survey.reachable_free_m2,+ n_clusters=0, n_on_the_map=on_the_map)+ # Backed off once already and the picture has not changed, or the map+ # holds no frontier at all: that is the end.+ state.last_directive = ""+ return None++ # --- temporary exclusions ----------------------------------------------+ radius2 = tuning.failed_goal_radius_m ** 2+ eligible: list[Cluster] = []+ n_blocked = 0+ n_seen_from = 0+ for cluster in clusters:+ gx, gy = cluster.goal_xy+ if cluster.retired:+ # Every viewpoint within reach of this frontier has already had its+ # revolution. Not a failure and not a timer: driving to a spot the+ # rover is effectively already at reveals nothing, wherever it is+ # then asked to look.+ n_seen_from += 1+ continue+ if any((gx - f[0]) ** 2 + (gy - f[1]) ** 2 < radius2 for f in state.failed):+ n_blocked += 1+ else:+ eligible.append(cluster)++ if not eligible and not n_blocked:+ # Every cluster is one the rover has already stood at and looked from.+ # Nothing reachable is worth going to: that is the end of the run.+ state.last_directive = ""+ return None++ if not eligible:+ # Every remaining cluster sits on a failed goal whose trigger has not+ # fired: the map around it has not changed and the rover has not found+ # a new viewpoint. Waiting is the answer, dying is not - and there is+ # no expiry to wait out: the loop polls, and the WORLD reopens the+ # spot. But a motionless robot cannot change the world, so after+ # WAIT_REOPEN_POLLS silent asks the OLDEST failed entry is reopened+ # and asked again.+ state.wait_streak += 1+ reopened = None+ if state.failed and state.wait_streak >= WAIT_REOPEN_POLLS:+ reopened = state.failed.pop(0)[:2]+ state.wait_streak = 0+ state.last_directive = DIRECTIVE_WAIT+ return _target_pose(rx, ry_world, frame_id=frame_id, ts=ts,+ directive=DIRECTIVE_WAIT, wait_s=0.0,+ reopened_xy=reopened,+ n_clusters=len(clusters), n_excluded=n_blocked,+ n_already_seen_from=n_seen_from, n_on_the_map=on_the_map)++ # --- score (information gain per path cost, PR #2830) -----------------+ costs = _path_cost(survey.reachable, survey.cell_cost, survey.seed, res)++ best: Cluster | None = None+ best_score = -math.inf+ best_cost = math.inf+ for cluster in eligible:+ gy, gx = cluster.goal_yx+ cost = float(costs[gy, gx]) if survey.reachable[gy, gx] else math.inf+ if not math.isfinite(cost):+ # Unreachable: PR #2830's -inf, dropped rather than published.+ continue+ score = cluster.gain / (1.0 + cost)+ if cluster.probe:+ score *= tuning.probe_penalty++ gxw, gyw = cluster.goal_xy+ dx, dy = gxw - rx, gyw - ry_world+ span = math.hypot(dx, dy)+ if span > 0.1: # under 10 cm a direction means nothing (dimOS uses the same floor)+ align = max(0.0, math.cos(heading) * dx / span + math.sin(heading) * dy / span)+ score *= 1.0 + tuning.forward_bonus * align++ # Anti-revisit: going back to a place is only worth it once the rest of+ # the map is done. A goal near one already published this run fades:+ #+ # how much as the square of the distance, because what a second+ # look can add is an AREA - the crescent the first+ # revolution did not cover - and an area goes as the+ # square of how far the robot moved. A linear fade is+ # too gentle to matter: a frontier 10 cm from an old+ # goal keeps a tenth of its score and can still win,+ # because a mapped-floor errand pays neither the probe+ # penalty nor the distance the unmapped rooms do.+ # which goals every goal published this run EXCEPT the one just+ # attempted. That last one is not a revisit, it is+ # unfinished business: fading it makes a robot that+ # timed out three metres short of a frontier turn round+ # and cross the map for the next-best thing, and then+ # do it again (measured offline: 25 m of travel over+ # four goals for 2 m2).+ older = state.visited[:-1]+ if older:+ nearest = min(math.hypot(gxw - vx, gyw - vy) for vx, vy in older)+ if nearest < tuning.revisit_radius_m:+ score *= (nearest / tuning.revisit_radius_m) ** _REVISIT_FADE_POWER++ if score > best_score:+ best, best_score, best_cost = cluster, score, cost++ if best is None:+ # Clusters exist but none is reachable from where the body can stand.+ state.last_directive = ""+ return None++ state.visited.append(best.goal_xy)+ state.targets_issued += 1+ state.wait_streak = 0+ state.back_off_issued = False # a new pocket later gets its own back-off+ state.last_directive = DIRECTIVE_FRONTIER+ return _target_pose(best.goal_xy[0], best.goal_xy[1], frame_id=frame_id, ts=ts,+ directive=DIRECTIVE_FRONTIER, score=best_score,+ path_cost_m=best_cost, info_cells=best.size, info_gain=best.gain,+ centroid_xy=best.centroid_xy, look_at_xy=best.look_at_xy,+ on_frontier=best.on_frontier, probe=best.probe,+ in_sight=best.in_sight,+ n_clusters=len(clusters), n_excluded=n_blocked,+ n_already_seen_from=n_seen_from, n_on_the_map=on_the_map)+++# ===========================================================================+# The module: a loop around next_target, and nothing else.+# ===========================================================================++if HAVE_DIMOS: # pragma: no cover - needs the dimOS stack+ from typing import Any as _Any++ from dimos.core.core import rpc+ from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid+ from dimos.navigation.frontier_exploration.wavefront_frontier_goal_selector import (+ WavefrontConfig, WavefrontFrontierExplorer,+ )+ from dimos.utils.logging_config import setup_logger+ from dimos.core.stream import Out+ from dimos_lcm.std_msgs import Bool++ logger = setup_logger()++ WAIT_POLL_S = 1.0 # longest single sleep on a WAIT: the map keeps growing+ BACK_OFF_SETTLE_S = 3.0 # the planner's back-up is ~2 s at 0.10 m/s over 0.20 m+ # A goal that times out having closed less than this much of the gap is not+ # "still on its way", it is stuck against something, and re-publishing it+ # unchanged is how a robot spends 19 goals and 30 m in one corner+ # (measured offline). It becomes an exclusion, which holds until the map+ # around it changes or the robot finds a new viewpoint and can only ever+ # produce a WAIT - never an end of run. A goal that IS closing the gap is+ # left alone: at exploration speed a 7 m goal simply takes longer than+ # goal_timeout.+ GOAL_PROGRESS_M = 0.25 # the follower's own arrival tolerance++ class Explorer2Config(WavefrontConfig):+ """WavefrontConfig so the blueprint stays drop-in. The fields this+ explorer ignores (info_gain_threshold, num_no_gain_attempts, lookahead_distance,+ max_explored_distance, safe_distance) are the self-stop timers and the+ weighted-sum terms this explorer drops: do not pass them."""++ lethal_clearance_m: float = DEFAULT_TUNING.lethal_clearance_m+ pivot_clearance_m: float = DEFAULT_TUNING.pivot_clearance_m+ unknown_cost_factor: float = DEFAULT_TUNING.unknown_cost_factor+ standoff_max_m: float = DEFAULT_TUNING.standoff_max_m+ info_radius_m: float = DEFAULT_TUNING.info_radius_m+ probe_penalty: float = DEFAULT_TUNING.probe_penalty+ footprint_clear_m: float = DEFAULT_TUNING.footprint_clear_m+ observed_radius_m: float = DEFAULT_TUNING.observed_radius_m+ forward_bonus: float = DEFAULT_TUNING.forward_bonus+ revisit_radius_m: float = DEFAULT_TUNING.revisit_radius_m+ failed_goal_radius_m: float = DEFAULT_TUNING.failed_goal_radius_m+ failed_goal_moved_m: float = DEFAULT_TUNING.failed_goal_moved_m+ cornered_area_m2: float = DEFAULT_TUNING.cornered_area_m2+ back_off_m: float = DEFAULT_TUNING.back_off_m++ class Explorer2(WavefrontFrontierExplorer):+ """Drop-in for the wavefront explorer: same In/Out names, same LCM+ topics, same rpc and skill surface (subclassing is what guarantees it).+ Everything that decides anything lives in `next_target`; what is left+ here is publish, wait, remember.++ Extra Out: `bump` - the back-off directive (born cornered or pinched).+ RecoveringPlanner listens to it: stop, escape 0.20 m in reverse,+ abandon the goal. Any other contact source can publish on the same+ stream.++ Extra Out: `explore_done` - the end of the mission on the bus.+ """++ config: Explorer2Config++ bump: Out[Bool]+ # The end of the mission, said OUT LOUD on the bus. The base loop only+ # sets exploration_active False, logs one line and breaks, so every+ # other process would have to infer the end from silence: dimOS has no+ # other producer, status stream or upstream Out for it. The cockpit+ # reads it to show the end of the run. ONE publish per run, and only on+ # this exit: the explicit stops already cross the boundary on+ # `stop_explore_cmd`.+ explore_done: Out[Bool]++ def __init__(self, **kwargs: _Any) -> None:+ super().__init__(**kwargs)+ self._state = ExploreState()+ self._last_goal: tuple[float, float] | None = None+ self._last_goal_ok: bool | None = None+ # True while a goal that timed out unanswered can still be answered.+ # goal_reached is a bare Bool with no goal identity in it, and the+ # planner's monitor thread (10 Hz) can cancel the OLD goal at any+ # point after the new one has been published.+ self._late_answer_owed = False+ self._tuning = Tuning(+ lethal_clearance_m=self.config.lethal_clearance_m,+ pivot_clearance_m=self.config.pivot_clearance_m,+ unknown_cost_factor=self.config.unknown_cost_factor,+ min_frontier_perimeter_m=self.config.min_frontier_perimeter,+ info_radius_m=self.config.info_radius_m,+ probe_penalty=self.config.probe_penalty,+ standoff_max_m=self.config.standoff_max_m,+ footprint_clear_m=self.config.footprint_clear_m,+ observed_radius_m=self.config.observed_radius_m,+ forward_bonus=self.config.forward_bonus,+ revisit_radius_m=self.config.revisit_radius_m,+ failed_goal_radius_m=self.config.failed_goal_radius_m,+ failed_goal_moved_m=self.config.failed_goal_moved_m,+ cornered_area_m2=self.config.cornered_area_m2,+ back_off_m=self.config.back_off_m,+ )+ _warn_if_planner_disagrees(self._tuning)++ # A goal_reached of False means "the planner gave up", and the stock+ # module ignores it and sleeps its whole goal_timeout for nothing.+ def _on_goal_reached(self, msg: Bool) -> None:+ if self._late_answer_owed:+ # The previous goal's verdict, landing after the current goal was+ # published. Crediting it would exclude a frontier the rover has+ # never driven to. At most one is owed: the planner cancels the+ # old goal once, then adopts the new one.+ self._late_answer_owed = False+ logger.info("late goal_reached: the previous goal's verdict, not this one's - "+ "the goal in flight keeps its own timeout")+ return+ self._last_goal_ok = bool(getattr(msg, "data", False))+ self.goal_reached_event.set()++ def _run_exploration_loop(self) -> None: # type: ignore[override]+ self._state = ExploreState()+ state = self._state+ while self.exploration_active and not self.stop_event.is_set():+ if self.latest_costmap is None or self.latest_odometry is None:+ self.stop_event.wait(0.5)+ continue++ costmap: OccupancyGrid = self.latest_costmap+ t0 = time.perf_counter()+ target = next_target(costmap, self.latest_odometry, state, tuning=self._tuning)+ elapsed_ms = (time.perf_counter() - t0) * 1000.0++ if target is None:+ logger.info(f"exploration complete: no reachable frontier left "+ f"({state.targets_issued} targets, {elapsed_ms:.0f} ms)")+ # said on the bus BEFORE the loop exits: consumers learn the+ # end at this instant, not whenever someone notices the+ # goals stopped coming.+ if self.explore_done.transport is not None:+ self.explore_done.publish(Bool(data=True))+ self.exploration_active = False+ break++ directive = getattr(target, "directive", DIRECTIVE_FRONTIER)++ if directive == DIRECTIVE_WAIT:+ reopened = getattr(target, "reopened_xy", None)+ if reopened:+ logger.warning(f"starvation escape: the world stayed silent for "+ f"{WAIT_REOPEN_POLLS} asks - reopening the oldest failed "+ f"goal at ({reopened[0]:.2f}, {reopened[1]:.2f})")+ else:+ logger.info(f"{getattr(target, 'n_excluded', 0)} of "+ f"{getattr(target, 'n_clusters', 0)} clusters are on a recently "+ "failed goal: waiting for the map or the viewpoint "+ "to change, not stopping")+ self.stop_event.wait(WAIT_POLL_S)+ continue++ if directive == DIRECTIVE_BACK_OFF:+ logger.warning(+ f"born cornered: {getattr(target, 'reachable_free_m2', 0.0):.2f} m2 of "+ f"reachable floor and no frontier - one "+ f"{getattr(target, 'back_off_m', 0.0):.2f} m back-off via the bump reflex")+ if self.bump.transport is not None:+ self.bump.publish(Bool(data=True))+ self.stop_event.wait(BACK_OFF_SETTLE_S)+ continue++ goal = PoseStamped()+ goal.position.x = target.position.x+ goal.position.y = target.position.y+ goal.position.z = 0.0+ goal.orientation.w = 1.0+ goal.frame_id = "world"+ goal.ts = costmap.ts+ # The goal being replaced timed out unanswered: the planner still+ # owes it a verdict, so the first goal_reached after this publish+ # belongs to it, not to the goal below. Swallow that one. The+ # cost when it was in fact this goal's own "no path" is one+ # goal_timeout instead of an instant wake - and the timeout+ # branch then decides on the gap actually closed, in metres.+ self._late_answer_owed = self._last_goal is not None and self._last_goal_ok is None+ self._last_goal = (float(goal.position.x), float(goal.position.y))+ self._last_goal_ok = None+ gap_at_issue = self._gap_to(self._last_goal)+ self.goal_reached_event.clear()+ self.goal_request.publish(goal)+ logger.info(+ f"goal {state.targets_issued}: ({goal.position.x:.2f}, {goal.position.y:.2f}) "+ f"{getattr(target, 'path_cost_m', float('nan')):.1f} m away, "+ f"{getattr(target, 'info_cells', 0)} frontier cells, "+ f"{getattr(target, 'n_clusters', 0)} clusters, chosen in {elapsed_ms:.0f} ms")++ arrived = self.goal_reached_event.wait(timeout=self.config.goal_timeout)+ if arrived and self._last_goal_ok is False:+ # The planner gave up (no path, or stuck -> goal abandoned).+ # An exclusion, never a reason to stop; it reopens+ # when the map around it changes or the rover stands a+ # viewpoint away from here.+ self._note_failed(state)+ logger.info("planner gave up on that goal: excluded until "+ "the map or the viewpoint changes")+ elif not arrived:+ # Timed out mid-drive. Whether that is a failure depends on+ # one measurement: did the gap close (see GOAL_PROGRESS_M).+ closed = gap_at_issue - self._gap_to(self._last_goal)+ if closed < GOAL_PROGRESS_M:+ self._note_failed(state)+ logger.info(f"goal timeout after {self.config.goal_timeout:.0f} s having "+ f"closed {closed:.2f} m of {gap_at_issue:.2f} m: excluded until "+ "the map or the viewpoint changes")+ else:+ logger.info(f"goal timeout after {self.config.goal_timeout:.0f} s, "+ f"{closed:.2f} m closer, re-deciding")++ def _note_failed(self, state: ExploreState) -> None:+ """Exclude the goal that just failed, with its reopening triggers+ (where the rover stood + the unknown signature around the goal)."""+ odom = self.latest_odometry+ robot = ((float(odom.position.x), float(odom.position.y))+ if odom is not None else self._last_goal)+ state.note_failed(*self._last_goal, robot, self.latest_costmap,+ self._tuning.failed_goal_radius_m)++ def _gap_to(self, goal: tuple[float, float]) -> float:+ """Metres from where the rover is now to `goal`, or inf if unknown."""+ odom = self.latest_odometry+ if odom is None:+ return float("inf")+ return math.hypot(goal[0] - float(odom.position.x), goal[1] - float(odom.position.y))++ @rpc+ def explore_state(self) -> dict[str, _Any]:+ """The explorer's memory, for inspection from `dimos shell`."""+ return {"visited": list(self._state.visited),+ "failed": list(self._state.failed),+ "heading": self._state.heading,+ "targets_issued": self._state.targets_issued,+ "last_directive": self._state.last_directive}++ def _warn_if_planner_disagrees(tuning: Tuning) -> None:+ """The explorer picks goals the planner has to accept. If the two+ clearances ever drift apart, say so at start-up rather than discover it+ as 'no path' in the field."""+ try:+ # the planner's radii for the active robot profile, read now: the+ # blueprint passes the same profile's clearances to this explorer+ from moss_dimos.recovering_planner import clearances+ LETHAL_CLEARANCE_M, PIVOT_CLEARANCE_M = clearances()+ except Exception as exc: # noqa: BLE001 - the explorer still runs+ logger.warning(f"explorer2 clearance check skipped: planner radii unavailable "+ f"({type(exc).__name__}: {exc})")+ return+ if abs(tuning.lethal_clearance_m - LETHAL_CLEARANCE_M) > 1e-9:+ logger.warning(f"explorer2 lethal clearance {tuning.lethal_clearance_m} m != planner "+ f"{LETHAL_CLEARANCE_M} m: goals may be picked the planner refuses")+ if abs(tuning.pivot_clearance_m - PIVOT_CLEARANCE_M) > 1e-9:+ logger.warning(f"explorer2 pivot clearance {tuning.pivot_clearance_m} m != planner "+ f"{PIVOT_CLEARANCE_M} m")diff --git a/moss_dimos/gamepad.py b/moss_dimos/gamepad.pyindex c186f1a..124a259 100644--- a/moss_dimos/gamepad.py+++ b/moss_dimos/gamepad.py@@ -1,21 +1,51 @@-"""Gamepad teleop for MOSS as a dimOS module: reads a pygame joystick, publishes Twist.+"""Gamepad teleop for MOSS as a dimOS module: pygame.joystick in, Twist out.-All decision logic lives in `moss_dimos.teleop_logic` (pure, cold-tested here);-this file only wires it to pygame and to the dimOS stream. Runs headless (SDL-dummy drivers) and waits for a pad instead of failing at startup.+dimOS ships keyboard, phone and Quest teleop but no gamepad; this module+fills the gap. It publishes on ``tele_cmd_vel``: wire it through dimOS's+MovementManager so navigation and teleop coexist, or remap it to ``cmd_vel``+for direct drive.-Not yet run on the MOSS base: there is no motor driver module in this-repository yet (see docs/software.md).+Mapping (standard dual-stick):+ left stick Y -> vx (forward) left stick X -> vy (strafe)+ right stick X -> wz (rotation) R2 held -> boost+The tracked base ignores vy (moss_adapter); it is still published, so the+same module drives a holonomic base unchanged.++Publishes ONLY while the deadman (R1) is held, plus a brake of zeros right+after it is released - 0.5 s by default, up to 1.0 s with MOSS_BRAKE_S - then+silence (see BRAKE_S and resolve_brake_s). A pad lying at rest owns nothing,+so the same stack can explore on its own while the pad is plugged in.+Pressing the deadman again ends that brake window on the spot+(BRAKE_CANCEL_ON_DEADMAN), but only on a tick that is trusted and about to+publish - see the worker.++Wiring in this package's blueprints (moss_dimos.blueprints):+ moss-gamepad, moss-nav tele_cmd_vel is REMAPPED to cmd_vel: direct+ drive, no MovementManager in the path+ moss-explore tele_cmd_vel is NOT remapped: the Twist goes+ tele_cmd_vel -> MovementManager -> cmd_vel, and+ any pad message cancels the current nav goal++Runs headless: the robot's computer has no display, so SDL is pinned to its+dummy video/audio drivers below. The pad is hot-pluggable - the module waits+for one instead of giving up at startup, and goes back to waiting if it is+unplugged, so ``dimos run moss-dimos.moss-gamepad`` can be started before the+receiver is in."""from __future__ import annotations+from dataclasses import dataclassimport osimport threadingimport time+from typing import Any+# SDL picks its drivers at import time, so this has to run before "import+# pygame" (which happens lazily in the worker thread below). setdefault, not+# assignment: an operator who exports SDL_VIDEODRIVER=x11 keeps their display.os.environ.setdefault("SDL_VIDEODRIVER", "dummy")os.environ.setdefault("SDL_AUDIODRIVER", "dummy")-os.environ.setdefault("PYGAME_HIDE_SUPPORT_PROMPT", "1")+os.environ.setdefault("PYGAME_HIDE_SUPPORT_PROMPT", "1") # no banner in daemon logsfrom dimos.core.core import rpcfrom dimos.core.module import Module@@ -24,74 +54,433 @@ from dimos.msgs.geometry_msgs.Twist import Twistfrom dimos.msgs.geometry_msgs.Vector3 import Vector3from dimos.utils.logging_config import setup_logger-from .teleop_logic import DeadmanBrake, TeleopConfig, axes_to_twist-logger = setup_logger()-# Standard dual-stick pad (pygame numbering). Override through the constructor.+# Generic defaults. The MOSS blueprints always pass the base's own scale+# (moss_adapter.teleop_speeds), so these are only the fallback.+DEFAULT_LINEAR_SPEED = 0.6 # m/s at full stick+DEFAULT_ANGULAR_SPEED = 1.2 # rad/s at full stick+DEFAULT_DEADZONE = 0.32+DEFAULT_RATE_HZ = 50.0+DEFAULT_BOOST = 2.0+RESCAN_PERIOD_S = 2.0 # how often we re-scan SDL for a pad++# SDL axis indices on a standard dual-stick pad (DS4/DS5, Xbox, 8BitDo).+AXIS_LEFT_X = 0AXIS_LEFT_Y = 1-AXIS_RIGHT_X = 2-BUTTON_DEADMAN = 4 # L1-BUTTON_BOOST = 5 # R1+AXIS_RIGHT_X = 2 # measured: on the 2563:0526 pad in Android mode, right-stick X sweeps axis 2 over its full travel (axis 3 is its vertical, which leaks a little)+AXIS_R2 = 5+++@dataclass(frozen=True)+class TeleopConfig:+ """Speed envelope for :func:`axes_to_twist` - no pygame, no module."""++ linear_speed: float = DEFAULT_LINEAR_SPEED+ angular_speed: float = DEFAULT_ANGULAR_SPEED+ deadzone: float = DEFAULT_DEADZONE+ boost_multiplier: float = DEFAULT_BOOST+ envelope: Any = None # TwistEnvelope (defined below); None = DEFAULT_ENVELOPE+++def apply_deadzone(value: float, deadzone: float) -> float:+ """Hard deadzone: below the threshold the axis reads zero.++ No rescaling of the remaining travel - a stick just past the deadzone+ gives a small command, which is what you want for fine positioning.+ """+ return 0.0 if abs(value) < deadzone else value+++# Absolute ceilings, applied LAST, regardless of config or boost. A userspace+# pad driver that has just started can report uninitialised axes at full+# deflection; without a last gate those go straight onto tele_cmd_vel (a+# priority channel). No teleop output may ever exceed these.+CLAMP_LINEAR_MS = 0.45+SLEW_LINEAR_MS2 = 0.6 # max change of linear command per second: full stick+ # reaches 0.45 m/s in ~0.75 s instead of one step, so+ # the base never sees a step command from the pad+CLAMP_ANGULAR_RADS = 0.8+# The rim envelope: translation and rotation ADD at the wheels (rim speed =+# |vx| + |vy| + lever * |wz|), so each axis under its own clamp can still+# drive the wheels near double. The fallback below is conservative: a long+# lever and the rim held to the fastest single-stick feel (0.45 m/s). MOSS+# passes its own envelope, sized to the tracks (moss_adapter.teleop_envelope).+DEFAULT_LEVER_M = 0.50 # m of rim speed per rad/s of rotation+DEFAULT_RIM_MS = 0.45 # max rim speed however the sticks are mixed+DEADMAN_BUTTON = 7 # R1: SDL button 7 on the 2563:0526 pad in Android mode. Held = commands allowed; released = zeros, always+BRAKE_S = 0.5 # how long the zeros keep flowing after the deadman is+ # released - then TOTAL SILENCE until it is held again.+ # dimOS's MovementManager reads ANY tele_cmd_vel message,+ # zeros included, as "a human is driving": it cancels the+ # nav goal and mutes nav_cmd_vel for tele_cooldown_sec+ # (1.0 s). A pad at rest publishing 50 Hz of zeros would+ # therefore stop exploration for good.+ # 0.5 s < 1.0 s: the brake is seen, then autonomy gets+ # the bus back and the two coexist.+# When the pad's radio dies mid-drive (sleep, battery), this module with this+# pad has been seen to stop the base cleanly, so there is no radio watchdog+# here. Revisit only if a radio loss ever leaves a non-zero command running.+++BRAKE_CANCEL_ON_DEADMAN = True+# A new deadman press CANCELS whatever is left of the brake window, on its+# RISING EDGE (brake_until = 0.0). From that instant the brake path cannot emit+# one more zero, however long the window was.+#+# At the default BRAKE_S = 0.5 s and 50 Hz, with the deadman re-pressed 0.200 s+# after the release, the module publishes the stick command from the first+# tick after the press either way (+0.213 s -> 0.012 m/s, one slew step) and no+# zero after it: the cancel does not change the default behaviour, and the lag+# felt on a quick re-press is the slew restarting from rest (0.6 m/s2 = 0.75 s+# to the ceiling). What the cancel buys is the right to LENGTHEN the zero+# window (brake_s below) without a stale zero from the release ever chasing a+# fresh command down the bus.+# False = the window always runs to its end.++# The length of that window is a knob. 0.5 s is the default (see BRAKE_S);+# raising it towards 1.0 s keeps the base being TOLD to stop for longer+# instead of falling silent and coasting. dimOS builds this module from a+# blueprint, so the environment of the `dimos run` process is the operator's+# lever (workers inherit it):+# MOSS_BRAKE_S=1.0 dimos run moss-dimos.moss-gamepad+BRAKE_S_ENV = "MOSS_BRAKE_S"+BRAKE_S_MAX = 1.0+# Why the ceiling is 1.0 and not more - the cost, in seconds.+# dimOS's MovementManager treats ANY tele_cmd_vel message, zeros included, as+# "a human is driving": _on_teleop cancels the nav goal on EVERY message and+# _on_nav refuses to forward nav_cmd_vel until tele_cooldown_sec = 1.0 s has+# passed since the LAST teleop message (movement_manager.py). So on+# moss-explore every release mutes autonomy for brake_s + 1.0 s: 1.5 s at the+# default, 2.0 s at this ceiling. The zeros always stop at brake_s, whatever+# its value; the ceiling only bounds that muted time.+++def resolve_brake_s(brake_s: float | None = None,+ env: dict | None = None) -> float:+ """How long zeros keep flowing after the deadman is released, in seconds.++ Pure, cold-testable. Precedence: explicit argument > MOSS_BRAKE_S >+ BRAKE_S (0.5 s). Out-of-range values are CLAMPED to [0.0, BRAKE_S_MAX]+ and logged; unparseable text falls back to BRAKE_S, loudly - a mistyped+ knob gives the default window, never a random one.+ """+ raw: float | None = None if brake_s is None else float(brake_s)+ if raw is None:+ text = (os.environ if env is None else env).get(BRAKE_S_ENV, "")+ text = text.strip() if isinstance(text, str) else ""+ if text:+ try:+ raw = float(text)+ except ValueError:+ logger.warning("%s=%r is not a number - brake window stays at "+ "%.2f s", BRAKE_S_ENV, text, BRAKE_S)+ raw = None+ if raw is None:+ return BRAKE_S+ value = max(0.0, min(BRAKE_S_MAX, raw))+ if value != raw:+ logger.warning("brake window %.2f s is outside [0.0, %.2f] s - clamped "+ "to %.2f s (each release mutes dimOS autonomy for the "+ "window + 1.0 s; see BRAKE_S_MAX)",+ raw, BRAKE_S_MAX, value)+ return value+++def brake_window_after_press(brake_until: float, deadman: bool,+ prev_deadman: bool) -> float:+ """The brake deadline this tick keeps - pure, cold-testable.++ RISING edge of the deadman (released -> held) cancels the window by+ returning 0.0, which is always in the past: the brake path can no longer+ publish. Anything else returns the window untouched - in particular a+ deadman simply HELD, because every driving tick re-arms the window itself+ and that one must live its full brake_s after the next release.++ With BRAKE_CANCEL_ON_DEADMAN = False this is the identity function.+ """+ if BRAKE_CANCEL_ON_DEADMAN and deadman and not prev_deadman:+ return 0.0+ return brake_until+++def slew(prev: float, target: float, dt: float) -> float:+ """Rate-limit a linear command - pure, cold-testable."""+ step = SLEW_LINEAR_MS2 * dt+ return max(prev - step, min(prev + step, target))+++@dataclass(frozen=True)+class TwistEnvelope:+ """The ceilings of clamp_twist. The defaults are the conservative+ fallback above. MOSS passes its own (moss_adapter.teleop_envelope): a+ tracked base's rim is |vx| + |wz| * track/2."""+ linear: float = CLAMP_LINEAR_MS+ angular: float = CLAMP_ANGULAR_RADS+ lever: float = DEFAULT_LEVER_M+ rim: float = DEFAULT_RIM_MS+++DEFAULT_ENVELOPE = TwistEnvelope()+++def clamp_twist(vx: float, vy: float, wz: float,+ env: TwistEnvelope = DEFAULT_ENVELOPE) -> tuple[float, float, float]:+ """The last gate before the bus - pure, cold-testable. Per-axis clamps,+ then the rim envelope: translation and rotation ADD at the rim, so a+ mixed command is scaled down proportionally (the feel is preserved, the+ top speed is not exceeded)."""+ lim = env.linear+ vx = max(-lim, min(lim, vx))+ vy = max(-lim, min(lim, vy))+ wz = max(-env.angular, min(env.angular, wz))+ rim = abs(vx) + abs(vy) + env.lever * abs(wz)+ if rim > env.rim:+ k = env.rim / rim+ vx, vy, wz = vx * k, vy * k, wz * k+ return (vx, vy, wz)+++def axes_neutral(ax0: float, ax1: float, ax2: float, deadzone: float) -> bool:+ """True when every drive axis rests inside the deadzone - the trust gate:+ a pad (or a userspace driver) must be SEEN at neutral once before a single+ non-zero command is believed. Uninitialised axes never pass this."""+ return abs(ax0) < deadzone and abs(ax1) < deadzone and abs(ax2) < deadzone+++def axes_to_twist(ax0: float, ax1: float, ax2: float, ax5: float,+ cfg: TeleopConfig) -> tuple[float, float, float]:+ """Raw SDL axis values -> (vx m/s, vy m/s, wz rad/s).++ Pure function - the whole mapping lives here so it is testable without a+ pad, a display, or pygame.++ Each argument is named after its SDL axis index (AXIS_LEFT_X = 0,+ AXIS_LEFT_Y = 1, AXIS_RIGHT_X = 2, AXIS_R2 = 5).++ SDL reports stick up and stick left as NEGATIVE, while the robot frame is+ x forward / y left / wz counter-clockwise, hence the three sign flips:+ ax0 left stick X -> vy (stick left = -1 -> strafe left, +y)+ ax1 left stick Y -> vx (stick up = -1 -> forward, +x)+ ax2 right stick X -> wz (stick right = +1 -> clockwise, -wz)+ ax5 R2 -> boost when > 0 (trigger rests at -1)++ Boost multiplies the linear axes only: rotation stays at its capped rate,+ because a spin fast enough to be useful is already fast enough to hurt.+ """+ boost = cfg.boost_multiplier if ax5 > 0.0 else 1.0+ vx = -apply_deadzone(ax1, cfg.deadzone) * cfg.linear_speed * boost+ vy = -apply_deadzone(ax0, cfg.deadzone) * cfg.linear_speed * boost+ wz = -apply_deadzone(ax2, cfg.deadzone) * cfg.angular_speed+ return clamp_twist(vx, vy, wz, cfg.envelope or DEFAULT_ENVELOPE)class GamepadTeleop(Module):- """Publishes `tele_cmd_vel` while the deadman is held, zeros for brake_s after, then nothing."""+ """pygame.joystick teleop. Outputs Twist on tele_cmd_vel."""++ dedicated_worker = Truetele_cmd_vel: Out[Twist]- def __init__(self, config: TeleopConfig | None = None, axis_left_y: int = AXIS_LEFT_Y,- axis_right_x: int = AXIS_RIGHT_X, button_deadman: int = BUTTON_DEADMAN,- button_boost: int = BUTTON_BOOST, **kwargs) -> None:+ def __init__(self, linear_speed: float = DEFAULT_LINEAR_SPEED,+ angular_speed: float = DEFAULT_ANGULAR_SPEED,+ deadzone: float = DEFAULT_DEADZONE,+ rate_hz: float = DEFAULT_RATE_HZ,+ boost_multiplier: float = DEFAULT_BOOST,+ joystick_index: int = 0, brake_s: float | None = None,+ envelope: Any = None,+ **kwargs: Any) -> None:super().__init__(**kwargs)- self.cfg = config or TeleopConfig()- self.axis_left_y, self.axis_right_x = axis_left_y, axis_right_x- self.button_deadman, self.button_boost = button_deadman, button_boost- self._stop = threading.Event()+ # envelope: a TwistEnvelope, or its dict (blueprints carry plain+ # values), or None = DEFAULT_ENVELOPE. MOSS passes moss_adapter.teleop_envelope().+ if isinstance(envelope, dict):+ envelope = TwistEnvelope(**envelope)+ self.envelope = envelope or DEFAULT_ENVELOPE+ self.cfg = TeleopConfig(linear_speed=linear_speed,+ angular_speed=angular_speed,+ deadzone=deadzone,+ boost_multiplier=boost_multiplier,+ envelope=self.envelope)+ self.rate_hz = rate_hz+ self.joystick_index = joystick_index+ # None -> MOSS_BRAKE_S, else BRAKE_S (0.5 s). Resolved HERE, at+ # construction, so the value is fixed and logged once per run - not+ # re-read per tick.+ self.brake_s = resolve_brake_s(brake_s)+ self._stop_event = threading.Event()self._thread: threading.Thread | None = None+ # @rpc is not decoration for its own sake: Module.start/stop carry it,+ # and an override that drops it falls out of the class's rpcs table.+ # dimOS then proxies the call by pickling the module across the worker+ # pipe, which dies on our threading.Event ('cannot pickle _thread.lock').@rpcdef start(self) -> None:- self._stop.clear()- self._thread = threading.Thread(target=self._loop, name="moss-gamepad", daemon=True)+ super().start()+ self._stop_event.clear()+ self._thread = threading.Thread(target=self._worker,+ name="moss-gamepad", daemon=True)self._thread.start()@rpcdef stop(self) -> None:- self._stop.set()- if self._thread:+ self._stop_event.set()+ if self._thread is not None:self._thread.join(timeout=2.0)+ super().stop()++ def _publish(self, vx: float, vy: float, wz: float) -> None:+ self.tele_cmd_vel.publish(Twist(+ linear=Vector3(x=vx, y=vy, z=0.0),+ angular=Vector3(x=0.0, y=0.0, z=wz)))- def _publish(self, vx: float, wz: float) -> None:- self.tele_cmd_vel.publish(Twist(linear=Vector3(x=vx, y=0.0, z=0.0), angular=Vector3(x=0.0, y=0.0, z=wz)))+ def _acquire(self, pygame: Any) -> Any:+ """Re-scan SDL for the pad. Returns an initialised joystick or None."""+ try:+ pygame.event.pump() # let SDL process hotplug events+ pygame.joystick.quit() # full re-scan: SDL caches the device+ pygame.joystick.init() # list from the last init()+ if pygame.joystick.get_count() <= self.joystick_index:+ return None+ pad = pygame.joystick.Joystick(self.joystick_index)+ pad.init()+ return pad+ except pygame.error as exc:+ logger.debug("Gamepad scan failed: %s", exc)+ return None- def _loop(self) -> None:- import pygame # optional dependency: pip install 'moss-dimos[gamepad]'+ # Named _worker, NOT _loop: dimOS's Module keeps its asyncio event+ # loop in self._loop, which would shadow the method and make the+ # thread target the event loop object.+ def _worker(self) -> None:+ try:+ import pygame+ except ImportError:+ logger.error("pygame is not installed - gamepad teleop is off "+ "(pip install pygame)")+ return- pygame.init()+ pygame.init() # dummy video/audio drivers: works with no displaypygame.joystick.init()- period = 1.0 / self.cfg.rate_hz- brake = DeadmanBrake(self.cfg.brake_s)+ period = 1.0 / self.rate_hzpad = None- while not self._stop.is_set():- if pad is None:- pygame.joystick.quit(); pygame.joystick.init()- if pygame.joystick.get_count() == 0:- time.sleep(1.0); continue- pad = pygame.joystick.Joystick(0); pad.init()- logger.info(f"gamepad connected: {pad.get_name()}")- pygame.event.pump()+ trusted = False # neutral-first trust gate+ prev_vx = prev_vy = 0.0 # slew-limiter state+ brake_until = 0.0 # publish zeros until this instant, then shut up+ prev_deadman = False # to see the RISING edge of the deadman+ waiting_logged = False+ logger.info("gamepad brake window: %.2f s of zeros after the deadman is "+ "released, then silence (cancel-on-press: %s)",+ self.brake_s, BRAKE_CANCEL_ON_DEADMAN)+ try:+ while not self._stop_event.is_set():+ if pad is None:+ pad = self._acquire(pygame)+ if pad is None:+ if not waiting_logged:+ logger.info("waiting for gamepad (index %d)",+ self.joystick_index)+ waiting_logged = True+ self._stop_event.wait(RESCAN_PERIOD_S)+ continue+ waiting_logged = False+ trusted = False # every (re)connection re-earns trust at neutral+ logger.info("Gamepad connected: %s - waiting to see the sticks at NEUTRAL "+ "before trusting a single command", pad.get_name())+ try:+ pygame.event.pump()+ axes = (_axis(pad, AXIS_LEFT_X), _axis(pad, AXIS_LEFT_Y),+ _axis(pad, AXIS_RIGHT_X), _axis(pad, AXIS_R2))+ deadman = bool(pad.get_button(DEADMAN_BUTTON)) if pad.get_numbuttons() > DEADMAN_BUTTON else False+ if pygame.joystick.get_count() <= self.joystick_index:+ raise pygame.error("joystick disappeared")+ except pygame.error as exc:+ pad = None+ trusted = False+ prev_deadman = False # a pad that is gone holds nothing+ # ... and it holds no RAMP either. The slew state must die+ # with the pad: otherwise a pad that vanished for one read+ # while driving at the ceiling comes back with 0.45 m/s of+ # slew state, and the first trusted tick, STICKS AT+ # NEUTRAL, publishes +0.438 m/s - the rover leaves on its+ # own for ~0.75 s. The trust gate does not catch it:+ # neutral axes satisfy the gate, and the danger is inside+ # this module.+ prev_vx = prev_vy = 0.0+ self._publish(0.0, 0.0, 0.0) # one zero Twist, then wait+ logger.warning("Gamepad lost (%s) - back to waiting", exc)+ continue+ # TRUST GATE: until the sticks have been SEEN at neutral once,+ # this pad's words are worth nothing.+ if not trusted:+ if axes_neutral(axes[0], axes[1], axes[2], self.cfg.deadzone):+ trusted = True+ logger.info("Gamepad axes seen at neutral - commands now trusted "+ "(hold the deadman button to drive)")+ else:+ if time.monotonic() < brake_until:+ self._publish(0.0, 0.0, 0.0)+ time.sleep(period)+ continue+ # BRAKE CANCEL: the RISING edge of the deadman ends the brake+ # window there and then, so not one zero left over from the+ # release can still go out behind the commands of the new+ # press. Rising edge, not "held": a window re-armed by a tick+ # that DROVE (below) must keep its own life.+ #+ # ORDER MATTERS: these two lines sit BELOW the trust gate. On+ # a pad that has not yet earned trust - after EVERY+ # reconnection, and after every one-tick read glitch, which is+ # exactly when the rover may be rolling with nobody commanding+ # it - a deadman press above the gate would cancel the brake+ # window while the untrusted path can publish NOTHING to+ # replace it: a single missed read at 0.45 m/s would cut the+ # zeros published after the dropout from 25 (to 0.486 s) down+ # to 1 (0.011 s). BRAKE_S exists to REPEAT the stop order on a+ # latest-only bus; the cancel may therefore only run on a tick+ # that is itself about to publish a command - i.e. here.+ brake_until = brake_window_after_press(brake_until, deadman,+ prev_deadman)+ prev_deadman = deadman+ # DEADMAN: no held button, no motion - ever.+ if not deadman:+ # Releasing also resets the slew state: the ramp restarts+ # from rest, so a re-press with the sticks centred cannot+ # replay the last driven speed.+ prev_vx = prev_vy = 0.0+ # BRAKE THEN SILENCE (see BRAKE_S / self.brake_s): zeros for+ # brake_s after the last command, then not one message until+ # the deadman is held again - a released pad must not mute+ # autonomy.+ if time.monotonic() < brake_until:+ self._publish(0.0, 0.0, 0.0)+ time.sleep(period)+ continue+ vx, vy, wz = axes_to_twist(*axes, self.cfg)+ vx = slew(prev_vx, vx, period)+ vy = slew(prev_vy, vy, period)+ prev_vx, prev_vy = vx, vy+ # envelope AFTER the slew too: while vx decays and wz is+ # instant, the mix could transiently exceed the rim ceiling+ vx, vy, wz = clamp_twist(vx, vy, wz, self.envelope)+ self._publish(vx, vy, wz)+ brake_until = time.monotonic() + self.brake_s+ time.sleep(period)+ finally:try:- held = bool(pad.get_button(self.button_deadman))- boost = bool(pad.get_button(self.button_boost))- ly, rx = pad.get_axis(self.axis_left_y), pad.get_axis(self.axis_right_x)- except pygame.error:- logger.warning("gamepad lost; publishing zeros then waiting for it")- self._publish(0.0, 0.0); pad = None; continue- state = brake.tick(held, time.monotonic())- if state == "command":- vx, wz = axes_to_twist(ly, rx, self.cfg, boost_held=boost)- self._publish(vx, wz)- elif state == "brake":- self._publish(0.0, 0.0)- time.sleep(period)+ self._publish(0.0, 0.0, 0.0)+ except Exception: # stream already torn down: nothing to do+ logger.debug("no zero Twist on shutdown", exc_info=True)+ pygame.joystick.quit()+ pygame.quit()+++def _axis(pad: Any, index: int) -> float:+ """Axis value, or 0.0 when this pad has no such axis.++ Pads differ (some report 4 axes, not 6). A missing R2 must read as "no+ boost", not throw the loop into the reconnect path on every tick.+ """+ if index >= pad.get_numaxes():+ return 0.0+ return float(pad.get_axis(index))diff --git a/moss_dimos/latest_only.py b/moss_dimos/latest_only.pynew file mode 100644index 0000000..af11fb7--- /dev/null+++ b/moss_dimos/latest_only.py@@ -0,0 +1,117 @@+"""Latest-only delivery of velocity commands, on any transport.++Why. lcm_latest bounds the LCM queue of the command channels to one message,+and that clamp is the whole guard against stale twists (a coordinator that+falls behind otherwise executes a backlog of old twists: a 1 s command can+run for nearly 2 s and drive twice the distance asked). It is LCM-only. Under TRANSPORT=zenoh the same commands ride+ZenohPubSub, which offers nothing to clamp: `subscribe()` declares a plain+callback subscriber (dimos/protocol/pubsub/impl/zenohpubsub.py), dimOS never+hands zenoh a RingChannel/FifoChannel handler, and the one per-topic knob,+`Topic.qos`, configures the PUBLISHER (reliability, congestion control), not+the subscriber. Zenoh then delivers the backlog in order, in full, late.++How. Consumer side, so it holds whatever the transport is. The subscriber+callback becomes a slot of ONE: store the newest message, wake the drain+thread, return. The consumer runs on that drain thread and always takes the+newest message pending, so a command arriving while the consumer is busy+replaces the one waiting - nothing older than one publish period is executed.+Same net effect as an LCM capacity of 1, and the same structure dimOS itself+uses in ZenohPubSubBase.subscribe_all ("delivering only the latest per+topic"). A Twist carries no timestamp, so "newest" is arrival order in this+process, nothing else.++Only a velocity command may be dropped this way: a superseded twist has no+effect its successor does not have. The zero that ends a move is never dropped+in favour of motion - it IS the newest message - and JointVelocityTask's 0.2 s+watchdog still zeroes the base if the stream stops.+"""+from __future__ import annotations++import logging+import threading+import time+from typing import Any, Callable++DROP_LOG_PERIOD_S = 1.0+JOIN_TIMEOUT_S = 2.0++_EMPTY = object()+++def _log() -> Any:+ try:+ from dimos.utils.logging_config import setup_logger+ return setup_logger()+ except Exception:+ return logging.getLogger(__name__)+++class LatestOnly:+ """Callable: messages in from any thread, only the newest reaches `callback`.++ A slot of one, not a queue of one. `received` / `delivered` / `dropped` are+ the mux counters a run reads back.+ """++ def __init__(self, callback: Callable[[Any], None], name: str = "command") -> None:+ self._callback = callback+ self._name = name+ self._slot: Any = _EMPTY+ self._lock = threading.Lock()+ self._wake = threading.Event()+ self._stopped = threading.Event()+ self._last_log = 0.0+ self.received = 0+ self.delivered = 0+ self.dropped = 0+ self._thread = threading.Thread(+ target=self._drain, name=f"latest-only-{name}", daemon=True)+ self._thread.start()++ def __call__(self, msg: Any) -> None:+ """Transport thread: O(1), never waits for the consumer."""+ with self._lock:+ superseded = self._slot is not _EMPTY+ self._slot = msg+ self.received += 1+ self.dropped += int(superseded)+ dropped = self.dropped+ self._wake.set()+ if superseded:+ self._log_drop(dropped)++ def rebind(self, callback: Callable[[Any], None]) -> None:+ """Point at a freshly built consumer callback: dimOS builds one per+ subscription, the drain thread outlives them."""+ with self._lock:+ self._callback = callback++ def stop(self, timeout: float = JOIN_TIMEOUT_S) -> None:+ """Stop the drain thread. It is a daemon, so a run never has to; the+ benches do."""+ self._stopped.set()+ self._wake.set()+ self._thread.join(timeout)++ def _log_drop(self, dropped: int) -> None:+ now = time.monotonic()+ if now - self._last_log < DROP_LOG_PERIOD_S:+ return+ self._last_log = now+ _log().warning(f"{self._name}: consumer behind, executing the newest command "+ f"only ({dropped} stale dropped so far)")++ def _drain(self) -> None:+ while not self._stopped.is_set():+ self._wake.wait()+ self._wake.clear()+ with self._lock:+ msg, self._slot = self._slot, _EMPTY+ callback = self._callback+ if msg is _EMPTY:+ continue+ try:+ callback(msg)+ except Exception:+ _log().error(f"{self._name}: consumer raised", exc_info=True)+ self.delivered += 1diff --git a/moss_dimos/lcm_latest.py b/moss_dimos/lcm_latest.pynew file mode 100644index 0000000..720310e--- /dev/null+++ b/moss_dimos/lcm_latest.py@@ -0,0 +1,98 @@+"""Bound the LCM queue of velocity-command topics to ONE pending message.++Why. dimOS subscribes every topic with `set_queue_capacity(10000)`+(dimos/protocol/pubsub/impl/lcmpubsub.py). A Twist carries no timestamp, so+when the coordinator process falls behind (a busy CPU, a point cloud and a+viewer bridge in the same process: a 1 s command can run for nearly 2 s and+drive twice the distance asked) the backlog of old twists is executed late -+the robot keeps moving on stale orders, and the zero that ends a move is the last+one in the line. For a velocity command only the latest value is meaningful:+with a capacity of 1, LCM drops new datagrams while one is still pending, so+the command that gets executed is never more than one publish period old.+The JointVelocityTask watchdog (0.2 s) still zeroes the base when the stream+stops, dropped final zero included.++How. dimOS's LCM wrapper keeps the raw `lcm.LCM` in `self.l` and calls+`self.l.subscribe(channel, handler)` then `.set_queue_capacity(10000)` on the+result. `install()` swaps `self.l` for a proxy whose subscriptions clamp the+capacity on command channels; everything else is delegated untouched.+Installed from moss_dimos.blueprints, i.e. in every process that imports the+coordinator class, the coordinator's worker included.+"""+from __future__ import annotations++import logging+from typing import Any, Callable++COMMAND_CHANNEL_MARKERS = ("cmd_vel", "twist_command")+COMMAND_QUEUE_CAPACITY = 1++def _log() -> Any:+ try:+ from dimos.utils.logging_config import setup_logger+ return setup_logger()+ except Exception:+ return logging.getLogger(__name__)+++def capacity_for(channel: str) -> int | None:+ """Queue capacity override for a channel, None = leave dimOS's default."""+ return COMMAND_QUEUE_CAPACITY if any(m in channel for m in COMMAND_CHANNEL_MARKERS) else None+++class _SubscriptionProxy:+ def __init__(self, sub: Any, cap: int):+ self._sub, self._cap = sub, cap++ def set_queue_capacity(self, n: int) -> None:+ self._sub.set_queue_capacity(min(int(n), self._cap))++ def __getattr__(self, name: str) -> Any:+ return getattr(self._sub, name)+++class _RawLCMProxy:+ """Wraps the raw lcm.LCM object: same API, clamped queues on command channels."""++ def __init__(self, raw: Any):+ self._raw = raw++ def subscribe(self, channel: str, handler: Callable[..., Any]) -> Any:+ sub = self._raw.subscribe(channel, handler)+ cap = capacity_for(channel)+ if cap is None:+ return sub+ _log().info(f"LCM queue capacity clamped to {cap} on {channel}")+ return _SubscriptionProxy(sub, cap)++ def unsubscribe(self, sub: Any) -> Any:+ return self._raw.unsubscribe(getattr(sub, "_sub", sub))++ def __getattr__(self, name: str) -> Any:+ return getattr(self._raw, name)+++_installed = False+++def install() -> bool:+ """Patch dimOS's LCM.subscribe so command channels get a 1-deep queue."""+ global _installed+ if _installed:+ return True+ try:+ from dimos.protocol.pubsub.impl.lcmpubsub import LCM+ except Exception: # dimos not importable here (cold benches) - nothing to patch+ return False+ original = LCM.subscribe++ def subscribe(self: Any, topic: Any, callback: Any) -> Any:+ raw = getattr(self, "l", None)+ if raw is not None and not isinstance(raw, _RawLCMProxy):+ self.l = _RawLCMProxy(raw)+ return original(self, topic, callback)++ subscribe.__wrapped__ = original # type: ignore[attr-defined]+ LCM.subscribe = subscribe # type: ignore[method-assign]+ _installed = True+ return Truediff --git a/moss_dimos/lidar_odometry.py b/moss_dimos/lidar_odometry.pynew file mode 100644index 0000000..d8d3226--- /dev/null+++ b/moss_dimos/lidar_odometry.py@@ -0,0 +1,377 @@+"""Lidar odometry for MOSS: KISS-ICP scan-to-map on the RPLIDAR C1.++dimOS ships no odometry for a 2D lidar, so this module supplies the pose. Each+revolution from RPLidarC1 (a flat cloud in `lidar_link`) is registered against+kiss-icp's local map, and the result is projected on SE(2) and published.++A planar scan leaves z/roll/pitch unobservable for a 3D ICP: the cloud is+thickened (copies at +-PLANE_THICKNESS_M) and the pose is projected on SE(2).++The prior of each registration is kiss-icp's own constant-velocity model, or a+gyro's yaw rate when `use_gyro_prior` is on and `imu` messages arrive. The+wheel (track) odometry never feeds the prior: tracks slip in every turn, and a+scan stamped at a slipping pose doubles the walls. It is only logged, as a+sanity signal. Rotation without a gyro is the known weak spot: on C1 scans of+~340 points, kiss-icp alone was measured under-estimating turns by 20-35 %.+Bench the turn tracking before trusting long runs.++Frames and heights:+ * `world` is kiss-icp's own frame: every run starts at the origin, facing +x.+ * `odom` is published on the floor plane (z = 0) - see handle_pointcloud.+ * the lidar returns are lifted to exactly the profile's lidar_height_m, in+ the `lidar` cloud and in the base_link->lidar_link tf. costmap2d recognises+ lidar returns by that exact height, so both read the same profile value.++Optional depth camera: when `depth_image` and `camera_info` are fed (a+RealSense built with depth on), obstacles seen by the camera are added to the+next `lidar` clouds at their true height, and the bare floor it sees goes out+on `camera_floor` (z = 0). The camera mount comes from the profile+(camera_xyz_base_m, camera_pitch_deg). The MOSS blueprints build the RealSense+without depth, so this path is idle there, and costmap2d maps lidar returns+only.++Streams+ pointcloud : In[PointCloud2] one revolution, lidar_link (rplidar_c1.py)+ imu : In[Imu] gyro (angular_velocity) for the optional rotation prior+ coordinator_joint_state : In[JointState] base/vx, base/vy, base/wz positions = wheel odometry, logged only+ depth_image : In[Image] optional depth camera, DEPTH16 mm, aligned to colour+ camera_info : In[CameraInfo] its colour intrinsics+ odom : Out[PoseStamped] base pose in `world`, z = 0+ lidar : Out[PointCloud2] the revolution re-expressed in `world` (costmap2d input)+ camera_floor : Out[PointCloud2] world-frame floor samples from the depth camera, z = 0+ tf : Out[TFMessage] world->base_link, base_link->lidar_link+"""+from __future__ import annotations++import math+import threading+import time+from typing import Any++import numpy as np++from dimos.core.core import rpc+from dimos.core.module import Module+from dimos.core.stream import In, Out+from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped+from dimos.msgs.geometry_msgs.Quaternion import Quaternion+from dimos.msgs.geometry_msgs.Transform import Transform+from dimos.msgs.geometry_msgs.Vector3 import Vector3+from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo+from dimos.msgs.sensor_msgs.Image import Image+from dimos.msgs.sensor_msgs.Imu import Imu+from dimos.msgs.sensor_msgs.JointState import JointState+from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2+from dimos.msgs.tf2_msgs.TFMessage import TFMessage+from dimos.utils.logging_config import setup_logger++from moss_dimos.robot_profile import RobotProfile, current as _current_profile++logger = setup_logger()++PLANE_THICKNESS_M = 0.05+DEPTH_STRIDE = 8 # 640x480 -> 80x60 samples per frame: what the map needs, not more+DEPTH_EVERY = 2 # one depth frame in two (15 fps -> 7.5 Hz)+DEPTH_MAX_M = 3.0 # beyond that the floor noise (1-2 % of range) leaks into the obstacle band+OBSTACLE_MAX_M = 3.0 # camera obstacles are trusted up to this range; past it, depth reflections+ # on a glossy floor arm phantom obstacles. Floor samples keep the full+ # DEPTH_MAX_M range: erasing stays cheap.+OBSTACLE_Z_M = (0.12, 1.30) # world z band the camera turns into floor obstacles. Upper bound 1.30, not 0.70: a table top is a BLOCK - the rover goes around tables, never between the legs+++def _profile() -> RobotProfile:+ """The geometry the tf, the lidar lift and the depth projection use, read+ at call time so a MOSS_* override takes effect without a restart."""+ return _current_profile()+++def _yaw_quat(yaw: float) -> Quaternion:+ return Quaternion(0.0, 0.0, math.sin(yaw / 2.0), math.cos(yaw / 2.0))+++def _se2(dx: float, dy: float, dyaw: float) -> np.ndarray:+ c, s = math.cos(dyaw), math.sin(dyaw)+ return np.array([[c, -s, 0.0, dx], [s, c, 0.0, dy], [0.0, 0.0, 1.0, 0.0], [0.0, 0.0, 0.0, 1.0]])+++def split_floor_and_obstacles(bx: np.ndarray, by: np.ndarray, bz: np.ndarray,+ floor_z: np.ndarray, cell_m: float = 0.05) -> tuple[np.ndarray, np.ndarray]:+ """Masks (obstacle, floor) over base-frame depth points.++ Obstacle = above the floor threshold and below OBSTACLE_Z_M[1] (1.30 m:+ table tops included, so a table is a block the rover drives around).+ Floor = at floor level. Under a table the camera sees BOTH the top and+ the floor between the legs on the same ground cell; the obstacle wins,+ so floor samples on a cell that also holds an obstacle are dropped -+ otherwise hit and miss cancel and the table vanishes from the costmap.+ """+ obst = (bz > floor_z) & (bz < OBSTACLE_Z_M[1])+ floor = (bz <= floor_z) & (bz > -0.10)+ if obst.any() and floor.any():+ keys_o = set(map(tuple, np.floor(np.stack([bx[obst], by[obst]], 1) / cell_m).astype(np.int64)))+ fk = np.floor(np.stack([bx, by], 1) / cell_m).astype(np.int64)+ under = np.array([tuple(k) in keys_o for k in fk])+ floor = floor & ~under+ return obst, floor+++class LidarOdometry(Module):+ pointcloud: In[PointCloud2]+ imu: In[Imu]+ coordinator_joint_state: In[JointState]+ depth_image: In[Image] # depth camera (aligned to colour), DEPTH16 mm+ camera_info: In[CameraInfo] # colour intrinsics (= aligned depth intrinsics)+ odom: Out[PoseStamped]+ lidar: Out[PointCloud2]+ camera_floor: Out[PointCloud2] # world-frame floor samples (z = 0) the depth camera saw bare+ tf: Out[TFMessage]++ def __init__(self, max_range_m: float = 12.0, min_range_m: float = 0.35,+ voxel_size_m: float = 0.05, initial_threshold_m: float = 0.3,+ gyro_axis: str = "-y", use_gyro_prior: bool = True,+ world_frame: str = "world",+ base_frame: str = "base_link", lidar_frame: str = "lidar_link",+ log_every_s: float = 2.0, **kwargs: Any) -> None:+ super().__init__(**kwargs)+ self.max_range_m, self.min_range_m, self.voxel_size_m = max_range_m, min_range_m, voxel_size_m+ self.initial_threshold_m = initial_threshold_m+ # which IMU axis carries the robot's yaw rate, with its sign. A D455F+ # motion module reports in the camera's optical frame (x right, y+ # down, z forward): a level camera looking forward gives+ # yaw_rate = -gyro.y. A camera mounted upside down flips the sign.+ self.gyro_axis = gyro_axis+ self.use_gyro_prior = use_gyro_prior+ self.world_frame, self.base_frame, self.lidar_frame = world_frame, base_frame, lidar_frame+ self.log_every_s = log_every_s+ # dimOS pickles module instances into their worker: no lock, no native+ # object before start().+ self._kiss: Any = None+ self._lock: Any = None+ self._n = 0+ self._last_log = 0.0+ self._last_ms = 0.0+ self.pose2d = (0.0, 0.0, 0.0)+ # gyro integration since the last scan (+ totals per axis for calibration)+ self._gyro_last_ts: float | None = None+ self._gyro_acc = 0.0+ self._gyro_seen = False+ self._gyro_totals = np.zeros(3)+ self._wheel: tuple[float, float, float] | None = None # latest wheel odometry: logged only+ self._prior_used = "cv"+ self._K: tuple[float, float, float, float] | None = None+ self._pose_hist: list[tuple[float, float, float, float]] = [] # (wall ts, x, y, yaw), last ~3 s+ self._depth_n = 0+ self._depth_pts_last = 0+ self._pending_cam_pts = None++ # ── lifecycle ──────────────────────────────────────────────────────+ @rpc+ def start(self) -> None:+ super().start()+ from kiss_icp.config import KISSConfig+ from kiss_icp.kiss_icp import KissICP+ cfg = KISSConfig()+ cfg.data.max_range = self.max_range_m+ cfg.data.min_range = self.min_range_m+ cfg.data.deskew = False+ cfg.mapping.voxel_size = self.voxel_size_m+ cfg.mapping.max_points_per_voxel = 30+ # kiss-icp defaults are tuned for cars (2 m initial threshold)+ cfg.adaptive_threshold.initial_threshold = self.initial_threshold_m+ cfg.adaptive_threshold.min_motion_th = 0.02+ self._kiss = KissICP(cfg)+ self._lock = threading.Lock()+ logger.info(f"lidar odometry up (kiss-icp, voxel {self.voxel_size_m} m, range "+ f"{self.min_range_m}-{self.max_range_m} m, prior: gyro={self.use_gyro_prior} "+ f"axis {self.gyro_axis}, wheels NEVER) -> {self.world_frame}")++ @rpc+ def stop(self) -> None:+ super().stop()++ # ── priors ─────────────────────────────────────────────────────────+ async def handle_imu(self, msg: Imu) -> None:+ w = msg.angular_velocity+ ts = float(getattr(msg, "ts", 0.0) or time.time())+ if self._gyro_last_ts is not None:+ dt = ts - self._gyro_last_ts+ if 0.0 < dt < 0.1:+ v = np.array([w.x, w.y, w.z], dtype=float)+ self._gyro_totals += v * dt+ axis = {"x": 0, "y": 1, "z": 2}[self.gyro_axis[-1]]+ sign = -1.0 if self.gyro_axis.startswith("-") else 1.0+ self._gyro_acc += sign * v[axis] * dt+ self._gyro_seen = True+ self._gyro_last_ts = ts++ async def handle_coordinator_joint_state(self, msg: JointState) -> None:+ names = list(msg.name); pos = list(msg.position)+ try:+ ix, iy, ith = (names.index(n) for n in ("base/vx", "base/vy", "base/wz"))+ except ValueError:+ return+ self._wheel = (float(pos[ix]), float(pos[iy]), float(pos[ith]))++ async def handle_camera_info(self, msg: CameraInfo) -> None:+ K = list(msg.K or [])+ if len(K) == 9 and K[0] > 0:+ self._K = (float(K[0]), float(K[4]), float(K[2]), float(K[5]))++ async def handle_depth_image(self, msg: Image) -> None:+ """Sparse obstacle cloud from the depth image, in `world`, onto `lidar`.++ The 2D lidar sees one plane: it misses objects lower than its scan and+ thin dark legs. The depth camera sees them: every DEPTH_STRIDE-th pixel+ is back-projected, placed in the world with the pose at capture time,+ cropped to the rover's height band, voxel-deduplicated and attached to+ the next `lidar` clouds."""+ self._depth_n += 1+ if self._K is None or self._depth_n % DEPTH_EVERY:+ return+ d = np.asarray(msg.data)+ if d.ndim == 3:+ d = d[..., 0]+ fx, fy, cx, cy = self._K+ h, w = d.shape+ vs, us = np.mgrid[0:h:DEPTH_STRIDE, 0:w:DEPTH_STRIDE]+ z = d[vs, us].astype(np.float64) / 1000.0+ ok = (z > 0.30) & (z < DEPTH_MAX_M) # < 0.30 m = the rover's own front, not the world+ if not ok.any():+ return+ z, us, vs = z[ok], us[ok], vs[ok]+ xo = (us - cx) * z / fx # optical x (right)+ yo = (vs - cy) * z / fy # optical y (down)+ # optical (x right, y down, z forward) -> base (x forward, y left, z up),+ # camera pitched down by the profile's camera_pitch_deg: forward axis =+ # (cos, 0, -sin), down axis = (-sin, 0, -cos); then the mount offset+ prof = _profile()+ pitch = math.radians(prof.camera_pitch_deg)+ cam = prof.camera_xyz_base_m+ cp, sp = math.cos(pitch), math.sin(pitch)+ bx = z * cp - yo * sp + cam[0]+ by = -xo + cam[1]+ bz = -z * sp - yo * cp + cam[2]+ # floor threshold grows with range (depth noise ~1-2 % of range): a 5 cm+ # chair base is an obstacle at 1 m, floor noise is not an obstacle at 3 m+ floor_z = 0.03 + 0.03 * np.clip(bx - 1.0, 0.0, None)+ obst, floor = split_floor_and_obstacles(bx, by, bz, floor_z)+ if not obst.any() and not floor.any():+ return+ fx, fy = bx[floor], by[floor] # bare floor: published separately, z = 0+ near = np.hypot(bx, by) < OBSTACLE_MAX_M+ bx, by, bz = bx[obst & near], by[obst & near], bz[obst & near]+ # pose at the frame's capture time: the depth handler runs 50-200 ms+ # late, and at 17 deg/s the latest pose would smear the camera points+ # by several degrees per frame+ fts = float(getattr(msg, "ts", 0.0) or 0.0)+ pose = self._pose_at(fts) if fts > 0 else self.pose2d+ x, y, yaw = pose+ c, s_ = math.cos(yaw), math.sin(yaw)+ if len(fx):+ fwx, fwy = c * fx - s_ * fy + x, s_ * fx + c * fy + y+ fpts = np.stack([fwx, fwy, np.zeros_like(fwx)], axis=1)+ fkeys = np.floor(fpts / 0.05).astype(np.int64)+ _, fidx = np.unique(fkeys, axis=0, return_index=True)+ self.camera_floor.publish(PointCloud2.from_numpy(fpts[fidx].astype(np.float32), frame_id=self.world_frame, timestamp=time.time()))+ if len(bx) == 0:+ return+ wx, wy = c * bx - s_ * by + x, s_ * bx + c * by + y+ pts = np.stack([wx, wy, bz], axis=1)+ # voxel-deduplicate at the map resolution+ keys = np.floor(pts / 0.05).astype(np.int64)+ _, idx = np.unique(keys, axis=0, return_index=True)+ pts = pts[idx].astype(np.float32)+ self._depth_pts_last = len(pts)+ # attached to EVERY lidar revolution until replaced (0.5 s cap): the+ # 10 Hz lidar against 7.5 Hz depth would otherwise leave one revolution+ # in four without camera points, and the camera points would blink.+ self._pending_cam_pts = (pts, time.monotonic())++ def _pose_at(self, ts: float) -> tuple[float, float, float]:+ """Pose interpolated at wall time ts from the recent history (else the latest)."""+ h = self._pose_hist+ if len(h) < 2 or ts <= h[0][0]:+ return self.pose2d if not h else (h[0][1], h[0][2], h[0][3])+ if ts >= h[-1][0]:+ return (h[-1][1], h[-1][2], h[-1][3])+ for a, b in zip(h, h[1:]):+ if a[0] <= ts <= b[0]:+ f = (ts - a[0]) / max(b[0] - a[0], 1e-6)+ dyaw = math.atan2(math.sin(b[3] - a[3]), math.cos(b[3] - a[3]))+ return (a[1] + f * (b[1] - a[1]), a[2] + f * (b[2] - a[2]), a[3] + f * dyaw)+ return self.pose2d++ def _prior_delta(self) -> np.ndarray:+ """Motion since the last scan, in the previous lidar frame (SE(2) as 4x4)."""+ last_delta = np.asarray(self._kiss.last_delta)+ dx, dy = float(last_delta[0, 3]), float(last_delta[1, 3])+ dyaw = math.atan2(last_delta[1, 0], last_delta[0, 0])+ used = []+ # the wheels are NEVER consulted here - see the module docstring+ if self.use_gyro_prior and self._gyro_seen:+ dyaw = self._gyro_acc+ used.append("gyro")+ self._prior_used = "+".join(used) or "cv"+ return _se2(dx, dy, dyaw)++ # ── the work ───────────────────────────────────────────────────────+ async def handle_pointcloud(self, msg: PointCloud2) -> None:+ out = msg.as_numpy()+ pts = np.asarray(out[0] if isinstance(out, tuple) else out, dtype=np.float64)+ if pts.ndim != 2 or pts.shape[0] < 30:+ return+ t0 = time.monotonic()+ thick = np.concatenate([pts, pts + [0, 0, PLANE_THICKNESS_M], pts - [0, 0, PLANE_THICKNESS_M]])+ with self._lock:+ k = self._kiss+ prior = self._prior_delta()+ frame = k.preprocessor.preprocess(thick, np.zeros(len(thick)), prior)+ source, frame_down = k.voxelize(frame)+ sigma = k.adaptive_threshold.get_threshold()+ initial_guess = np.asarray(k.last_pose) @ prior+ new_pose = k.registration.align_points_to_map(+ points=source, voxel_map=k.local_map, initial_guess=initial_guess,+ max_correspondance_distance=3 * sigma, kernel=sigma)+ corr = np.linalg.inv(initial_guess) @ np.asarray(new_pose)+ k.adaptive_threshold.update_model_deviation(corr)+ k.local_map.update(frame_down, new_pose)+ k.last_delta = np.linalg.inv(np.asarray(k.last_pose)) @ new_pose+ k.last_pose = new_pose+ # consume the priors+ self._gyro_acc, self._gyro_seen = 0.0, False+ pose = np.asarray(new_pose)+ R, t = pose[:3, :3], pose[:3, 3]+ x, y, yaw = float(t[0]), float(t[1]), math.atan2(R[1, 0], R[0, 0])+ now_wall = time.time()+ self._pose_hist.append((now_wall, x, y, yaw))+ self._pose_hist = [h for h in self._pose_hist if now_wall - h[0] < 3.0]+ self.pose2d = (x, y, yaw)+ c, s = math.cos(yaw), math.sin(yaw)+ R2 = np.array([[c, -s, 0.0], [s, c, 0.0], [0.0, 0.0, 1.0]])+ lidar_h = _profile().lidar_height_m # costmap2d tags lidar returns by this exact z+ world_pts = (pts @ R2.T) + np.array([x, y, lidar_h])+ ts = time.time(); q = _yaw_quat(yaw)+ # odom on the FLOOR PLANE (z=0): dimOS's planner measures 3D distances+ # (goal_tolerance 0.2 m, path checks down to 0.01 m) - an odom published+ # at lidar height keeps the robot lidar_height_m away from every z=0+ # goal FOREVER. The lidar height lives in the tf child and in the+ # world-cloud transform, never in the pose.+ self.odom.publish(PoseStamped(ts, self.world_frame, position=Vector3(x, y, 0.0), orientation=q))+ cam = self._pending_cam_pts+ if cam is not None and time.monotonic() - cam[1] < 0.5:+ world_pts = np.vstack([world_pts, cam[0]])+ self.lidar.publish(PointCloud2.from_numpy(world_pts.astype(np.float32), frame_id=self.world_frame, timestamp=ts))+ self.tf.publish(TFMessage(+ Transform(translation=Vector3(x, y, 0.0), rotation=q, frame_id=self.world_frame, child_frame_id=self.base_frame, ts=ts),+ Transform(translation=Vector3(0.0, 0.0, lidar_h), rotation=Quaternion(0, 0, 0, 1),+ frame_id=self.base_frame, child_frame_id=self.lidar_frame, ts=ts)))+ self._n += 1+ self._last_ms = (time.monotonic() - t0) * 1000.0+ if t0 - self._last_log >= self.log_every_s:+ self._last_log = t0+ g = np.degrees(self._gyro_totals); w = self._wheel+ logger.info(f"lidar odom #{self._n}: x={x:+.3f} y={y:+.3f} yaw={math.degrees(yaw):+.1f}deg "+ f"({len(pts)} pts, {self._last_ms:.1f} ms, prior {self._prior_used}, depth cloud "+ f"{self._depth_pts_last} pts) | gyro integral "+ f"x={g[0]:+.1f} y={g[1]:+.1f} z={g[2]:+.1f}deg | wheels "+ + (f"x={w[0]:+.3f} y={w[1]:+.3f} th={math.degrees(w[2]):+.1f}deg" if w else "none"))diff --git a/moss_dimos/memory.py b/moss_dimos/memory.pynew file mode 100644index 0000000..99d2143--- /dev/null+++ b/moss_dimos/memory.py@@ -0,0 +1,217 @@+"""MOSS's run recording: what the cockpit shows, written to SQLite.++dimOS's Recorder (its "memory") stores the In ports below with a pose per+observation; afterwards the run can be replayed without the robot: draw the+map, try a costmap function or a planner on it, compare. The recording also+survives a crash, as long as the rotation of the previous recording keeps+the whole SQLite WAL trio together - see rotate_recording() below.++Two layers:++ RotatingRecorder odom + global_costmap, the odom pose setter, and the+ WAL-safe rotation of the previous recording in start()+ MossRecorder MOSS's streams on top of it++MossRecorder's streams:++ odom PoseStamped the pose source of every anchored stream+ global_costmap OccupancyGrid world frame, poseless+ lidar PointCloud2 world-frame cloud from lidar_odometry, at the last odom pose+ color_image Image D455F colour (JPEG on the bus and in the db), at the last odom pose+ wrist_image Image the arm's UVC camera (JPEG), at the last odom pose+ cmd_vel Twist what the base was told, poseless+ base_telemetry String firmware JSON (battery, mode, track outputs), poseless+ episode_marker String {action, name, ts} from the cockpit, poseless++The db path is absolute (robot_profile.moss_recording_path, default+~/.local/state/moss/recordings/moss.db, MOSS_STATE_DIR moves the whole+tree): a relative path would land in dimOS's site-packages.+"""++from __future__ import annotations++from datetime import datetime+from pathlib import Path+import re++from pydantic import Field++from dimos.core.core import rpc+from dimos.core.stream import In+from dimos.memory.module import OnExisting, Recorder, RecorderConfig, pose_setter_for+from dimos.msgs.geometry_msgs.Pose import Pose+from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped+from dimos.msgs.geometry_msgs.Twist import Twist+from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid+from dimos.msgs.sensor_msgs.Image import Image+from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2+from dimos.msgs.std_msgs.String import String+from dimos.utils.logging_config import setup_logger++from moss_dimos.robot_profile import moss_recording_path++logger = setup_logger()++# a WAL recording is three files: the .db + these two+WAL_SIDECARS = ("-wal", "-shm")+++def _sidecars(db: Path) -> list[Path]:+ """The -wal/-shm of *db* that exist right now."""+ return [p for p in (db.with_name(db.name + s) for s in WAL_SIDECARS) if p.exists()]+++def _rotated(db: Path, stamp: str) -> Path:+ return db.with_name(f"{db.stem}.{stamp}{db.suffix}")+++def _free_stamp(db: Path) -> str:+ """A stamp no rotated file carries yet: a rotation never overwrites."""+ now = stamp = datetime.now().strftime("%Y%m%d%H%M%S")+ n = 0+ while True:+ taken = _rotated(db, stamp)+ if not any(taken.with_name(taken.name + s).exists() for s in ("", *WAL_SIDECARS)):+ return stamp+ n += 1+ stamp = f"{now}-{n}"+++def quarantine_orphans(db: Path) -> Path | None:+ """Park a -wal/-shm left with no db beside it, dated, never deleted.++ Whatever the killed run committed can live in that -wal alone, and it takes+ its own db to read it back: moving it out of the way is all we may do. The+ next run then opens a fresh db with nothing stale beside it - opening one on+ top of an orphaned -wal rewrites the -wal, which is the loss itself.+ """+ orphans = _sidecars(db)+ if db.exists() or not orphans:+ return None+ stamp = datetime.now().strftime("%Y%m%d%H%M%S")+ dest = db.parent / "quarantine" / stamp+ n = 0+ while dest.exists():+ n += 1+ dest = db.parent / "quarantine" / f"{stamp}-{n}"+ dest.mkdir(parents=True)+ for p in orphans:+ p.rename(dest / p.name)+ logger.warning("Orphaned %s with no %s -> quarantined in %s",+ [p.name for p in orphans], db.name, dest)+ return dest+++def _prune(db: Path, keep_last: int) -> None:+ """backup_keep_last, applied to whole trios - dimOS's prune unlinks the .db+ and leaves its -wal behind to shadow a later recording. keep_last=0 means+ 'delete the previous recording' upstream; here it prunes nothing, a+ recording is never destroyed to save room."""+ if keep_last <= 0:+ return+ pattern = re.compile(rf"^{re.escape(db.stem)}\.(\d{{14}})(?:-(\d+))?{re.escape(db.suffix)}$")+ found = [(m, p) for m, p in+ ((pattern.match(p.name), p) for p in db.parent.glob(f"{db.stem}.*{db.suffix}"))+ if m]+ # oldest first, by stamp then by same-second rank: sorting the names would+ # put "<stamp>-2.db" before "<stamp>.db" and retire the newest recording+ olds = [p for _, p in sorted(found, key=lambda f: (f[0].group(1), int(f[0].group(2) or 0)))]+ for old in olds[:-keep_last]:+ for p in (old, *_sidecars(old)):+ p.unlink()+++def rotate_recording(db_path: str | Path, keep_last: int = 10) -> Path | None:+ """Move the previous recording aside - the WAL trio, not the db alone.++ A recording is moss.db + moss.db-wal + moss.db-shm, and a run killed with+ SIGKILL (`dimos stop` escalates to it) leaves its last commits in the+ -wal. dimOS's backup_file() renames the .db alone: the backup then opens+ with an empty schema and the next run's fresh db writes over the -wal, so+ the crashed run - the one worth reading back - is exactly the one+ destroyed. Rename the three together instead.++ Returns the rotated .db, or None when there was no recording to rotate.+ """+ db = Path(db_path)+ db.parent.mkdir(parents=True, exist_ok=True)+ if not db.exists():+ quarantine_orphans(db)+ return None++ rotated = _rotated(db, _free_stamp(db))+ # the db moves first: interrupted mid-rotation, the sidecars are quarantined+ # by the next run instead of being adopted by a fresh recording+ for src in (db, *_sidecars(db)):+ src.rename(rotated.with_name(rotated.name + src.name[len(db.name):]))+ logger.info("Rotated recording %s -> %s (WAL trio)", db, rotated)+ quarantine_orphans(db)+ _prune(db, keep_last)+ return rotated+++class RotatingRecorderConfig(RecorderConfig):+ # a factory, not a value: MOSS_STATE_DIR is read when the module is built+ db_path: str | Path = Field(default_factory=moss_recording_path)+ # world-frame streams need no pose anchoring; the odom stream sets the pose+ poseless_streams: list[str] = Field(default_factory=lambda: ["global_costmap"])+ # tf at ~23 Hz is recorded by default as the POSE FALLBACK for streams+ # without a setter - every stream here has a setter or is poseless, so+ # recording tf would buy ~23 sqlite commits/s for nothing (the recorder+ # then eats a full core, one commit per observation).+ record_tf: bool = False+++class RotatingRecorder(Recorder):+ """Recording base: trajectory + decision map, with the odom pose setter+ (subclasses anchor their own streams on `_last_odom_pose`) and the+ WAL-safe rotation of the previous recording in start()."""+ odom: In[PoseStamped]+ global_costmap: In[OccupancyGrid]+ config: RotatingRecorderConfig++ _last_odom_pose: Pose | None = None++ # dimOS rotates in Recorder.start(), with backup_file(): the .db alone.+ # Rotate here first, trio and all - Recorder then finds no db and rotates+ # nothing. Only BACKUP is ours; APPEND/OVERWRITE/ERROR stay upstream's.+ @rpc+ def start(self) -> None:+ if not self.config.g.replay and self.config.on_existing is OnExisting.BACKUP:+ rotate_recording(self.config.db_path, keep_last=self.config.backup_keep_last)+ super().start()++ @pose_setter_for("odom")+ async def _odom_pose(self, msg: PoseStamped) -> Pose | None:+ self._last_odom_pose = Pose(position=msg.position, orientation=msg.orientation)+ return self._last_odom_pose+++MOSS_POSELESS_STREAMS = ["global_costmap", "cmd_vel", "base_telemetry", "episode_marker"]+++class MossMemoryConfig(RotatingRecorderConfig):+ # a factory, not a value: MOSS_STATE_DIR is read when the module is built+ db_path: str | Path = Field(default_factory=moss_recording_path)+ poseless_streams: list[str] = Field(default_factory=lambda: list(MOSS_POSELESS_STREAMS))+ record_tf: bool = False+++class MossRecorder(RotatingRecorder):+ """odom + global_costmap (inherited, with the odom pose setter and the+ WAL-safe rotation in start()), plus the MOSS streams."""++ lidar: In[PointCloud2]+ color_image: In[Image]+ wrist_image: In[Image]+ cmd_vel: In[Twist]+ base_telemetry: In[String]+ episode_marker: In[String]+ config: MossMemoryConfig++ @pose_setter_for("lidar", "color_image", "wrist_image")+ async def _anchor_on_odom(self, msg) -> Pose | None:+ # the cloud is already in the world frame and the images have no tf+ # chain on MOSS: anchor both at the rover's last pose, so memory+ # queries ("what did it see from here") still work+ return self._last_odom_posediff --git a/moss_dimos/moss_adapter.py b/moss_dimos/moss_adapter.pynew file mode 100644index 0000000..c1f6973--- /dev/null+++ b/moss_dimos/moss_adapter.py@@ -0,0 +1,642 @@+"""MOSS tracked base for dimOS: a TwistBaseAdapter over the QT Py motor firmware.++MOSS drives two geared DC motors through a Cytron MDD3A dual driver from an+Adafruit QT Py ESP32-S3 running `firmware/MOSS_QTPY_MDD3A` (this repo, fw+"qtpy-mdd3a-v3"; an MDD10A build adds a "driver" key to the telemetry). The+firmware speaks a line protocol on the QT Py's native USB CDC port (115200+nominal; the host must hold DTR high to receive anything):++ arm -> "OK" READY for 10 s, then it disarms itself+ drive L R -> "OK" L, R in percent, -100..+100 (firmware limit)+ must be REFRESHED within 300 ms or the+ firmware stops ("timeout") - a deadman+ stop -> "OK"+ ramp UP DOWN -> "OK" ms per 1 % rise / fall, 1..500 each (RAM only)+ status -> one telemetry JSON line+ (unsolicited) one telemetry JSON line every 200 ms:+ {"ms":..,"fw":"qtpy-mdd3a-v3","mode":0-3,"reason":"..","target_pct":[l,r],+ "output_pct":[l,r],"ramp_ms":[rise,fall],"ticks":[l,r],"invalid_edges":[l,r],+ "ina_ok":..,"bus_mV":..,"shunt_uV":..,"current_mA":null}+ errors: "ERR not_armed" | "ERR invalid_command" | "ERR stopped_busy"++Modes: 0 OFF, 1 READY (armed, idle), 2 DRIVE, 3 TEST.++What this adapter does with it:++- write_enable(True) = "arm"; write_enable(False) = "stop".+- write_velocities([vx, vy, wz]) maps the body twist to left/right track+ speeds (differential drive: vy is IGNORED, a tracked base cannot strafe),+ then to percent of the firmware limit, and sends "drive L R". The firmware+ is open loop (PWM percent): the metres-per-second scale is a CALIBRATION+ number (MossGeometry.mps_at_limit), measured on the floor, not derived.+- a keepalive thread re-sends the last non-zero drive every KEEPALIVE_S+ while the last command is fresher than STALE_S; past that it sends "stop"+ ONCE and holds. So a dead commander stops the tracks in <= STALE_S here+ and, if this process dies too, in 300 ms in the firmware. Nothing here+ ever prolongs a command nobody is refreshing.+- the firmware disarms itself 10 s after "arm" with no drive, and after any+ invalid line. The adapter does NOT guess that from telemetry (a line up to+ 200 ms old can say OFF while the firmware is already in DRIVE, and an "arm"+ sent in DRIVE is "arm_while_moving": a STOP). The firmware is the source of+ truth: a drive answered "ERR not_armed" re-arms and retries once.+- odometry: the firmware counts quadrature ticks; with MossGeometry+ .ticks_per_m set (measured), read_velocities()/read_odometry() come from+ tick deltas. Without it (the default), read_velocities() returns the COMMANDED+ twist (open loop, low confidence) and read_odometry() returns None -+ an honest None beats a pose made of guesses.+- telemetry: the last firmware JSON line is kept (`telemetry`, a copy, and+ `telemetry_age_s`); the coordinator republishes it on `base_telemetry`+ (blueprints.MossControlCoordinator, payload = telemetry_payload below).++dof: reported as 3 so the stock dimOS twist joints (vx, vy, wz) and the+gamepad module work unchanged; vy is dropped at the tracks.++Calibration (env, all optional):+ MOSS_PORT serial port (default: the QT Py by-id path,+ /dev/serial/by-id/*QT_Py*; when absent the sentinel+ /dev/serial/by-id/MISSING-QT-PY, which fails loudly -+ never /dev/ttyUSB0, the lidar on MOSS)+ MOSS_TRACK_M distance between the two track centrelines, metres+ (default robot_profile.MOSS.track_m, 0.266 CAD, placeholder)+ MOSS_MPS_AT_LIMIT floor speed in m/s at MOSS_MAX_PCT, measured+ MOSS_MAX_PCT percent ceiling, <= the firmware's 100+ MOSS_MIN_PCT static-friction floor in percent (0 = off): a non-zero track command below it is raised to it, sign kept+ MOSS_TICKS_PER_M encoder ticks per metre of track travel, measured+ MOSS_TURN_GAIN turn authority vs forward speed (teleop_speeds), 0.5..4, default 2+ MOSS_RAMP_MS "UP DOWN" sent as 'ramp UP DOWN' after connect+ MOSS_MOCK_BUS any value but 0/false/no/off: drive the fake firmware (no motors)+"""+from __future__ import annotations++import json+import logging+import math+import os+import threading+import time+from dataclasses import dataclass++from moss_dimos import robot_profile++BAUDRATE = 115200+++def default_port() -> str:+ """MOSS_PORT, else the QT Py by-id path, else a sentinel that cannot open+ (robot_profile.moss_qtpy_port). Resolved when an adapter is built, so a+ board plugged after import is still found."""+ return robot_profile.moss_qtpy_port()+++DEFAULT_PORT = default_port() # import-time snapshot, kept for callers that read it+FIRMWARE_MAX_PCT = 100 # Control.h MAX_PERCENT: the firmware refuses more+FIRMWARE_COMMAND_TIMEOUT_S = 0.300 # Control.h COMMAND_TIMEOUT_MS+FIRMWARE_ARM_TIMEOUT_S = 10.0 # Control.h ARM_TIMEOUT_MS+KEEPALIVE_S = 0.100 # re-send period, 3x inside the firmware deadman+STALE_S = 0.500 # no fresh write for this long -> "stop", once+REPLY_TIMEOUT_S = 0.150 # the firmware answers within a loop tick (ms)+CONNECT_PROBE_S = 1.000 # the first "status" of a process: reader thread start + first parse on a Jetson take longer than a reply tick+TELEMETRY_FRESH_S = 1.0 # mode read from telemetry older than this = unknown+MOCK_BUS_ENV = "MOSS_MOCK_BUS"++MODE_OFF, MODE_READY, MODE_DRIVE, MODE_TEST = 0, 1, 2, 3+++_LOGGER = None+++def _log():+ """dimOS's structlog logger when dimOS is there (that is what `dimos log`+ shows - a plain logging.getLogger() has no handler under the daemon, so+ every line of this adapter would be lost), the stdlib with a handler+ otherwise. Resolved on first use so that importing this+ module never imports dimos (the cold bench)."""+ global _LOGGER+ if _LOGGER is None:+ try:+ from dimos.utils.logging_config import setup_logger+ _LOGGER = setup_logger()+ except Exception:+ lg = logging.getLogger(__name__)+ if not lg.handlers and not logging.getLogger().handlers:+ handler = logging.StreamHandler()+ handler.setFormatter(+ logging.Formatter("%(levelname)s %(name)s: %(message)s"))+ lg.addHandler(handler)+ lg.setLevel(logging.INFO)+ _LOGGER = lg+ return _LOGGER+++def _env_float(name: str, default):+ raw = os.environ.get(name, "").strip()+ if not raw:+ return default+ try:+ return float(raw)+ except ValueError:+ _log().warning(f"MOSS base: {name}={raw!r} is not a number - using {default}")+ return default+++@dataclass+class MossGeometry:+ """Everything the twist <-> tracks mapping needs. PLACEHOLDERS until+ measured on the rover (ticks per revolution included). Each field has an+ env override so a flight can be re-scaled without a code change."""+ track_m: float = robot_profile.MOSS.track_m # PLACEHOLDER 0.266 (CAD track centre distance), to measure+ mps_at_limit: float = 0.45 # PLACEHOLDER: floor speed at 100 % (JGB37-520 178 rpm on a ~60 mm sprocket ~ 0.5 m/s), to measure+ max_pct: int = FIRMWARE_MAX_PCT+ ticks_per_m: float | None = None # None = no encoder calibration yet+ min_pct: int = 0 # static-friction floor: a non-zero track command below this percent is raised to it (sign kept). 0 = off. MOSS_MIN_PCT, to measure on the floor (the planner's 0.2 rad/s pivots map to ~8 %, which does not move the tracks)++ @classmethod+ def from_env(cls) -> "MossGeometry":+ pct = int(_env_float("MOSS_MAX_PCT", FIRMWARE_MAX_PCT))+ tpm = _env_float("MOSS_TICKS_PER_M", None)+ max_pct = max(1, min(FIRMWARE_MAX_PCT, pct))+ floor = int(_env_float("MOSS_MIN_PCT", cls.min_pct))+ return cls(track_m=_env_float("MOSS_TRACK_M", cls.track_m),+ mps_at_limit=_env_float("MOSS_MPS_AT_LIMIT", cls.mps_at_limit),+ max_pct=max_pct,+ ticks_per_m=(tpm if tpm and tpm > 0 else None),+ min_pct=max(0, min(max_pct, floor)))+++def twist_to_tracks(vx: float, wz: float, g: MossGeometry) -> tuple[int, int]:+ """Body twist -> (left, right) percent, clamped to +-g.max_pct.++ Differential drive: v_left = vx - wz*track/2, v_right = vx + wz*track/2.+ Percent is proportional to speed with g.mps_at_limit <-> g.max_pct.+ A twist too small to round to 1 % IS a stop (0, 0). A non-zero percent+ below g.min_pct is raised to g.min_pct with its sign (static-friction+ floor: the tracks do not turn below it, so the alternative is a stall)."""+ half = g.track_m / 2.0+ v_l = vx - wz * half+ v_r = vx + wz * half+ scale = g.max_pct / g.mps_at_limit if g.mps_at_limit > 0 else 0.0+ lim = int(g.max_pct)+ floor = max(0, min(lim, int(g.min_pct)))++ def pct(v: float) -> int:+ p = int(round(v * scale))+ p = max(-lim, min(lim, p))+ if p and abs(p) < floor:+ p = floor if p > 0 else -floor+ return p++ return pct(v_l), pct(v_r)+++TURN_GAIN_DEFAULT = 2.0 # MOSS_TURN_GAIN: turn authority vs forward speed, see teleop_speeds+++def turn_gain() -> float:+ return max(0.5, min(4.0, _env_float("MOSS_TURN_GAIN", TURN_GAIN_DEFAULT)))+++def teleop_envelope(g: MossGeometry) -> dict:+ """The gamepad's last gate (clamp_twist), sized to the tracks rather+ than to the gamepad's generic default. Per-axis ceilings = teleop_speeds; the rim is set+ so that a mixed command is NEVER scaled down as a whole: the adapter+ clamps each track on its own (twist_to_tracks), which is what gives the+ tank feel below. Keys match gamepad.TwistEnvelope."""+ lin, ang = teleop_speeds(g)+ lever = g.track_m / 2.0+ return {"linear": lin, "angular": ang, "lever": lever, "rim": lin + ang * lever}+++def teleop_speeds(g: MossGeometry) -> tuple[float, float]:+ """(linear m/s, angular rad/s) at FULL stick for the gamepad module, sized+ to the base: full stick forward = g.max_pct on both tracks. Full stick+ sideways = turn_gain() times the spin-in-place rate (v = wz * track/2):+ with the default gain 2, at full forward the inner track STOPS at half+ turn stick and REVERSES beyond it, full turn = a full pivot (the inner+ track reverses past a turn threshold instead of only stopping); gain 1 =+ the inner track only ever stops.+ The gamepad module's own defaults (0.6 m/s, 1.2 rad/s) would saturate at+ 25 % from the deadzone on, an on/off throttle; this keeps the stick+ proportional."""+ lin = float(g.mps_at_limit)+ ang = turn_gain() * 2.0 * lin / g.track_m if g.track_m > 0 else 0.0+ return lin, ang+++def tracks_to_twist(v_l: float, v_r: float, g: MossGeometry) -> tuple[float, float, float]:+ """(left, right) track speeds in m/s -> (vx, vy=0, wz)."""+ vx = (v_l + v_r) / 2.0+ wz = (v_r - v_l) / g.track_m if g.track_m > 0 else 0.0+ return vx, 0.0, wz+++class MossBaseAdapter:+ """TwistBaseAdapter for MOSS (two tracks, QT Py ESP32-S3 + MDD3A firmware, USB CDC).++ Args:+ dof: reported DOF (3, see module docstring).+ address: serial port; None = default_port() (MOSS_PORT, the QT Py by-id, or the sentinel).+ port_factory: injectable callable returning a serial-like object+ (write / readline / in_waiting / close / is_open). Benches inject+ the fake firmware; MOSS_MOCK_BUS does the same in a real dimOS run.+ geometry: MossGeometry; None = MossGeometry.from_env().+ """++ def __init__(self, dof: int = 3, address: str | None = None,+ hardware_id: str = "base", port_factory=None,+ geometry: MossGeometry | None = None, **_ignored):+ self._dof = dof+ self._port = address or default_port()+ self._hardware_id = hardware_id+ self._port_factory = port_factory+ self._geometry = geometry or MossGeometry.from_env()+ self._ser = None+ self._mock = False+ self._lock = threading.RLock()+ self._reply_cv = threading.Condition(self._lock)+ self._replies: list[str] = []+ self._telemetry: dict | None = None+ self._tel_logged_t = 0.0+ self._telemetry_t: float | None = None+ self._connected = False+ self._enabled = False+ self._target = (0, 0)+ self._target_twist = (0.0, 0.0, 0.0)+ self._target_t = 0.0+ self._stopped_for_stale = True+ self._last_logged = None+ self._reader = None+ self._keepalive = None+ self._stop_threads = threading.Event()+ # odometry from ticks+ self._pose = [0.0, 0.0, 0.0]+ self._last_ticks: tuple[int, int] | None = None+ self._last_ticks_t: float | None = None+ self._velocity = (0.0, 0.0, 0.0)++ # ── properties for benches ─────────────────────────────────────────+ @property+ def geometry(self) -> MossGeometry:+ return self._geometry++ @property+ def mock_bus(self) -> bool:+ return self._mock++ @property+ def target_pct(self) -> tuple[int, int]:+ return self._target++ @property+ def firmware_mode(self) -> int | None:+ """Mode from the last telemetry line, None if none or stale."""+ with self._lock:+ if self._telemetry is None or self._telemetry_t is None:+ return None+ if time.monotonic() - self._telemetry_t > TELEMETRY_FRESH_S:+ return None+ try:+ return int(self._telemetry.get("mode"))+ except (TypeError, ValueError):+ return None++ @property+ def telemetry(self) -> dict | None:+ with self._lock:+ return dict(self._telemetry) if self._telemetry else None++ @property+ def telemetry_age_s(self) -> float | None:+ """Seconds since the last telemetry line arrived (same monotonic stamp+ firmware_mode uses), None before the first one."""+ with self._lock:+ if self._telemetry_t is None:+ return None+ return max(0.0, time.monotonic() - self._telemetry_t)++ # ── connection ─────────────────────────────────────────────────────+ def _open_port(self):+ if self._port_factory is not None:+ self._mock = getattr(self._port_factory, "is_mock", False)+ return self._port_factory()+ if robot_profile.env_on(MOCK_BUS_ENV, False): # the one switch parser for MOSS_* on/off+ from .moss_mock import FakeMossFirmware+ self._mock = True+ _log().info(f"MOSS base: MOCK BUS ({MOCK_BUS_ENV} set) - no motors will move")+ return FakeMossFirmware(auto_time=True) # wall clock: live telemetry, real deadmen+ import serial+ return serial.Serial(self._port, BAUDRATE, timeout=0.05)++ def connect(self) -> bool:+ with self._lock:+ if self._connected:+ return True+ try:+ self._ser = self._open_port()+ except Exception as exc:+ _log().error(f"MOSS base: cannot open {self._port} @{BAUDRATE}: {exc!r}")+ self._ser = None+ return False+ self._stop_threads.clear()+ self._reader = threading.Thread(target=self._read_loop, name="moss-rx", daemon=True)+ self._reader.start()+ # probe: the firmware answers "status" with one telemetry line+ self._replies.clear()+ if not self._send("status", expect_status=True, timeout=CONNECT_PROBE_S):+ _log().error(f"MOSS base: no telemetry from the firmware on "+ f"{self._port} @{BAUDRATE} (is it the QT Py by-id port, "+ "and is the firmware flashed?)")+ self._teardown()+ return False+ self._connected = True+ self._keepalive = threading.Thread(target=self._keepalive_loop,+ name="moss-keepalive", daemon=True)+ self._keepalive.start()+ ramp = os.environ.get("MOSS_RAMP_MS", "").strip() # "10 5" = ms per % up / down+ if ramp and not self._mock:+ if self._send(f"ramp {ramp}"):+ _log().info(f"MOSS base: firmware ramp set to {ramp} ms per % (rise fall)")+ else:+ _log().warning(f"MOSS base: 'ramp {ramp}' refused ({self._last_err}) - firmware default kept")+ _log().info(f"MOSS base: connected on {self._port}, firmware mode="+ f"{self.firmware_mode} geometry={self._geometry}")+ return True++ def _teardown(self) -> None:+ self._stop_threads.set()+ ser, self._ser = self._ser, None+ if ser is not None:+ try:+ ser.close()+ except Exception:+ pass+ self._connected = False+ self._enabled = False++ def disconnect(self) -> None:+ with self._lock:+ if self._connected:+ try:+ self._send("stop")+ except Exception:+ pass+ self._teardown()++ def is_connected(self) -> bool:+ return self._connected++ def get_dof(self) -> int:+ return self._dof++ # ── serial plumbing ────────────────────────────────────────────────+ def _read_loop(self) -> None:+ while not self._stop_threads.is_set():+ ser = self._ser+ if ser is None:+ return+ try:+ raw = ser.readline()+ except Exception as exc:+ # the port closed under us (stack stopping): not an error+ if self._stop_threads.is_set() or not getattr(ser, "is_open", True):+ return+ _log().error(f"MOSS base: serial read failed: {exc!r}")+ time.sleep(0.05)+ continue+ if not raw:+ continue+ line = raw.decode(errors="replace").strip()+ if not line:+ continue+ with self._lock:+ if line.startswith("{"):+ try:+ self._telemetry = json.loads(line)+ self._telemetry_t = time.monotonic()+ self._ingest_ticks(self._telemetry)+ except (ValueError, TypeError):+ pass+ self._replies.append("STATUS")+ elif line == "OK" or line.startswith("ERR"):+ self._replies.append(line)+ else:+ # boot banner and the like+ _log().info(f"MOSS firmware: {line}")+ continue+ self._reply_cv.notify_all()++ def _send(self, line: str, expect_status: bool = False, timeout: float | None = None) -> bool:+ """Write one command line and wait for its reply. Called with the lock+ held (RLock). True on "OK" (or a telemetry line when expect_status)."""+ ser = self._ser+ if ser is None:+ return False+ before = len(self._replies)+ try:+ ser.write((line + "\n").encode())+ except Exception as exc:+ _log().error(f"MOSS base: serial write failed ({line!r}): {exc!r}")+ return False+ deadline = time.monotonic() + (REPLY_TIMEOUT_S if timeout is None else timeout)+ while True:+ for reply in self._replies[before:]:+ if expect_status and reply == "STATUS":+ return True+ if not expect_status and reply == "OK":+ return True+ if reply.startswith("ERR"):+ self._last_err = reply+ return False+ remaining = deadline - time.monotonic()+ if remaining <= 0:+ self._last_err = "timeout"+ return False+ self._reply_cv.wait(remaining)++ _last_err = ""++ # ── enable ─────────────────────────────────────────────────────────+ def write_enable(self, enable: bool) -> bool:+ with self._lock:+ if not self._connected:+ return False+ if not enable:+ ok = self._send("stop")+ self._enabled = False+ self._target = (0, 0)+ self._stopped_for_stale = True+ return ok+ if not self._arm():+ self._enabled = False+ return False+ self._enabled = True+ return True++ def _arm(self) -> bool:+ ok = self._send("arm")+ if not ok:+ _log().error(f"MOSS base: 'arm' refused ({self._last_err})")+ return ok++ def read_enabled(self) -> bool:+ return self._enabled++ # ── commands ───────────────────────────────────────────────────────+ def write_velocities(self, velocities: list[float]) -> bool:+ with self._lock:+ if not self._connected:+ return False+ try:+ vx, vy, wz = (list(velocities) + [0.0, 0.0, 0.0])[:3]+ vx, vy, wz = float(vx), float(vy), float(wz)+ except Exception:+ return False+ left, right = twist_to_tracks(vx, wz, self._geometry)+ if (left, right) != (0, 0) and not self._enabled:+ return False+ ok = self._drive(left, right)+ if ok:+ self._target = (left, right)+ self._target_twist = (vx, 0.0, wz)+ self._target_t = time.monotonic()+ self._stopped_for_stale = (left, right) == (0, 0)+ self._velocity = self._velocity if self._geometry.ticks_per_m else (vx, 0.0, wz)+ self._log_cmd((vx, vy, wz), (left, right))+ return ok++ def _drive(self, left: int, right: int) -> bool:+ """Send 'drive L R'; on 'ERR not_armed' (arm expiry, invalid line,+ the firmware's own deadman) re-arm and retry ONCE. Never pre-arm+ from telemetry: see the module docstring. A (0, 0) drive is sent as+ 'stop' when the firmware is not armed: stopping never needs an arm."""+ if (left, right) == (0, 0) and self.firmware_mode in (MODE_OFF, None):+ return self._send("stop")+ if self._send(f"drive {left} {right}"):+ return True+ if self._last_err == "ERR not_armed" and self._enabled:+ reason = (self._telemetry or {}).get("reason")+ _log().warning(f"MOSS base: 'drive' refused not_armed (firmware reason: "+ f"{reason}) - re-arming once")+ if self._arm() and self._send(f"drive {left} {right}"):+ return True+ _log().error(f"MOSS base: 'drive {left} {right}' refused ({self._last_err})")+ return False++ def write_stop(self) -> bool:+ with self._lock:+ if not self._connected:+ return False+ ok = self._send("stop")+ if ok:+ self._target = (0, 0)+ self._target_twist = (0.0, 0.0, 0.0)+ self._target_t = time.monotonic()+ self._stopped_for_stale = True+ if not self._geometry.ticks_per_m:+ self._velocity = (0.0, 0.0, 0.0)+ self._log_cmd((0.0, 0.0, 0.0), (0, 0))+ return ok++ def _keepalive_loop(self) -> None:+ """Refresh the last drive inside the firmware deadman while it is+ fresh; past STALE_S send one 'stop' and hold until a new write."""+ while not self._stop_threads.is_set():+ time.sleep(KEEPALIVE_S)+ with self._lock:+ if not self._connected or self._stopped_for_stale:+ continue+ age = time.monotonic() - self._target_t+ if age > STALE_S:+ _log().warning(f"MOSS base: no twist for {age:.2f}s - stop (deadman)")+ self._send("stop")+ self._target = (0, 0)+ self._target_twist = (0.0, 0.0, 0.0)+ self._stopped_for_stale = True+ if not self._geometry.ticks_per_m:+ self._velocity = (0.0, 0.0, 0.0)+ continue+ if self._target != (0, 0):+ self._send(f"drive {self._target[0]} {self._target[1]}")+ # what the firmware actually applies, and the battery+ # voltage under load: one line per 2 s+ tel = self._telemetry+ now = time.monotonic()+ if tel and now - self._tel_logged_t > 2.0:+ self._tel_logged_t = now+ _log().info(f"MOSS telemetry: out={tel.get('output_pct')} target={tel.get('target_pct')} "+ f"bus={tel.get('bus_mV')} mV ticks={tel.get('ticks')} mode={tel.get('mode')}")++ def _log_cmd(self, twist, pct) -> None:+ if pct == self._last_logged:+ return+ self._last_logged = pct+ head = "MOSS base MOCK:" if self._mock else "MOSS base:"+ vx, vy, wz = twist+ _log().info(f"{head} twist vx={vx:+.3f} vy={vy:+.3f}(ignored) wz={wz:+.3f} "+ f"-> tracks L={pct[0]:+d}% R={pct[1]:+d}% (limit {self._geometry.max_pct}%)")++ # ── feedback ───────────────────────────────────────────────────────+ def _ingest_ticks(self, tel: dict) -> None:+ """Called with the lock held, on every telemetry line."""+ tpm = self._geometry.ticks_per_m+ if not tpm:+ return+ try:+ l, r = tel["ticks"]+ l, r = int(l), int(r)+ except (KeyError, TypeError, ValueError):+ return+ now = time.monotonic()+ if self._last_ticks is not None and self._last_ticks_t is not None:+ dt = now - self._last_ticks_t+ if dt > 0:+ v_l = (l - self._last_ticks[0]) / tpm / dt+ v_r = (r - self._last_ticks[1]) / tpm / dt+ vx, _, wz = tracks_to_twist(v_l, v_r, self._geometry)+ self._velocity = (vx, 0.0, wz)+ x, y, th = self._pose+ d = (v_l + v_r) / 2.0 * dt+ dth = wz * dt+ x += d * math.cos(th + dth / 2.0)+ y += d * math.sin(th + dth / 2.0)+ th += dth+ self._pose = [x, y, th]+ self._last_ticks = (l, r)+ self._last_ticks_t = now++ def read_velocities(self) -> list[float]:+ with self._lock:+ return list(self._velocity)++ def read_odometry(self) -> list[float] | None:+ with self._lock:+ if not self._geometry.ticks_per_m:+ return None+ return list(self._pose)+++# ── telemetry payload for the bus (MossControlCoordinator.base_telemetry) ──++TELEMETRY_KEYS = ("fw", "driver", "mode", "reason", "bus_mV", "ina_ok", "ticks",+ "target_pct", "output_pct", "ramp_ms")+++def telemetry_payload(adapter, now: float) -> dict | None:+ """One base_telemetry document from anything shaped like MossBaseAdapter+ (`telemetry` dict or None, `telemetry_age_s`, `mock_bus`); None when the+ adapter has no telemetry (an adapter without a `telemetry` property, or+ MOSS before its first line), so the coordinator publishes nothing.++ Firmware keys are copied as they are; a key the flashed build does not+ send (driver before the MDD10A build) is None, never invented. `ts` is+ the caller's wall clock, `firmware_age_s` the adapter's monotonic age.+ """+ tel = getattr(adapter, "telemetry", None)+ if not isinstance(tel, dict) or not tel:+ return None+ age = getattr(adapter, "telemetry_age_s", None)+ doc = {"ts": float(now)}+ for key in TELEMETRY_KEYS:+ doc[key] = tel.get(key)+ doc["firmware_age_s"] = None if age is None else round(float(age), 3)+ doc["mock"] = bool(getattr(adapter, "mock_bus", False))+ return docdiff --git a/moss_dimos/moss_mock.py b/moss_dimos/moss_mock.pynew file mode 100644index 0000000..348ba71--- /dev/null+++ b/moss_dimos/moss_mock.py@@ -0,0 +1,261 @@+"""FakeMossFirmware: the MOSS QT Py motor firmware's serial behaviour in Python.++A serial-shaped object (write / readline / in_waiting / close / is_open) that+runs the same state machine as `firmware/MOSS_QTPY_MDD3A/Control.h` in this+repository, build "qtpy-mdd3a-v3": OFF / READY / DRIVE / TEST, the 300 ms drive+deadman, the 10 s arm expiry, the +-100 % number check, `ramp UP DOWN` (1..500+ms per % each), the replies ("OK", "ERR not_armed", "ERR invalid_command",+"ERR stopped_busy"), the telemetry JSON line with the v3 keys (fw, ramp_ms,+ina_ok, bus_mV, shunt_uV, current_mA; no "driver": that key only exists in+the MDD10A build).++Not modelled, on purpose: the ramp itself and the 100 ms reverse dwell+(output_pct always equals target_pct - the ramp setting is stored and+reported, never simulated), the partial-line timeout, the battery (bus_mV is+a constant 11600 mV, a rested 3S pack), the INA219 re-init.++Time is the fake's own millisecond clock: benches call advance(ms) to make+the firmware's watchdogs fire deterministically. Telemetry lines are queued+on `status`, on every 200 ms of advanced time, and on emit_telemetry(). With+auto_time=True (what MOSS_MOCK_BUS builds) the clock is the wall clock and+every readline() runs the watchdogs and the 200 ms telemetry, like the+firmware's loop.++Also the bus behind MOSS_MOCK_BUS, so the real dimOS runtime can be driven+end to end on a machine with no motors wired.+"""+from __future__ import annotations++import json+import threading+import time+from collections import deque++FW_NAME = "qtpy-mdd3a-v3"+MAX_PERCENT = 100+RAMP_MS_MIN, RAMP_MS_MAX = 1, 500 # Control.h RAMP_MS_MIN / RAMP_MS_MAX+RAMP_RISE_MS_DEFAULT, RAMP_FALL_MS_DEFAULT = 30, 15 # boot defaults, RAM only+BUS_MV = 11600 # constant: a rested 3S pack+COMMAND_TIMEOUT_MS = 300+ARM_TIMEOUT_MS = 10000+TEST_MS = 1000+TELEMETRY_MS = 200++OFF, READY, DRIVE, TEST = 0, 1, 2, 3+++class FakeMossFirmware:+ is_mock = True # read by MossBaseAdapter when used as a port_factory++ def __init__(self, auto_time: bool = False):+ """auto_time=True: the clock follows the wall clock (MOSS_MOCK_BUS+ runs); False: it only moves with advance(ms) (benches)."""+ self._auto = auto_time+ self._t0 = time.monotonic()+ self._ms = 0+ self.mode = OFF+ self.left = 0+ self.right = 0+ self.since = 0+ self.reason = "boot"+ self.ticks = [0, 0]+ self.ramp_ms = [RAMP_RISE_MS_DEFAULT, RAMP_FALL_MS_DEFAULT]+ self.commands: list[str] = [] # every line received, in order+ self.sent: list[str] = [] # every line emitted, in order+ self._out: deque[bytes] = deque()+ self._lock = threading.Lock()+ self._last_telemetry = 0+ self.is_open = True+ self._queue_line(f"MOSS V0.4 firmware {FW_NAME}; USB CDC 115200; limit {MAX_PERCENT}%; "+ f"ramp {self.ramp_ms[0]}/{self.ramp_ms[1]} ms per %; type status")++ # ── clock ──────────────────────────────────────────────────────────+ def now(self) -> int:+ if self._auto:+ self._ms = int((time.monotonic() - self._t0) * 1000)+ return self._ms++ def advance(self, ms: int) -> None:+ """Move the fake clock; runs the watchdogs and the 200 ms telemetry."""+ with self._lock:+ target = self._ms + int(ms)+ while self._ms < target:+ self._ms = min(target, self._ms + 10)+ self._poll()+ if self._ms - self._last_telemetry >= TELEMETRY_MS:+ self._last_telemetry = self._ms+ self._queue_line(self._telemetry_line())++ def _poll(self) -> None:+ age = self._ms - self.since+ if self.mode == DRIVE and age >= COMMAND_TIMEOUT_MS:+ self._stop("timeout")+ elif self.mode == READY and age >= ARM_TIMEOUT_MS:+ self._stop("arm_expired")+ elif self.mode == TEST and age >= TEST_MS:+ self._stop("test_done")++ def _stop(self, why: str) -> None:+ self.mode = OFF+ self.left = self.right = 0+ self.reason = why++ # ── serial side ────────────────────────────────────────────────────+ def _queue_line(self, text: str) -> None:+ self.sent.append(text)+ self._out.append((text + "\n").encode())++ @property+ def in_waiting(self) -> int:+ return sum(len(b) for b in self._out)++ def readline(self) -> bytes:+ for _ in range(4): # ~20 ms of patience, like a serial timeout+ with self._lock:+ self._auto_tick()+ if self._out:+ return self._out.popleft()+ time.sleep(0.005)+ return b""++ def _auto_tick(self) -> None:+ """Wall-clock mode (MOSS_MOCK_BUS): the reader's polling drives the+ watchdogs and the 200 ms telemetry, as the firmware's loop does - so+ a mock run shows a live battery and a firmware deadman that fires."""+ if not self._auto:+ return+ now = self.now()+ self._poll()+ if now - self._last_telemetry >= TELEMETRY_MS:+ self._last_telemetry = now+ self._queue_line(self._telemetry_line())++ def write(self, data: bytes) -> int:+ text = data.decode(errors="replace")+ for line in text.split("\n"):+ line = line.replace("\r", "")+ if line == "" and not text.endswith("\n"):+ continue+ if line == "":+ continue+ with self._lock:+ self._handle(line)+ return len(data)++ def close(self) -> None:+ self.is_open = False++ def emit_telemetry(self) -> None:+ with self._lock:+ self._queue_line(self._telemetry_line())++ # ── Control.h ──────────────────────────────────────────────────────+ @staticmethod+ def _number(s: str):+ try:+ n = int(s, 10)+ except ValueError:+ return None+ if str(n) != s.lstrip("+") and not (s.startswith("-") and str(n) == s):+ # strtol accepts "+5"; keep it simple: accept what int() accepts+ pass+ if n < -MAX_PERCENT or n > MAX_PERCENT:+ return None+ return n++ @staticmethod+ def _ramp_number(s: str):+ try:+ n = int(s, 10)+ except ValueError:+ return None+ return n if RAMP_MS_MIN <= n <= RAMP_MS_MAX else None++ def _handle(self, line: str) -> None:+ self.commands.append(line)+ now = self.now()+ self._poll()+ if any((ord(c) < 32 and c != "\t") or ord(c) > 126 for c in line) or len(line) > 95:+ self._stop("serial_invalid")+ return+ argv = line.split()+ if len(argv) > 4:+ self._stop("invalid")+ self._queue_line("ERR invalid_command")+ return+ if not argv:+ self._queue_line("OK")+ return+ if argv == ["stop"]:+ self._stop("stop")+ self._queue_line("OK")+ return+ if argv == ["status"]:+ self._queue_line(self._telemetry_line())+ return+ if argv == ["zero"]:+ if self.mode != OFF:+ self._stop("zero_requires_stop")+ self._queue_line("ERR stopped_busy")+ return+ self.ticks = [0, 0]+ self._queue_line("OK")+ return+ if argv == ["arm"]:+ if self.mode in (DRIVE, TEST):+ self._stop("arm_while_moving")+ self._queue_line("ERR stopped_busy")+ return+ self.mode = READY+ self.left = self.right = 0+ self.since = now+ self.reason = "armed"+ self._queue_line("OK")+ return+ if len(argv) == 3 and argv[0] == "drive":+ l, r = self._number(argv[1]), self._number(argv[2])+ if l is not None and r is not None:+ if self.mode not in (READY, DRIVE):+ self._stop("not_armed")+ self._queue_line("ERR not_armed")+ return+ self.left, self.right = l, r+ self.mode = DRIVE+ self.since = now+ self.reason = "drive"+ self._queue_line("OK")+ return+ if len(argv) == 3 and argv[0] == "ramp":+ # allowed in any mode, never stops anything; out of range falls+ # through to the generic invalid-command path, as in Control.h+ up, down = self._ramp_number(argv[1]), self._ramp_number(argv[2])+ if up is not None and down is not None:+ self.ramp_ms = [up, down]+ self._queue_line("OK")+ return+ if len(argv) == 3 and argv[0] == "test" and argv[1] in ("left", "right"):+ p = self._number(argv[2])+ if p is not None:+ if self.mode != OFF:+ self._stop("test_requires_stop")+ self._queue_line("ERR stopped_busy")+ return+ self.left = p if argv[1] == "left" else 0+ self.right = p if argv[1] == "right" else 0+ self.mode = TEST+ self.since = now+ self.reason = "test"+ self._queue_line("OK")+ return+ self._stop("invalid")+ self._queue_line("ERR invalid_command")++ def _telemetry_line(self) -> str:+ return json.dumps({+ "ms": self.now(), "fw": FW_NAME, "mode": self.mode, "reason": self.reason,+ "target_pct": [self.left, self.right],+ "output_pct": [self.left, self.right], # ramp not modelled: see the module docstring+ "ramp_ms": list(self.ramp_ms),+ "ticks": list(self.ticks), "invalid_edges": [0, 0],+ "ina_ok": True, "bus_mV": BUS_MV, "shunt_uV": 0, "current_mA": None,+ }, separators=(",", ":"))diff --git a/moss_dimos/recovering_planner.py b/moss_dimos/recovering_planner.pynew file mode 100644index 0000000..b8cd58c--- /dev/null+++ b/moss_dimos/recovering_planner.py@@ -0,0 +1,467 @@+"""ReplanningAStarPlanner that abandons a goal instead of fighting for it.++The stuck detector tells the truth; what goes wrong is a scripted reflex on+top of it. A blind back-off on every "stuck" verdict walks the robot+backwards, step by step, into whatever is behind it: a recovery manoeuvre+that creates the crash is worse than none. So this planner has two rules:++ * stuck -> STOP and abandon the goal (``cancel_goal`` publishes+ goal_reached=False; the explorer excludes that spot and picks another+ frontier - the pivot toward it is the only motion that follows).+ No scripted motion of any kind.+ * a CONTACT on the ``bump`` stream (on MOSS its producer is the explorer's+ born-cornered back-off) is the one thing still allowed a scripted move:+ away from the contact, 0.20 m, once. A second contact during that escape+ aborts it dead - a contact is never dropped or queued behind a running+ move.++Paths are planned on clearance_cost_map(), a cost field built from the+active robot profile (moss_dimos.robot_profile) at call time.+"""++import threading+import time++import numpy as np+from scipy import ndimage++from dimos.mapping.occupancy.gradient import voronoi_gradient+from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped+from dimos.msgs.geometry_msgs.Twist import Twist+from dimos.msgs.geometry_msgs.Vector3 import Vector3+from dimos.msgs.nav_msgs.OccupancyGrid import CostValues, OccupancyGrid+from dimos.msgs.nav_msgs.Path import Path+from dimos.core.stream import In+from dimos.navigation.base import NavigationState+from dimos.navigation.replanning_a_star.global_planner import GlobalPlanner+from dimos.navigation.replanning_a_star.min_cost_astar import min_cost_astar+from dimos.navigation.replanning_a_star.module import ReplanningAStarPlanner+from dimos_lcm.std_msgs import Bool+from dimos.utils.logging_config import setup_logger++from moss_dimos.robot_profile import RobotProfile, current as _current_profile, derived as _derived++logger = setup_logger()++ESCAPE_DISTANCE_M = 0.20 # just enough to turn+ESCAPE_SPEED_MPS = 0.10+ESCAPE_TIMEOUT_S = 5.0+ESCAPE_PERIOD_S = 0.05 # base watchdog is 0.2 s; 10 Hz left gaps that stopped the wheels++# --- obstacle cost gradient -------------------------------------------------+#+# dimOS does NOT plan on a flat costmap: global_planner.py builds a gradient+# (navigation_map.py -> path_map.py) and its A* really does pay per-cell cost+# rather than only thresholding it (min_cost_astar.py; the C++ extension+# behaves identically). On a recorded indoor map that field already keeps a+# path within 3 cm of the widest bottleneck any route could possibly have.+# Two things it still gets wrong:+#+# 1. It cannot tell a wide door from a narrow one. voronoi_gradient puts cost+# 0 on the medial axis of EVERY corridor, so a 0.65 m doorway and a 0.90 m+# doorway both offer a zero-cost route and the tie falls to path length.+# Measured: with dimOS's field alone the planner takes the 0.65 m corridor+# when a 0.90 m one of the same length is right there - the path then+# hugs the walls of the narrow door.+# 2. simple_inflate rounds its radius up to whole cells (inflation.py) and+# dilates with a disc, so the nominal 0.275 m becomes "block everything+# within 6 cells INCLUSIVE" = 0.30 m inclusive. The centre cell of a 0.55 m+# doorway sits at exactly 0.30 m and dies by one cell. Measured with a+# 0.50 m wide reference body: every gap up to 0.62 m plans "no path", and+# 0.65 m is the first that plans, although the body fits through all of+# them. That comes from dimOS's own default inflation.+#+# So: keep dimOS's ridge cost (it is already near-optimal and its zero-cost+# medial axis is what stops A*, which compares summed cell cost before length,+# from wandering off on infinite detours), take the inflation from a distance+# transform instead of a ceil'd dilation, and add one term dimOS has no way to+# know about - the turning circle.++# The two radii come from the robot profile (clearances() below):+#+# lethal = robot width / 2 + control margin. Lethal is STRICTLY below it, so+# a cell sitting exactly ON the radius stays passable. That cell is+# the centre of a doorway the body fits through, and rounding it+# away - which is what a ceil'd dilation does - walls off doorways+# the robot can drive through.+# pivot = pivot diameter / 2. Below this a turn in place sweeps into the+# obstacle. It is also where the extra cost stops: everything the+# robot can pivot in must stay at the cost dimOS gave it, because A*+# minimises the SUM of cell costs, so a penalty that never reaches+# zero is just a penalty on length. An influence radius past the+# turning circle made both clearance and path length worse.+#+# Cost added at the lethal edge, on top of dimOS's 0-99 ridge cost, and how+# sharply it falls off across the band. Both were swept on recorded maps and+# on synthetic corridors (0.50 m reference body). The exponent matters more+# than the size: A* minimises the SUM of cell costs, so a penalty that is+# still noticeable in the middle of a pinch just makes the path shorter - it+# cuts the corner and LOSES clearance. The 4th power keeps the middle of the+# band almost free and puts the whole cost against the wall. Measured over+# five recorded start/goal pairs:+#+# penalty ramp mean worst-min length corridor chosen+# 0 - 0.507 0.316 31.28 narrow+# 100 ^1 0.506 0.292 31.34 wide+# 100 ^2 0.508 0.300 31.34 wide+# 100 ^4 0.508 0.316 31.28 wide <- chosen+# 200 ^4 0.509 0.316 32.63 wide (starts detouring)+#+# So 100 with a 4th-power ramp is the one setting that takes the wide door and+# gives up nothing: same worst case and same path length as dimOS's own field.+PIVOT_PENALTY = 100+PIVOT_RAMP_EXPONENT = 4+# Matches path_map.py so the ridge cost keeps the shape dimOS tuned.+GRADIENT_DISTANCE_M = 1.5+++def clearances(profile: RobotProfile | None = None) -> tuple[float, float]:+ """(lethal, pivot) clearance radii in metres for the active robot profile,+ read at call time: lethal = width/2 + control margin, pivot = pivot+ diameter/2 (robot_profile.derived)."""+ d = _derived(profile or _current_profile())+ return d.lethal_clearance_m, d.pivot_clearance_m+++def clearance_cost_map(+ binary: OccupancyGrid,+ lethal_m: float | None = None,+ pivot_m: float | None = None,+ penalty: int = PIVOT_PENALTY,+) -> OccupancyGrid:+ """Binary costmap -> the cost field A* searches.++ Three layers: lethal only where the body cannot fit, dimOS's Voronoi ridge+ cost outside it, and a penalty across the band the body can drive through+ but not pivot in. The penalty ramps as the 4th power of how far into that+ band a cell is, so it stays near zero at the turning circle and only bites+ hard against the wall - see PIVOT_RAMP_EXPONENT for why the shape matters.++ lethal_m / pivot_m None = clearances() of the active robot profile.+ """+ if lethal_m is None or pivot_m is None:+ prof_lethal, prof_pivot = clearances()+ lethal_m = prof_lethal if lethal_m is None else lethal_m+ pivot_m = prof_pivot if pivot_m is None else pivot_m+ grid = binary.grid+ resolution = binary.resolution++ # metres from every cell to the nearest observed obstacle cell+ distance_m = ndimage.distance_transform_edt(grid < CostValues.OCCUPIED) * resolution+ # strictly inside the radius, with a tolerance so a cell landing exactly on+ # it survives float noise (6 cells x 0.05 is not exactly 0.30)+ too_close = distance_m + 1e-6 < lethal_m++ lethal = OccupancyGrid(+ grid=np.where(too_close, np.int8(CostValues.OCCUPIED), grid).astype(np.int8),+ resolution=resolution,+ origin=binary.origin,+ frame_id=binary.frame_id,+ ts=binary.ts,+ )+ base = voronoi_gradient(lethal, max_distance=GRADIENT_DISTANCE_M).grid.astype(np.int16)++ # unknown keeps its -1 (A* prices it itself, see min_cost_astar.py) and+ # lethal keeps its 100; only genuinely passable cells take the penalty.+ passable = (base >= CostValues.FREE) & (base < CostValues.OCCUPIED)+ ramp = np.clip((pivot_m - distance_m) / (pivot_m - lethal_m), 0.0, 1.0)+ extra = (penalty * ramp**PIVOT_RAMP_EXPONENT).astype(np.int16)++ out = base.copy()+ out[passable] = np.clip(+ base[passable] + extra[passable], CostValues.FREE, CostValues.OCCUPIED - 1+ )++ return OccupancyGrid(+ grid=out.astype(np.int8),+ resolution=resolution,+ origin=binary.origin,+ frame_id=binary.frame_id,+ ts=binary.ts,+ )+++class RecoveringGlobalPlanner(GlobalPlanner):+ """Stuck -> stop, abandon the goal. Contact -> one escape move, then abandon."""++ escape_distance_m: float = ESCAPE_DISTANCE_M+ escape_speed_mps: float = ESCAPE_SPEED_MPS+ escape_timeout_s: float = ESCAPE_TIMEOUT_S++ def __init__(self, *args, **kwargs) -> None: # type: ignore[no-untyped-def]+ super().__init__(*args, **kwargs)+ self._escape_abort = threading.Event()++ def _find_wide_path(self, goal: Vector3, robot_pos: Vector3) -> Path | None:+ """Plan on the clearance cost field instead of dimOS's fixed 1.1x+ inflation (global_planner.py, _find_wide_path).++ Same three steps as upstream - build a costmap, keep the cells under the+ robot passable, run A* - with our field in place of+ make_gradient_costmap(1.1). Upstream's single 1.1x try walls off+ doorways the body fits through; the lethal radius here is strictly+ smaller, so this only ever adds routes.+ """+ binary = self._navigation_map.binary_costmap+ costmap = clearance_cost_map(binary)+ self._clear_robot_footprint(costmap, binary, robot_pos)+ path = min_cost_astar(costmap, goal, robot_pos)+ if path and path.poses:+ return path+ return None++ def _replan_path(self) -> None:+ # Runs in the planner's monitoring thread. Upstream asserts a goal+ # exists; the goal can vanish (explorer cancels on "no path") while an+ # escape runs, and an assert there kills the thread for the rest of+ # the run (no "Arrived"/"stuck" is handled after it).+ try:+ if self._escaping:+ return # a contact escape owns the wheels right now+ # The stuck detector reads position only: while the follower turns in+ # place (initial/final rotation) the position does not change and a+ # 2.5 s window reads "stuck" -> replan -> new rotation -> "stuck"...+ # Rotating is not stuck.+ if not self._in_stop_message and self._local_planner_state() in ("initial_rotation", "final_rotation", "idle"):+ # "idle": while the explorer WAITS out an exclusion the follower+ # commands nothing - stillness without intent is not "stuck".+ # And the tracker's window must RESTART here: a turn in place+ # can take seconds, and if the tracker counted that stillness+ # the first blink into path_following would fire a replan - new+ # path, sometimes the other way round an obstacle, new+ # rotation, replan again: a 1 Hz bang-bang wiggle. Stillness+ # while rotating never feeds the stuck verdict.+ self._position_tracker.reset_data()+ return+ with self._lock:+ has_goal = self._current_goal is not None and self._current_odom is not None+ if not has_goal:+ logger.info("Replan requested without a goal; ignoring.")+ return+ # Stuck counts only if we were actually driving a path for a while:+ # a fresh goal with "no path" also reads as "stuck" (the tracker's+ # window is already still).+ driving_for = time.perf_counter() - self._path_started_at+ if self._position_tracker.is_stuck() and driving_for >= self._stuck_time_window:+ self.abandon_goal(trigger="stuck_detector")+ return+ super()._replan_path()+ except Exception: # noqa: BLE001 - never let the monitor thread die+ logger.exception("Replan failed; planner monitor keeps running")++ _path_started_at: float = float("inf")+ _escaping: bool = False+ _in_stop_message: bool = False+ _held_goal: PoseStamped | None = None++ def _local_planner_state(self) -> str:+ with self._local_planner._lock:+ return str(self._local_planner._state)++ def _handle_stop_message(self, stop_message) -> None: # type: ignore[override]+ # so _replan_path can tell "the follower stopped (obstacle/arrived)" from "the monitor thinks we are stuck"+ self._in_stop_message = True+ try:+ super()._handle_stop_message(stop_message)+ finally:+ self._in_stop_message = False++ # --- one writer on cmd_vel ------------------------------------------------+ #+ # The escape thread writes cmd_vel at 20 Hz. Upstream plans a goal the moment+ # it arrives, and start_planning() puts the path follower on that same+ # Subject at 10 Hz: the wheels then get interleaved forward-path and+ # reverse-escape twists until the escape ends and its cancel_goal() kills a+ # goal accepted seconds earlier - goal_reached=False, and the explorer+ # excludes a frontier it never drove to. So while an escape runs the+ # goal is HELD instead: latest-only, the last one wins, planned when the+ # escape gives the wheels back.++ def handle_goal_request(self, goal: PoseStamped) -> None:+ with self._lock:+ if self._escaping:+ self._held_goal = goal+ logger.info("Goal held: a contact escape owns the wheels")+ return+ super().handle_goal_request(goal)++ def _hold_current_goal(self) -> None:+ """An escape started under a planning attempt: keep the goal for later."""+ with self._lock:+ self._held_goal = self._current_goal+ logger.warning("Planning refused: a contact escape owns the wheels")++ def _plan_held_goal(self) -> bool:+ """Plan the goal an escape held, if any. True when there was one.++ Planned, not cancelled: it never had a follower, and cancel_goal would+ publish goal_reached=False for a goal the rover never drove."""+ with self._lock:+ goal, self._held_goal = self._held_goal, None+ if goal is None:+ return False+ logger.warning("Planning the goal held during the escape")+ self.handle_goal_request(goal)+ return True++ def abandon_goal(self, trigger: str) -> None:+ """Stop, give the goal up, no recovery motion of any kind.++ ``cancel_goal`` publishes goal_reached=False; the explorer notes the+ failed spot (an exclusion) and picks another frontier. There is no+ blind reverse here on purpose: a scripted back-off on every stuck+ verdict walks the robot into whatever is behind it."""+ logger.warning("Goal abandoned, no recovery motion", trigger=trigger)+ self._path_started_at = float("inf")+ self.cancel_goal()++ def escape(self, direction: float, trigger: str) -> bool:+ """Contact escape: the ONE scripted move left. Away from the contact,+ escape_distance_m, once, then the goal is abandoned.++ A second contact while an escape runs aborts the motion: stop dead+ rather than script another move. A contact is never dropped.++ Returns False if this contact aborted a running escape instead."""+ with self._lock:+ # under the lock: a goal request arriving right here is held, never planned+ if self._escaping:+ self._escape_abort.set()+ logger.warning("Contact during an escape: stopping dead", trigger=trigger)+ return False+ self._escaping = True+ self._escape_id += 1+ escape_id = self._escape_id+ # cleared under the lock: a halt() landing right after this+ # escape took the wheels must not be wiped by a late clear()+ self._escape_abort.clear()+ threading.Thread(target=self._escape_run, args=(direction, trigger, escape_id),+ daemon=True).start()+ return True++ _escape_abort: threading.Event+ _escape_id: int = 0++ def halt(self, trigger: str) -> bool:+ """Operator stop (stop_movement): abort a running escape, drop the+ goal it held, cancel the current goal.++ Without this a soft STOP only cancelled the goal: the escape thread+ kept writing its reverse twist, and once MovementManager's teleop+ cooldown (1 s) ran out the back-off resumed until its 0.20 m - then+ planned the goal it had held. A stop is a stop: no scripted motion+ and no held goal survive it. Contacts keep their own semantics+ (escape()). Returns True when an escape was running."""+ with self._lock:+ self._held_goal = None+ escaping = self._escaping+ if escaping:+ self._escape_abort.set()+ if escaping:+ logger.warning("Stop during an escape: stopping dead", trigger=trigger)+ self.cancel_goal()+ return escaping++ def _end_escape(self, escape_id: int) -> None:+ """Give the wheels back - unless a newer contact has already taken them."""+ with self._lock:+ if self._escape_id == escape_id:+ self._escaping = False++ def _escape_run(self, direction: float, trigger: str, escape_id: int) -> None:+ try:+ self._local_planner.stop_planning()+ with self._lock:+ start = self._current_odom+ if start is None:+ return+ twist = Twist(linear=Vector3(direction * self.escape_speed_mps, 0.0, 0.0))+ t0 = time.perf_counter()+ travelled = 0.0+ while time.perf_counter() - t0 < self.escape_timeout_s:+ if self._escape_abort.is_set():+ break+ with self._lock:+ odom = self._current_odom+ travelled = odom.position.distance(start.position)+ if travelled >= self.escape_distance_m:+ break+ self._local_planner.cmd_vel.on_next(twist)+ time.sleep(ESCAPE_PERIOD_S)+ self._local_planner.cmd_vel.on_next(Twist())+ self._position_tracker.reset_data()+ self._path_started_at = float("inf")+ logger.warning(+ "Contact escape",+ trigger=trigger,+ direction="forward" if direction > 0 else "reverse",+ travelled_m=round(travelled, 3),+ seconds=round(time.perf_counter() - t0, 1),+ aborted=self._escape_abort.is_set(),+ )+ with self._lock:+ held = self._held_goal is not None+ if not held:+ self.cancel_goal() # unchanged: a contact abandons the goal+ return+ # The wheels are free again: the flag drops BEFORE the follower can+ # be started, so cmd_vel is never written by two threads at once.+ self._end_escape(escape_id)+ self._plan_held_goal()+ except Exception: # noqa: BLE001+ logger.exception("Contact escape failed")+ finally:+ self._end_escape(escape_id)+ try:+ self._plan_held_goal() # an early return must not strand it+ except Exception: # noqa: BLE001+ logger.exception("Held goal could not be planned")++ # dimOS's default is 8 s / 0.4 m. At exploration speed (around 0.3 m/s)+ # eight seconds without progress is far too long to notice a stall:+ # 2.5 s / 0.12 m keeps about the same 5 cm/s floor.+ _stuck_time_window: float = 2.5+ _stuck_threshold: float = 0.12++ def _plan_path(self) -> None:+ if self._escaping:+ self._hold_current_goal() # a replan racing the start of an escape+ return+ super()._plan_path()+ if self._escaping:+ # a contact fired while A* ran: the escape owns the wheels, so the+ # follower that just started goes, and the goal waits for it+ self._local_planner.stop_planning()+ self._hold_current_goal()+ self._path_started_at = float("inf")+ return+ if self._local_planner.get_state() != NavigationState.IDLE:+ self._path_started_at = time.perf_counter()+ else:+ self._path_started_at = float("inf")+++class RecoveringPlanner(ReplanningAStarPlanner):+ """Drop-in for ReplanningAStarPlanner where stuck = goal abandoned (no+ motion) and a contact on the ``bump`` In triggers the one scripted escape+ move, in reverse. On MOSS the producer of ``bump`` is the explorer's+ born-cornered back-off (Explorer2.bump); any other contact source can+ publish on the same stream."""++ bump: In[Bool]++ def __init__(self, **kwargs) -> None: # type: ignore[no-untyped-def]+ super().__init__(**kwargs)+ self._planner = RecoveringGlobalPlanner(self._planner._global_config)++ def _on_stop_movement(self, msg: Bool) -> None: # type: ignore[override]+ """stop_movement (the operator's soft STOP): upstream only cancels the+ goal; here it also aborts a running contact escape and its held goal."""+ if getattr(msg, "data", False):+ self._planner.halt(trigger="stop_movement")++ async def handle_bump(self, msg: Bool) -> None:+ """Contact ahead: stop, escape 0.20 m in reverse, abandon the goal."""+ if getattr(msg, "data", False):+ acted = self._planner.escape(direction=-1.0, trigger="bump")+ logger.warning("BUMP received", reflex="escape reverse 0.20 m", acted=acted)diff --git a/moss_dimos/robot_profile.py b/moss_dimos/robot_profile.pynew file mode 100644index 0000000..bf3b341--- /dev/null+++ b/moss_dimos/robot_profile.py@@ -0,0 +1,257 @@+"""MOSS geometry: one place for the body numbers the nav stack reads.++The lidar driver, the lidar odometry, the costmap and the planner do not bake+in the rover's dimensions: they read a RobotProfile at call time through+``current()``. The values below are MOSS's, and every one of them can be+overridden by a MOSS_* environment variable (read at call time too, so a+flight can be re-scaled without a code change).++Frames: base_link IS the lidar's floor projection. The odometry publishes+world->base_link at the lidar's x, y; the costmap body footprint and the+planner clearances centre on it. A lidar mounted off the rotation centre+would need that offset applied in lidar_odometry first.++This module is pure (no dimOS import): the benches, the launcher's preflight+and the cockpit import it on any machine. It also holds MOSS's device and+state-dir resolution and the on/off switch parser (env_on), for the same+reason.+"""+from __future__ import annotations++import glob+import logging+import math+import os+import sys+from dataclasses import dataclass, replace+from pathlib import Path+from typing import NamedTuple++# Read only to warn: this package knows one robot, MOSS.+PROFILE_ENV = "ROBOT_PROFILE"++_STDLIB_LOG = logging.getLogger(__name__)+_DIMOS_LOG = None+_DIMOS_LOGGING = "dimos.utils.logging_config"+++def _log():+ """Where this module's warnings go.++ Inside a dimOS process (the blueprint build and every worker: dimOS's+ logging is already loaded there), dimOS's own logger, so the line lands+ in the run's main.jsonl - a plain logging.getLogger() has no handler+ under dimOS, and a worker's stderr is /dev/null. Anywhere else (the+ preflight, the cockpit, a bench), the stdlib logger, whose warnings reach+ stderr. Never imports dimOS itself: this module stays pure."""+ global _DIMOS_LOG+ if _DIMOS_LOG is None:+ mod = sys.modules.get(_DIMOS_LOGGING)+ if mod is None:+ return _STDLIB_LOG # not cached: dimOS may load later in this process+ try:+ _DIMOS_LOG = mod.setup_logger()+ except Exception: # noqa: BLE001 - a broken dimOS logger must not stop the profile+ return _STDLIB_LOG+ return _DIMOS_LOG+++def env_on(name: str, default: bool = True, env: dict | None = None) -> bool:+ """An on/off switch from the environment (MOSS_CAMERA, MOSS_WRIST,+ MOSS_RECORD, MOSS_RERUN, MOSS_MOCK_BUS), parsed ONE way everywhere:+ unset or empty -> default; 0 / false / no / off (any case, spaces+ stripped) -> False; anything else -> True."""+ env = os.environ if env is None else env+ raw = str(env.get(name, "")).strip().lower()+ if not raw:+ return default+ return raw not in ("0", "false", "no", "off")+++@dataclass(frozen=True)+class RobotProfile:+ name: str+ lidar_height_m: float # lidar_link above base_link+ lidar_yaw_offset_deg: float # lidar 0-deg mark vs the body's +x+ scan_min_range_m: float # returns closer than this, any direction, are dropped+ scan_mask_sectors_deg: tuple[tuple[float, float], ...] # (start, end) sectors dropped under scan_mask_range_m; may wrap past 360+ scan_mask_range_m: float+ body_box_m: tuple[float, float] # half extents |x|, |y| of returns that are the robot itself+ body_clear_m: tuple[float, float] # half extents of the footprint certainly free under the body+ robot_width_m: float+ pivot_diameter_m: float+ control_margin_m: float+ track_m: float # track centre distance+ camera_xyz_base_m: tuple[float, float, float] # depth camera in base_link (lidar_odometry's projection)+ camera_pitch_deg: float # pitched DOWN by this much+++# EVERY value below is a PLACEHOLDER until measured on the rover (CAD nominal+# or a guess with margin). Each one has an env override, read at call time:+# MOSS_LIDAR_HEIGHT_M 0.20 lidar scan plane above the floor+# MOSS_LIDAR_YAW_DEG 0.0 lidar 0-deg mark vs the nose+# MOSS_SCAN_MIN_RANGE_M 0.15+# MOSS_SCAN_MASK_DEG "" sectors "a b, c d" (deg); empty = none+# MOSS_SCAN_MASK_RANGE_M 0.30+# MOSS_BODY_BOX_M "0.22 0.18" returns inside are the robot+# MOSS_BODY_CLEAR_M "0.20 0.16" cleared under the body+# MOSS_ROBOT_WIDTH_M 0.34+# MOSS_PIVOT_DIAMETER_M 0.50+# MOSS_CONTROL_MARGIN_M 0.05+# MOSS_TRACK_M 0.266 CAD centre distance (moss_adapter reads the same variable)+# MOSS_CAMERA_XYZ_M "0.156 0 0.075" D455F face on the nose, CAD nominal+# MOSS_CAMERA_PITCH_DEG 0.0+MOSS = RobotProfile(+ name="moss",+ lidar_height_m=0.20,+ lidar_yaw_offset_deg=0.0,+ scan_min_range_m=0.15,+ scan_mask_sectors_deg=(),+ scan_mask_range_m=0.30,+ body_box_m=(0.22, 0.18),+ body_clear_m=(0.20, 0.16),+ robot_width_m=0.34,+ pivot_diameter_m=0.50,+ control_margin_m=0.05,+ track_m=0.266,+ camera_xyz_base_m=(0.156, 0.0, 0.075),+ camera_pitch_deg=0.0,+)++# field -> (env var, number of floats; 0 = list of (start, end) pairs)+_MOSS_ENV = {+ "lidar_height_m": ("MOSS_LIDAR_HEIGHT_M", 1),+ "lidar_yaw_offset_deg": ("MOSS_LIDAR_YAW_DEG", 1),+ "scan_min_range_m": ("MOSS_SCAN_MIN_RANGE_M", 1),+ "scan_mask_sectors_deg": ("MOSS_SCAN_MASK_DEG", 0),+ "scan_mask_range_m": ("MOSS_SCAN_MASK_RANGE_M", 1),+ "body_box_m": ("MOSS_BODY_BOX_M", 2),+ "body_clear_m": ("MOSS_BODY_CLEAR_M", 2),+ "robot_width_m": ("MOSS_ROBOT_WIDTH_M", 1),+ "pivot_diameter_m": ("MOSS_PIVOT_DIAMETER_M", 1),+ "control_margin_m": ("MOSS_CONTROL_MARGIN_M", 1),+ "track_m": ("MOSS_TRACK_M", 1),+ "camera_xyz_base_m": ("MOSS_CAMERA_XYZ_M", 3),+ "camera_pitch_deg": ("MOSS_CAMERA_PITCH_DEG", 1),+}++_warned: set[tuple[str, str]] = set()+++def _warn_once(var: str, raw: str, why: str) -> None:+ # call-time parsing runs every lidar revolution: say a bad value once+ if (var, raw) not in _warned:+ _warned.add((var, raw))+ _log().warning("%s=%r ignored (%s) - placeholder kept", var, raw, why)+++def _floats(raw: str) -> list[float]:+ vals = [float(t) for t in raw.replace(",", " ").replace(";", " ").split()]+ if not all(math.isfinite(v) for v in vals):+ raise ValueError("not finite")+ return vals+++def _parse(var: str, raw: str, count: int, default):+ try:+ if count == 0:+ if raw.strip().lower() in ("", "none", "-"):+ return ()+ vals = _floats(raw)+ if len(vals) % 2:+ raise ValueError("sectors come in (start, end) pairs")+ return tuple((vals[i] % 360.0, vals[i + 1] % 360.0) for i in range(0, len(vals), 2))+ vals = _floats(raw)+ if len(vals) != count:+ raise ValueError(f"expected {count} number(s)")+ return vals[0] if count == 1 else tuple(vals)+ except ValueError as exc:+ _warn_once(var, raw, str(exc))+ return default+++def moss_profile(env: dict | None = None) -> RobotProfile:+ """MOSS with the MOSS_* env overrides applied (env defaults to os.environ)."""+ env = os.environ if env is None else env+ changes = {}+ for field_name, (var, count) in _MOSS_ENV.items():+ raw = env.get(var)+ if raw is None or (raw.strip() == "" and count != 0):+ continue+ value = _parse(var, raw, count, getattr(MOSS, field_name))+ if value != getattr(MOSS, field_name):+ changes[field_name] = value+ return replace(MOSS, **changes) if changes else MOSS+++def current(env: dict | None = None) -> RobotProfile:+ """The active profile, read NOW (not at import): MOSS with its MOSS_*+ overrides. ROBOT_PROFILE is not read; set to anything but "moss" it+ earns a single warning per process, so a stray export is visible: on+ the preflight's terminal, and in the run log (main.jsonl) of a flight."""+ env = os.environ if env is None else env+ name = env.get(PROFILE_ENV, "").strip().lower()+ if name and name != "moss" and (PROFILE_ENV, name) not in _warned:+ _warned.add((PROFILE_ENV, name))+ _log().warning("ROBOT_PROFILE is ignored: this package only knows MOSS (got %r)", name)+ return moss_profile(env)+++class Derived(NamedTuple):+ lethal_clearance_m: float+ pivot_clearance_m: float+++def derived(profile: RobotProfile | None = None) -> Derived:+ """Planner/explorer clearances from the body: lethal = width/2 + margin,+ pivot = pivot diameter / 2 (MOSS defaults: 0.22 and 0.25 m)."""+ p = profile or current()+ return Derived(p.robot_width_m / 2 + p.control_margin_m, p.pivot_diameter_m / 2)+++# ── devices and state (pure: the preflight and the cockpit use them) ──++BYID_ROOT = "/dev/serial/by-id"+MOSS_QTPY_GLOB = "*QT_Py*"+MOSS_LIDAR_GLOB = "*CP2102N*"+# A missing device resolves to a path that cannot exist: the module that opens+# it fails loudly with this name in the log. Never a fallback to another+# device - on MOSS /dev/ttyUSB0 is the lidar, not the motor board.+MOSS_QTPY_MISSING = "/dev/serial/by-id/MISSING-QT-PY"+MOSS_LIDAR_MISSING = "/dev/serial/by-id/MISSING-C1"+++def resolve_by_id(pattern: str, missing: str, root: str | None = None) -> str:+ """First match (sorted, so the choice is stable) of root/pattern, else+ the sentinel. The path returned keeps the by-id form: it survives a+ re-enumeration that renumbers ttyACM/ttyUSB."""+ base = root if root is not None else BYID_ROOT+ hits = sorted(glob.glob(os.path.join(base, pattern)))+ return hits[0] if hits else missing+++def moss_qtpy_port(env: dict | None = None) -> str:+ """MOSS motor board: MOSS_PORT if set, else the QT Py by-id, else the sentinel."""+ env = os.environ if env is None else env+ return env.get("MOSS_PORT", "").strip() or resolve_by_id(MOSS_QTPY_GLOB, MOSS_QTPY_MISSING)+++def moss_lidar_port(env: dict | None = None) -> str:+ """MOSS lidar: MOSS_LIDAR_PORT if set, else the CP2102N by-id, else the sentinel."""+ env = os.environ if env is None else env+ return env.get("MOSS_LIDAR_PORT", "").strip() or resolve_by_id(MOSS_LIDAR_GLOB, MOSS_LIDAR_MISSING)+++def moss_state_dir(env: dict | None = None) -> Path:+ """MOSS_STATE_DIR, default ~/.local/state/moss (expanded, absolute)."""+ env = os.environ if env is None else env+ raw = env.get("MOSS_STATE_DIR", "").strip()+ # absolute, always: dimOS resolves a relative recorder path into its own+ # site-packages, and a cwd-relative state dir depends on who launched+ return Path(raw or "~/.local/state/moss").expanduser().absolute()+++def moss_recording_path(env: dict | None = None) -> Path:+ """Where MossRecorder writes: <state>/recordings/moss.db (absolute - a+ relative db_path lands in dimOS's site-packages)."""+ return moss_state_dir(env) / "recordings" / "moss.db"diff --git a/moss_dimos/rplidar_c1.py b/moss_dimos/rplidar_c1.pynew file mode 100644index 0000000..8324c01--- /dev/null+++ b/moss_dimos/rplidar_c1.py@@ -0,0 +1,237 @@+"""SLAMTEC RPLIDAR C1 -> flat PointCloud2 for dimOS.++dimOS has no 2D lidar path (no LaserScan message at all), so this module+publishes each 360-degree scan as a FLAT PointCloud2 (z = 0) in `lidar_link`.+LidarOdometry turns that cloud into the pose and the world-frame `lidar`+cloud the costmap is built from.++The geometry (mounting yaw, minimum range, masked sectors, the body box) comes+from the robot profile, read once per revolution, so a MOSS_* override takes+effect without a restart.++The C1 is hot-pluggable here: a missing port, a permission error or a mid-scan+unplug is logged once and retried every RETRY_PERIOD_S on the same path. The+module survives the sensor being pulled out and plugged back (its by-id path+stays the same), and with a known port (MOSS_LIDAR_PORT) it can start before+the sensor is plugged in. A lidar absent when the port was resolved gets the+sentinel path (DEFAULT_PORT) and stays silent until the stack is relaunched+with the lidar plugged in.+"""+from __future__ import annotations++import math+import threading+import time+from typing import Any++import numpy as np++from dimos.core.core import rpc+from dimos.core.module import Module+from dimos.core.stream import Out+from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2+from dimos.utils.logging_config import setup_logger++from moss_dimos import robot_profile+from moss_dimos.robot_profile import RobotProfile, current as _current_profile++logger = setup_logger()++# The port when nothing better is known: the sentinel path that cannot exist,+# so the open fails with this name in the log. The blueprint passes the port+# robot_profile.moss_lidar_port() resolves (MOSS_LIDAR_PORT, else the+# CP2102N by-id link); a module built without one resolves it the same way.+DEFAULT_PORT = robot_profile.MOSS_LIDAR_MISSING+# 460800 is the C1's line rate. The reader is plain pyserial (c1_serial.py).+DEFAULT_BAUDRATE = 460800+DEFAULT_FRAME = "lidar_link"+RETRY_PERIOD_S = 5.0+# Logged once per connection, on the first published cloud. The launcher+# greps "RPLIDAR C1 scanning": keep the words.+SCANNING_LINE = "RPLIDAR C1 scanning: %d points in the first turn"+++def polar_to_xy(angle_deg: float, distance_mm: float,+ yaw_offset_deg: float | None = None) -> tuple[float, float]:+ """One lidar measure -> (x, y) in metres in the sensor frame.++ The reader yields the heading in degrees over [0, 360) and the distance+ in millimetres. SLAMTEC's heading grows CLOCKWISE seen from above (C1+ datasheet, fig. 2-4, "left hand"), while the robot frame is right-handed+ (x forward, y left), so y is negated: 0 deg -> +X, 90 deg -> -Y.++ ``yaw_offset_deg`` is the lidar's mounting yaw (its 0-deg mark against the+ body's +x). None = the active profile's value; scan_to_points resolves it+ once per revolution and passes it in.+ """+ if yaw_offset_deg is None:+ yaw_offset_deg = _current_profile().lidar_yaw_offset_deg+ theta = math.radians(angle_deg + yaw_offset_deg)+ distance_m = distance_mm / 1000.0+ return distance_m * math.cos(theta), -distance_m * math.sin(theta)+++def _in_mask(angle_deg: float, distance_m: float,+ mask: tuple[tuple[float, float], ...], mask_range_m: float) -> bool:+ if distance_m >= mask_range_m:+ return False+ a = angle_deg % 360.0+ for start, end in mask:+ if (start <= a <= end) if start <= end else (a >= start or a <= end):+ return True+ return False+++def scan_to_points(scan: list[tuple[float, float, float]],+ min_quality: int,+ mask: tuple[tuple[float, float], ...] | None = None,+ mask_range_m: float | None = None,+ profile: RobotProfile | None = None) -> list[tuple[float, float, float]]:+ """A 360-degree scan -> flat (x, y, 0.0) points in metres.++ ``scan`` is what ``iter_scans`` yields: (quality, angle_deg, distance_mm)+ measures. Weak returns (quality below min_quality), invalid ones+ (distance 0), returns closer than the profile's minimum range, and parts+ of the robot standing in the scan plane (inside ``mask`` and closer than+ ``mask_range_m``) are dropped.++ Geometry (yaw offset, minimum range, mask, body box) comes from+ ``profile``, default the active robot_profile, resolved ONCE per scan;+ an explicit ``mask`` / ``mask_range_m`` still wins over the profile's.+ """+ p = profile or _current_profile()+ if mask is None:+ mask = p.scan_mask_sectors_deg+ if mask_range_m is None:+ mask_range_m = p.scan_mask_range_m+ yaw = p.lidar_yaw_offset_deg+ min_mm = p.scan_min_range_m * 1000.0+ pts = [(*polar_to_xy(angle, distance, yaw), 0.0)+ for (quality, angle, distance) in scan+ if quality >= min_quality and distance > min_mm+ and not _in_mask(angle, distance / 1000.0, mask, mask_range_m)]+ # Anything landing INSIDE the body rectangle is the robot itself:+ # geometric and angle-free, the profile's body box (half extents around+ # the lidar centre, margin included).+ bx, by = p.body_box_m+ return [q for q in pts if abs(q[0]) > bx or abs(q[1]) > by]+++class RPLidarC1(Module):+ """RPLIDAR C1 driver module. Publishes flat pointclouds."""++ dedicated_worker = True++ pointcloud: Out[PointCloud2]++ def __init__(self, port: str | None = None,+ baudrate: int = DEFAULT_BAUDRATE,+ frame_id: str = DEFAULT_FRAME,+ min_quality: int = 0, # at 10, ~14 % of valid returns vanished - whole thin or dark objects (table legs) with no strong return+ retry_period_s: float = RETRY_PERIOD_S,+ **kwargs: Any) -> None:+ # frame_id is a dimOS ModuleConfig field and Module exposes it as a+ # read-only property, so it goes through the config, never onto self.+ super().__init__(frame_id=frame_id, **kwargs)+ # No port given: resolved now, the way the blueprint resolves it.+ self.port = port or robot_profile.moss_lidar_port()+ self.baudrate = baudrate+ self.min_quality = min_quality+ self.retry_period_s = retry_period_s+ self._stop_event = threading.Event()+ self._thread: threading.Thread | None = None++ # @rpc is not decoration for its own sake: Module.start/stop carry it,+ # and an override that drops it falls out of the class's rpcs table.+ # dimOS then proxies the call by pickling the module across the worker+ # pipe, which dies on our threading.Event ('cannot pickle _thread.lock').+ @rpc+ def start(self) -> None:+ super().start()+ self._stop_event.clear()+ self._thread = threading.Thread(target=self._worker,+ name="rplidar-c1", daemon=True)+ self._thread.start()++ @rpc+ def stop(self) -> None:+ self._stop_event.set()+ if self._thread is not None:+ self._thread.join(timeout=3.0)+ super().stop()++ # Named _worker, NOT _loop: dimOS's Module keeps its asyncio event+ # loop in self._loop, which would shadow the method and make the+ # thread target the event loop object.+ # Injection point for the cold bench (a fake device class); None = C1Lidar.+ lidar_class: Any = None++ def _worker(self) -> None:+ from moss_dimos.c1_serial import C1Lidar+ RPLidar = self.lidar_class or C1Lidar++ last_error: str | None = None+ while not self._stop_event.is_set():+ lidar = None+ try:+ # The reader opens the serial port inside __init__: a missing+ # device or a permission error raises right here.+ lidar = RPLidar(self.port, baudrate=self.baudrate)+ logger.info("RPLIDAR C1 up on %s @ %d baud: %s",+ self.port, self.baudrate, _describe(lidar))+ last_error = None+ self._scan(lidar)+ except Exception as exc: # noqa: BLE001 - absent/unplugged sensor+ error = f"{type(exc).__name__}: {exc}"+ if error != last_error: # log once per distinct cause+ logger.warning(+ "RPLIDAR C1 unavailable on %s (%s) - retrying every "+ "%.0f s", self.port, error, self.retry_period_s)+ last_error = error+ finally:+ _shutdown(lidar)+ self._stop_event.wait(self.retry_period_s)++ def _scan(self, lidar: Any) -> None:+ """Publish one flat cloud per revolution until stop or a reader error.++ The first published cloud of each connection is logged once+ (SCANNING_LINE): "RPLIDAR C1 up" only proves the sensor answers on+ its port, this line proves it spins and sends scans. The launcher+ (tools/moss-fly.sh) waits for it before calling the lidar proven."""+ first = True+ for scan in lidar.iter_scans():+ if self._stop_event.is_set():+ return+ points = scan_to_points(scan, self.min_quality)+ if points:+ self.pointcloud.publish(PointCloud2.from_numpy(+ np.asarray(points, dtype=np.float32),+ frame_id=self.frame_id, timestamp=time.time()))+ if first:+ first = False+ logger.info(SCANNING_LINE, len(points))+++def _describe(lidar: Any) -> str:+ """Device info for the log line - never worth failing a session over."""+ try:+ return str(lidar.get_info())+ except Exception as exc: # noqa: BLE001+ return f"info unavailable ({type(exc).__name__})"+++def _shutdown(lidar: Any) -> None:+ """Stop the scan, stop the motor, close the port. Each step best-effort.++ On an unplug every step raises (the port is gone); that is expected and+ must not mask the original error or block the retry.+ """+ if lidar is None:+ return+ for step in (lidar.stop, lidar.stop_motor, lidar.disconnect):+ try:+ step()+ except Exception: # noqa: BLE001+ logger.debug("rplidar shutdown step %s failed",+ getattr(step, "__name__", step), exc_info=True)diff --git a/moss_pi/teleop.py b/moss_pi/teleop.pyindex 3f21f8e..d6c46ff 100644--- a/moss_pi/teleop.py+++ b/moss_pi/teleop.py@@ -32,7 +32,7 @@ from moss_dimos.teleop_logic import DeadmanBrake, TeleopConfig, axes_to_twistfrom .serial_bridge import MAX_PERCENT, SerialBridge, open_serial-# Standard dual-stick pad, pygame numbering (same as moss_dimos/gamepad.py).+# Standard dual-stick pad, pygame numbering (axes as moss_dimos/gamepad.py; the deadman differs: L1 here, R1 there).AXIS_LEFT_Y = 1AXIS_RIGHT_X = 2BUTTON_ARM = 0 # A / crossdiff --git a/pyproject.toml b/pyproject.tomlindex f096d01..dcad08c 100644--- a/pyproject.toml+++ b/pyproject.toml@@ -1,16 +1,42 @@+[build-system]+requires = ["setuptools>=64", "wheel"]+build-backend = "setuptools.build_meta"+[project]name = "moss-dimos"-version = "0.0.1"-description = "MOSS litter-picking rover: dimOS modules (scaffold)"+version = "0.1.0"+description = "MOSS litter-picking rover: dimOS modules and blueprints (base, lidar, odometry, map, planner, explorer, gamepad, recorder), plus the Pi teleop path"readme = "README.md"license = {text = "Apache-2.0"}requires-python = ">=3.10"-dependencies = ["numpy"]+dependencies = ["numpy", "pyserial>=3.5", "scipy"][project.optional-dependencies]+# No `dimos` extra on purpose. MOSS runs against one dimOS commit, installed+# from git as Software.md says:+# DIMOS_ALLOW_MISSING_COCKPIT=1 uv pip install --python .venv/bin/python \+# "dimos @ git+https://github.com/dimensionalOS/dimos.git@a7d19f76158b01739b26e7a17390f89764aaef83"+# The variable builds dimOS without its web UI, which MOSS does not use. dimOS+# main has since moved the planner and the explorer this package imports, so a+# newer commit must pass benches/run_all.sh first. moss_pi and the pure parts of+# moss_dimos (robot_profile, moss_adapter, moss_mock) need none of it.+nav = ["kiss-icp==1.3.0"] # lidar_odometry calls kiss-icp internals, run against this releasegamepad = ["pygame>=2.6"]+camera = ["pyrealsense2"] # the RealSense; on a Jetson see Software.md+firmware = ["esptool", "pyserial>=3.5"] # firmware/MOSS_QTPY_MDD3A/bench/flash_qtpy.shpi = ["pyserial>=3.5", "pygame>=2.6"]test = ["pytest"]+# dimOS names these `moss-dimos.<name>`: dimos run moss-dimos.moss-nav+[project.entry-points."dimos.blueprints"]+moss-base = "moss_dimos.blueprints:moss_base_blueprint"+moss-gamepad = "moss_dimos.blueprints:moss_gamepad_blueprint"+moss-nav = "moss_dimos.blueprints:moss_nav_blueprint"+moss-explore = "moss_dimos.blueprints:moss_explore_blueprint"+[tool.setuptools]packages = ["moss_dimos", "moss_pi"]++[tool.pytest.ini_options]+# the script benches live in benches/ and run through benches/run_all.sh+testpaths = ["tests"]diff --git a/tools/explore_ctl.py b/tools/explore_ctl.pynew file mode 100755index 0000000..8f7d768--- /dev/null+++ b/tools/explore_ctl.py@@ -0,0 +1,111 @@+#!/usr/bin/env python3+"""Start or stop autonomous exploration from outside the stack (usage: explore_ctl.py start|stop).++The explorer listens on two Bool streams, `explore_cmd` and `stop_explore_cmd`.+This script publishes Bool(True) on the LCM channel dimOS derives for each of+them (the stream name, then the message type after a '#'):++ start /explore_cmd#std_msgs.Bool+ stop /stop_explore_cmd#std_msgs.Bool++MOSS runs its stack on LCM, the launchers' default. dimOS picks the bus from+DIMOS_TRANSPORT first, then TRANSPORT (neither set: lcm); this script reads+them in the same order. Under any other transport nobody would hear an LCM+publish, so the script refuses to publish and says why instead of reporting a+success that reached no one.++Importable: tools/moss_cockpit.py publishes goals, the soft stop and episode+markers through publish_msg(name, msg), which lands any dimos_lcm message on+the same bus with the same naming rule.+"""+import os+import sys++from dimos_lcm.std_msgs import Bool++CHANNELS = {"start": "explore_cmd", "stop": "stop_explore_cmd"}+SUPPORTED_TRANSPORT = "lcm"+TRANSPORT_VARS = ("DIMOS_TRANSPORT", "TRANSPORT") # dimOS's order: the first one set wins+++class TransportError(RuntimeError):+ """The run is not on LCM: a publish from here would reach nobody."""+++def transport_setting(env=None) -> tuple:+ """(the variable that chose the bus, the bus) from `env` (default: this process's+ environment), in dimOS's order: DIMOS_TRANSPORT, then TRANSPORT, case-insensitive.+ Neither set: ("", "lcm"), dimOS's default on Linux."""+ env = os.environ if env is None else env+ for var in TRANSPORT_VARS:+ value = (env.get(var) or "").strip().lower()+ if value:+ return var, value+ return "", SUPPORTED_TRANSPORT+++def transport(env=None) -> str:+ """The run's bus, read in dimOS's order (see transport_setting)."""+ return transport_setting(env)[1]+++def require_lcm(env=None) -> None:+ """Raise TransportError, naming the variable it read and the fix, unless the run is on LCM."""+ var, bus = transport_setting(env)+ if bus != SUPPORTED_TRANSPORT:+ raise TransportError(f"{var}={bus}: explore_ctl publishes on LCM only, the bus MOSS runs on "+ f"(its launchers default DIMOS_TRANSPORT and TRANSPORT to lcm). Set both to "+ f"lcm, or unset both.")+++def channel(name: str, msg_name: str) -> str:+ """The LCM channel of stream `name` carrying `msg_name`."""+ return f"/{name}#{msg_name}"+++def topic(cmd: str) -> str:+ """The LCM channel this command lands on."""+ return channel(CHANNELS[cmd], Bool.msg_name)+++def publish_msg(name: str, msg, msg_name: str = None, bus=None, env=None) -> str:+ """Publish any dimos_lcm message on stream `name`; returns the channel used.++ `msg_name` is the type suffix dimOS puts in the channel. It defaults to the+ message's own msg_name, but pass it for types whose dimos_lcm class carries a+ bare name: dimos_lcm's PoseStamped says "PoseStamped" while dimOS's wrapper+ (and so every stack channel) says "geometry_msgs.PoseStamped". `bus`: an+ already open lcm.LCM to publish through (a long-running caller keeps one);+ None opens one per call. `env`: the environment that chose the run's bus (the+ cockpit passes the running stack's); None reads this process's. Raises+ TransportError when the run is not on LCM.+ """+ require_lcm(env)+ where = channel(name, msg_name or msg.msg_name)+ payload = msg.lcm_encode()+ if bus is None:+ import lcm # imported here: the cockpit runs without it in demo mode++ bus = lcm.LCM()+ bus.publish(where, payload)+ return where+++def publish(cmd: str) -> str:+ """Publish Bool(True) for 'start'|'stop'; returns the channel used."""+ return publish_msg(CHANNELS[cmd], Bool(data=True))+++def main() -> None:+ cmd = sys.argv[1] if len(sys.argv) > 1 else "start"+ if cmd not in CHANNELS:+ sys.exit("usage: explore_ctl.py start|stop")+ try:+ used = publish(cmd)+ except TransportError as exc:+ sys.exit(f"explore_ctl: {exc}")+ print(f"published {used} on {transport()}")+++if __name__ == "__main__":+ main()diff --git a/tools/lcm-loopback.service b/tools/lcm-loopback.servicenew file mode 100644index 0000000..9b583ed--- /dev/null+++ b/tools/lcm-loopback.service@@ -0,0 +1,23 @@+# LCM on loopback, set once at boot: what dimOS otherwise sets with sudo at every+# launch (a launch that cannot sudo dies without the first two). With this+# unit, a stack launched from the cockpit page needs no sudo (docs/cockpit.md).+# Not a template: install it as it is, from the checkout:+# sudo cp tools/lcm-loopback.service /etc/systemd/system/+# sudo systemctl daemon-reload && sudo systemctl enable lcm-loopback && sudo systemctl restart lcm-loopback+# Check: ip -o link show lo (MULTICAST), ip route show 224.0.0.0/4 (dev lo),+# sysctl net.core.rmem_max net.core.rmem_default (67108864 each).+[Unit]+Description=LCM multicast on loopback and socket buffers (dimOS)+After=network.target+Before=moss-cockpit.service++[Service]+Type=oneshot+RemainAfterExit=yes+ExecStart=/usr/sbin/ip link set lo multicast on+# replace, not add: idempotent, and it also moves a 224.0.0.0/4 route set on another interface+ExecStart=/usr/sbin/ip route replace 224.0.0.0/4 dev lo+ExecStart=/usr/sbin/sysctl -w net.core.rmem_max=67108864 net.core.rmem_default=67108864++[Install]+WantedBy=multi-user.targetdiff --git a/tools/moss-cockpit.service b/tools/moss-cockpit.servicenew file mode 100644index 0000000..97af50b--- /dev/null+++ b/tools/moss-cockpit.service@@ -0,0 +1,36 @@+# MOSS cockpit server: the bus tap and the page on :8904 (docs/cockpit.md).+# A template: MOSS_USER is the account that runs the rover's stack, MOSS_DIR the+# absolute path of this checkout (with its .venv). Install from the checkout:+# sed -e "s|MOSS_USER|$USER|g" -e "s|MOSS_DIR|$PWD|g" tools/moss-cockpit.service \+# | sudo tee /etc/systemd/system/moss-cockpit.service >/dev/null+# sudo systemctl daemon-reload && sudo systemctl enable moss-cockpit && sudo systemctl restart moss-cockpit+# `restart`, not `enable --now`: a cockpit already running (an older install, or+# another checkout) keeps serving its own code until it is restarted. The same+# three lines reinstall it after the checkout moved.+# No PrivateDevices here on purpose: the cockpit never opens a device itself, but the+# launcher it runs on request (tools/moss-fly.sh) must reach the motor board and the lidar.+# No NoNewPrivileges either, on purpose: a stack launched from the page inherits this+# unit's flags, and dimOS sets loopback multicast, its 224.0.0.0/4 route and the+# socket buffers with sudo at launch. Either the stack's user has passwordless sudo,+# or tools/lcm-loopback.service sets them at boot (ordered before this unit).+[Unit]+Description=MOSS cockpit (bus tap + web page)+Wants=lcm-loopback.service+After=network.target lcm-loopback.service++[Service]+Type=simple+User=MOSS_USER+WorkingDirectory=MOSS_DIR+Environment=GLIBC_TUNABLES=glibc.rtld.optional_static_tls=4194304+Environment=MOSS_COCKPIT_PORT=8904+ExecStart=MOSS_DIR/.venv/bin/python tools/moss_cockpit.py+Restart=always+RestartSec=3+# A stack launched from the page (tools/moss-fly.sh, then the dimOS daemon) lives+# in this unit's cgroup: a cockpit restart, crash or stop must end the cockpit+# only, never a flight it started. Stop a flight with tools/moss-fly.sh stop.+KillMode=process++[Install]+WantedBy=multi-user.targetdiff --git a/tools/moss-fly.sh b/tools/moss-fly.shnew file mode 100755index 0000000..80e4835--- /dev/null+++ b/tools/moss-fly.sh@@ -0,0 +1,384 @@+#!/usr/bin/env bash+# MOSS flight: gates -> mapping or exploration stack -> cockpit. Runs ON THE+# ROVER (the onboard computer the board, the lidar and the cameras are plugged+# into), from this checkout, with its .venv:+#+# tools/moss-fly.sh check flight check (tools/moss_preflight.py), nothing launched+# tools/moss-fly.sh map gates, then moss-dimos.moss-nav (mapping + pad, recording)+# tools/moss-fly.sh explore gates, then moss-dimos.moss-explore; exploration NOT armed+# EXPLORE=1 tools/moss-fly.sh explore ... and arm exploration once the stack is proven+# tools/moss-fly.sh stop stop exploration, then the stack+# tools/moss-fly.sh log [N] what the base, the lidar, the map and the pad said+# tools/moss-fly.sh cockpit start the cockpit (tools/moss_cockpit.py) if needed, print its URL+# MOCK=1 tools/moss-fly.sh map fake firmware (MOSS_MOCK_BUS=1); the lidar is still real+#+# From another machine: ssh <user>@<rover> 'cd ~/moss && tools/moss-fly.sh map'+# tools/moss-teleop.sh stays the pad-only launcher.+#+# Every gate REFUSES rather than guesses, with the fix on the same line.+# Nothing moves by itself: in `map` only the pad drives (deadman held); in+# `explore` the planner moves only on a goal or once exploration is armed+# (EXPLORE=1 here, or the cockpit's button). The firmware stops the tracks+# 300 ms after the last command whatever happens up here.+set -u+cd "$(dirname "$0")/.." || exit 1+DIMOS=.venv/bin/dimos+PY=.venv/bin/python+BYID_DIR=/dev/serial/by-id+# dimOS's state dir (XDG on Linux): run registry and, for an installed dimOS,+# the per-run logs. A dimOS run from a git clone logs under the clone instead,+# which is why the run dir is read from the registry first (see run_dir).+DIMOS_STATE="${XDG_STATE_HOME:-$HOME/.local/state}/dimos"+RUNS_DIR="$DIMOS_STATE/runs"+LOG_ROOT="$DIMOS_STATE/logs"+LAUNCH_LOG=/tmp/moss_fly_launch.log+COCKPIT_LOG=/tmp/moss_cockpit.log+# One check/map/explore at a time: a second launcher would sweep the first one's+# half-built stack as a zombie, or open the same board.+FLY_LOCK=/tmp/moss_fly.lock+# `dimos [root options] run <bp>`: the options sit between dimos and run, so a+# plain "bin/dimos run" never matches a real launch.+STACK_RE='[b]in/dimos( [^ ]+)* run( |$)'+# The cockpit itself (an interpreter running it), not an editor, a viewer or a+# shell line that only names the file.+COCKPIT_RE='^[^ ]*python[0-9.]* ([^ ]*/)?[t]ools/moss_cockpit\.py( |$)'+# Where a cockpit's working directory (its checkout) is read: /proc/<pid>/cwd.+PROC_DIR=/proc+PROOF_S=${PROOF_S:-120} # the lidar odometry's first kiss-icp import can take over 60 s on a cold cache+EXPLORE_PROOF_S=15+# FLY_TEST=1 swaps every command that reaches the stack, the clock or the+# network for a recorder (benches/bench_moss_fly_gates_cold.py). Nothing else sets it.+if [ "${FLY_TEST:-0}" = "1" ]; then . "${FLY_TEST_STUBS:?FLY_TEST=1 needs FLY_TEST_STUBS}"; fi++usage() {+ echo "usage: tools/moss-fly.sh check|map|explore|stop|log [N]|cockpit (EXPLORE=1 with explore, MOCK=1 with map/explore)"+}++# The stack's environment, exported once so the flight check sees exactly what+# the stack will see.+export GLIBC_TUNABLES=glibc.rtld.optional_static_tls=4194304 # larger static TLS reserve (some aarch64 imports fail without it); harmless elsewhere+# One bus under both names: dimOS reads DIMOS_TRANSPORT first, then TRANSPORT;+# the cockpit and tools/explore_ctl.py speak LCM only.+BUS="${DIMOS_TRANSPORT:-${TRANSPORT:-lcm}}"+export DIMOS_TRANSPORT="$BUS" TRANSPORT="$BUS"+export MOSS_MAX_PCT="${MOSS_MAX_PCT:-100}" # firmware limit 100; lower it for a cautious lap+export MOSS_RAMP_MS="${MOSS_RAMP_MS:-5 5}" # firmware ramp, ms per % up/down+export MOSS_STATE_DIR="${MOSS_STATE_DIR:-$HOME/.local/state/moss}"+export MOSS_RECORD="${MOSS_RECORD:-1}"+export MOSS_CAMERA="${MOSS_CAMERA:-1}"+NERF_SPEED="${MOSS_NERF_SPEED:-0.5}"+COCKPIT_PORT="${MOSS_COCKPIT_PORT:-8904}"+PREFLIGHT_JSON="$MOSS_STATE_DIR/preflight.json" # served by the cockpit as /preflight.json+if [ "${MOCK:-0}" = "1" ]; then export MOSS_MOCK_BUS=1; fi++# The ports come from by-id, never from the shell: a stale export of MOSS_PORT+# naming the CP2102N would aim motor commands at the lidar.+resolve_ports() {+ QTPY=$(ls "$BYID_DIR"/*QT_Py* 2>/dev/null | head -1)+ C1=$(ls "$BYID_DIR"/*CP2102N* 2>/dev/null | head -1)+ if [ -n "${MOSS_PORT:-}" ] && [ "$MOSS_PORT" != "$QTPY" ]; then+ echo " (ignoring MOSS_PORT=$MOSS_PORT from the shell: the by-id path wins)"+ fi+ if [ -n "${MOSS_LIDAR_PORT:-}" ] && [ "$MOSS_LIDAR_PORT" != "$C1" ]; then+ echo " (ignoring MOSS_LIDAR_PORT=$MOSS_LIDAR_PORT from the shell: the by-id path wins)"+ fi+ if [ -n "$QTPY" ]; then export MOSS_PORT="$QTPY"; else unset MOSS_PORT; fi+ if [ -n "$C1" ]; then export MOSS_LIDAR_PORT="$C1"; else unset MOSS_LIDAR_PORT; fi+}++cockpit_url() {+ local host+ host=$(hostname -I 2>/dev/null | awk '{print $1}')+ echo "http://${host:-127.0.0.1}:$COCKPIT_PORT/"+}++cockpit_answers() {+ curl -sf -m 3 -o /dev/null "http://127.0.0.1:$COCKPIT_PORT/state.json"+}++# The running cockpits, by checkout: OWN_COCKPIT is the first one whose working+# directory is this checkout, FOREIGN_COCKPIT "<pid> <dir>" the first one+# running anywhere else. Only this checkout's cockpit counts: another clone's+# page launches that clone's launcher and code.+find_cockpits() {+ local q cwd here+ here=$(pwd -P)+ OWN_COCKPIT=""; FOREIGN_COCKPIT=""+ for q in $(pgrep -f "$COCKPIT_RE"); do+ cwd=$(readlink -f "$PROC_DIR/$q/cwd" 2>/dev/null)+ if [ -n "$cwd" ] && [ "$cwd" = "$here" ]; then+ [ -n "$OWN_COCKPIT" ] || OWN_COCKPIT=$q+ elif [ -z "$FOREIGN_COCKPIT" ]; then+ FOREIGN_COCKPIT="$q ${cwd:-(unreadable: another user owns it?)}"+ fi+ done+}++# Refuses (returns 0, with the fix) when find_cockpits saw a foreign cockpit.+# Never kills it: it may be someone else's.+refuse_foreign_cockpit() {+ [ -n "$FOREIGN_COCKPIT" ] || return 1+ echo "A COCKPIT FROM ANOTHER CHECKOUT is running (pid ${FOREIGN_COCKPIT%% *}, in ${FOREIGN_COCKPIT#* }): its page would launch that checkout's code - no flight from here."+ echo " if it is moss-cockpit.service: install tools/moss-cockpit.service from $(pwd -P) (docs/cockpit.md), then"+ echo " sudo systemctl daemon-reload && sudo systemctl enable moss-cockpit && sudo systemctl restart moss-cockpit"+ echo " otherwise stop it: kill ${FOREIGN_COCKPIT%% *}"+ return 0+}++# Passive server (bus listener, never a serial port): start it when absent,+# restart it when its file is newer than the process (an old server keeps+# serving old code on a live port after a sync). Only this checkout's cockpit+# is used, restarted or killed; another checkout's is refused (above).+cockpit_up() {+ local p+ find_cockpits+ refuse_foreign_cockpit && return 1+ p=$OWN_COCKPIT+ if [ -n "$p" ] && [ "$(stat -c %Y tools/moss_cockpit.py)" -gt "$(stat -c %Y "$PROC_DIR/$p" 2>/dev/null || echo 0)" ]; then+ echo " cockpit pid $p is older than tools/moss_cockpit.py - restarting it"+ if command -v systemctl >/dev/null 2>&1 && systemctl is-active --quiet moss-cockpit 2>/dev/null; then+ # the unit owns the port: restart it, never race it with a nohup twin+ echo " it runs under moss-cockpit.service - restarting the unit"+ if ! sudo -n systemctl restart moss-cockpit 2>/dev/null \+ && ! systemctl --no-ask-password restart moss-cockpit 2>/dev/null; then+ echo " COULD NOT RESTART moss-cockpit (no passwordless sudo): the page still runs the code it started with - sudo systemctl restart moss-cockpit"+ fi+ sleep 2; cockpit_answers && return 0; return 1+ fi+ kill "$p" 2>/dev/null; sleep 1; p=""+ fi+ cockpit_answers && return 0+ if [ -z "$p" ]; then+ # Without this run's switches: a cockpit born under EXPLORE=1 or MOCK=1 would+ # pass them to every launch from its page. fd 9 (the launch lock) stays here.+ ( unset EXPLORE MOCK MOSS_MOCK_BUS+ nohup $PY tools/moss_cockpit.py --port "$COCKPIT_PORT" > "$COCKPIT_LOG" 2>&1 < /dev/null 9>&- & )+ fi+ for _ in $(seq 1 15); do+ sleep 1+ cockpit_answers && return 0+ done+ return 1+}++take_lock() {+ exec 9>>"$FLY_LOCK" || { echo "CANNOT OPEN $FLY_LOCK - remove it if another user owns it"; exit 1; }+ flock -n 9 || { echo "ANOTHER moss-fly.sh IS RUNNING (check, map or explore) - wait for it, or tools/moss-fly.sh stop"; exit 1; }+}++# The log dir of THIS launch. First the newest dimOS registry entry written+# after LAUNCH_MARK whose pid is alive and whose blueprint is $BP: it carries+# the run's log_dir wherever dimOS keeps its logs, but dimOS writes it only+# once the build passed. Until then, the newest <ts>-moss-dimos-$BP dir under+# LOG_ROOT created after LAUNCH_MARK (the build's own lines land there for an+# installed dimOS). An older run's dir never counts: its log also says+# "RPLIDAR C1 up".+run_dir() {+ local f pid bp d+ while IFS= read -r f; do+ [ -n "$f" ] || continue+ [ "$(stat -c %Y "$f" 2>/dev/null || echo 0)" -ge "$LAUNCH_MARK" ] || break # newest first: the rest is older+ pid=$(grep -o '"pid": *[0-9]*' "$f" 2>/dev/null | head -1 | grep -o '[0-9]*$')+ bp=$(grep -o '"blueprint": *"[^"]*"' "$f" 2>/dev/null | head -1 | sed 's/.*"\([^"]*\)"$/\1/')+ d=$(grep -o '"log_dir": *"[^"]*"' "$f" 2>/dev/null | head -1 | sed 's/.*"\([^"]*\)"$/\1/')+ if [ -n "$pid" ] && [ -d "/proc/$pid" ] && [ "${bp##*.}" = "$BP" ] && [ -n "$d" ] && [ -d "$d" ]; then+ echo "${d%/}"; return 0+ fi+ done < <(ls -t "$RUNS_DIR"/*.json 2>/dev/null)+ d=$(ls -td "$LOG_ROOT"/*-moss-dimos-"$BP"/ 2>/dev/null | head -1)+ if [ -n "$d" ] && [ "$(stat -c %Y "$d" 2>/dev/null || echo 0)" -ge "$LAUNCH_MARK" ]; then+ echo "${d%/}"; return 0+ fi+ return 1+}++stack_down() {+ $DIMOS stop 2>&1 | tail -2+ sleep 1+ if pgrep -f "$STACK_RE" >/dev/null; then+ echo "dimos still up - killing"; pkill -f "$STACK_RE"+ fi+}++CMD="${1:-}"+case "$CMD" in+ stop)+ # exploration first: the explorer stops publishing goals before the stack goes+ $PY tools/explore_ctl.py stop 2>&1 | tail -1+ stack_down+ echo "stack down - the tracks stopped on the firmware deadman (300 ms) before this line"+ exit 0 ;;+ log)+ N="${2:-30}"+ case "$N" in ''|*[!0-9]*) usage; exit 1 ;; esac+ # dimos log keeps its last 50 lines by default: filter a window wide enough+ # that the startup lines survive the 2 s telemetry chatter.+ W=$(( N * 5 > 200 ? N * 5 : 200 ))+ $DIMOS log -n "$W" 2>/dev/null | grep -a -E "MOSS|RPLIDAR|lidar odom|costmap|Gamepad|Traceback|Error" | tail -n "$N"+ exit 0 ;;+ cockpit)+ cockpit_up || { [ -n "$FOREIGN_COCKPIT" ] || echo "NO COCKPIT on :$COCKPIT_PORT - see $COCKPIT_LOG"; exit 1; }+ echo "cockpit: $(cockpit_url)"+ exit 0 ;;+ check)+ take_lock+ resolve_ports+ $PY tools/moss_preflight.py --save "$PREFLIGHT_JSON"+ exit $? ;;+ map) BP=moss-nav ;;+ explore) BP=moss-explore ;;+ *) usage; exit 1 ;;+esac++if [ "${EXPLORE:-0}" = "1" ] && [ "$CMD" != "explore" ]; then+ echo "CONTRADICTORY: EXPLORE=1 with '$CMD' - the mapping stack has no explorer; use: EXPLORE=1 tools/moss-fly.sh explore"; exit 1+fi+take_lock+N=3; [ "${EXPLORE:-0}" = "1" ] && N=4++echo "== 0/$N no dimOS stack already up =="+# A dimos process WITH a registered run is a live flight: refuse, untouched. One+# WITHOUT (the CLI says "No running DimOS instance") is either still building+# (dimOS registers a run only after its build and health check) or a zombie+# holding the ports. Younger than a proof window = building: refuse, untouched.+# Older, with that explicit answer = zombie: swept.+if pgrep -f "$STACK_RE" >/dev/null; then+ if ! $DIMOS status 2>/dev/null | grep -q "No running DimOS instance"; then+ echo "A DIMOS STACK IS ALREADY RUNNING - tools/moss-fly.sh stop first"; exit 1+ fi+ for sp in $(pgrep -f "$STACK_RE"); do+ age=$(ps -o etimes= -p "$sp" 2>/dev/null | tr -d ' ')+ if [ -n "$age" ] && [ "$age" -lt $((PROOF_S + 30)) ]; then+ echo "A DIMOS STACK IS STILL STARTING (pid $sp, ${age} s old, not registered yet) - wait for it, or tools/moss-fly.sh stop"; exit 1+ fi+ done+ echo " zombie dimos process (no registered run) - killing it (the firmware deadman already stopped the tracks)"+ pkill -9 -f "$STACK_RE"; sleep 2+ if pgrep -f "$STACK_RE" >/dev/null; then+ echo "THE ZOMBIE SURVIVED kill -9 - no flight, look at it by hand: pgrep -af 'bin/dimos'"; exit 1+ fi+fi+echo " none"++echo "== 1/$N flight check =="+resolve_ports+if [ "${MOCK:-0}" != "1" ] && [ -z "$QTPY" ]; then+ echo "NO QT PY on USB ($BYID_DIR/*QT_Py*) - cable, power, or the board is in its bootloader"; exit 1+fi+if [ -z "$C1" ]; then+ echo "NO LIDAR on USB ($BYID_DIR/*CP2102N*) - lidar cable or hub (mapping needs it, MOCK or not)"; exit 1+fi+[ "${MOCK:-0}" = "1" ] && echo " MOCK=1: fake firmware, the board is not touched"+# A cockpit from another checkout would be the one watching (and relaunching)+# this flight: refused before anything starts.+find_cockpits+refuse_foreign_cockpit && exit 1+# --no-spin, not --quick: gate 2 proves the lidar on its first scan, but only+# this listen reads the battery and proves the firmware disarmed before the+# stack opens it.+$PY tools/moss_preflight.py --no-spin --save "$PREFLIGHT_JSON" \+ || { echo "FLIGHT CHECK KO - no flight: fix the KO lines above (tools/moss-fly.sh check for the full check)"; exit 1; }++echo "== 2/$N stack: moss-dimos.$BP =="+: > "$LAUNCH_LOG"+LAUNCH_MARK=$(date +%s)+nohup $DIMOS --rerun-open none --nerf-speed "$NERF_SPEED" run "moss-dimos.$BP" --daemon > "$LAUNCH_LOG" 2>&1 < /dev/null 9>&- &+# The proof is read in THIS launch's run dir (run_dir): an older run's log also+# says "RPLIDAR C1 up" and would pass a dead launch as alive. The lidar counts+# as proven on its first scan ("RPLIDAR C1 scanning"), not on "RPLIDAR C1 up",+# which only says it answers on its port: a lidar whose motor does not spin+# answers too. The three log lines are not enough either: the build starts+# every module in parallel, and one failing after them (the camera) tears the+# whole stack down. Only the launcher's own success line closes the build.+PROOF_RE="MOSS base: connected|MOSS base: MOCK BUS|RPLIDAR C1 up|RPLIDAR C1 scanning|RPLIDAR C1 unavailable|lidar odometry up|Traceback|cannot open|no telemetry"+LAUNCH_OK_RE="DimOS running in background"+RUN_DIR=""; L=""; OK_BASE=0; OK_LIDAR=0; OK_ODOM=0; OK_LAUNCH=0; BAD=""+for _ in $(seq 1 "$PROOF_S"); do+ sleep 1+ if grep -q "^Error:" "$LAUNCH_LOG" 2>/dev/null; then BAD=$(grep -m1 "^Error:" "$LAUNCH_LOG"); break; fi+ D=$(run_dir)+ if [ -n "$D" ]; then+ RUN_DIR="$D"+ L=$(grep -a -E "$PROOF_RE" "$RUN_DIR/main.jsonl" 2>/dev/null)+ echo "$L" | grep -q -E "MOSS base: connected|MOSS base: MOCK BUS" && OK_BASE=1+ echo "$L" | grep -q "RPLIDAR C1 scanning" && OK_LIDAR=1+ echo "$L" | grep -q "lidar odometry up" && OK_ODOM=1+ BAD=$(echo "$L" | grep -a -E "Traceback|cannot open|no telemetry" | head -1)+ [ -n "$BAD" ] && break+ fi+ grep -q "$LAUNCH_OK_RE" "$LAUNCH_LOG" 2>/dev/null && OK_LAUNCH=1+ [ "$OK_BASE$OK_LIDAR$OK_ODOM$OK_LAUNCH" = "1111" ] && break+done+if [ -n "$RUN_DIR" ]; then+ grep -a -E "MOSS|RPLIDAR|lidar odom|costmap|Gamepad|Traceback|Error" "$RUN_DIR/main.jsonl" 2>/dev/null | tail -6+fi+if [ -n "$BAD" ] || [ "$OK_BASE$OK_LIDAR$OK_ODOM$OK_LAUNCH" != "1111" ]; then+ WHY=""+ [ "$OK_BASE" = "1" ] || WHY="$WHY base never connected (QT Py port, firmware);"+ if [ "$OK_LIDAR" != "1" ]; then+ if echo "$L" | grep -q "RPLIDAR C1 unavailable"; then+ WHY="$WHY lidar unavailable, never scanning (its 'RPLIDAR C1 unavailable' line in tools/moss-fly.sh log says why);"+ elif echo "$L" | grep -q "RPLIDAR C1 up"; then+ WHY="$WHY lidar answers but never sent a scan (motor, power, cable);"+ else+ WHY="$WHY lidar never up (CP2102N port, lidar power);"+ fi+ fi+ [ "$OK_ODOM" = "1" ] || WHY="$WHY no lidar odometry (kiss_icp);"+ [ "$OK_LAUNCH" = "1" ] || WHY="$WHY the dimOS launcher never said '$LAUNCH_OK_RE';"+ [ -z "$RUN_DIR" ] && WHY="$WHY no new run dir (registry $RUNS_DIR, logs $LOG_ROOT);"+ [ -n "$BAD" ] && WHY="$WHY saw: $(echo "$BAD" | cut -c1-160)"+ echo "STACK NOT PROVEN in ${PROOF_S} s -$WHY - see $LAUNCH_LOG and tools/moss-fly.sh log; stopping it"+ tail -5 "$LAUNCH_LOG"+ stack_down >/dev/null 2>&1+ exit 1+fi+echo " proven: base up, lidar scanning, lidar odometry up, build done, in $(basename "$RUN_DIR")"++echo "== 3/$N cockpit =="+cockpit_up || {+ if [ -n "$FOREIGN_COCKPIT" ]; then+ echo "stopping the stack: a run watched by another checkout's cockpit does not fly"+ else+ echo "NO COCKPIT on :$COCKPIT_PORT (see $COCKPIT_LOG) - a run nobody can watch does not fly: stopping the stack"+ fi+ stack_down >/dev/null 2>&1+ exit 1+}+echo " answering: $(cockpit_url)"++if [ "${EXPLORE:-0}" = "1" ]; then+ echo "== 4/$N exploration =="+ # count before publishing: only a NEW line proves this start was heard+ EXPLORE_RE="costmap: explore_cmd received|Received exploration start command|Started autonomous frontier exploration"+ BEFORE=$(grep -a -c -E "$EXPLORE_RE" "$RUN_DIR/main.jsonl" 2>/dev/null)+ BEFORE=${BEFORE:-0}+ $PY tools/explore_ctl.py start+ OK_EXP=0+ for _ in $(seq 1 "$EXPLORE_PROOF_S"); do+ sleep 1+ NOW=$(grep -a -c -E "$EXPLORE_RE" "$RUN_DIR/main.jsonl" 2>/dev/null)+ [ "${NOW:-0}" -gt "$BEFORE" ] && { OK_EXP=1; break; }+ done+ if [ "$OK_EXP" != "1" ]; then+ echo "EXPLORATION NOT PROVEN (no 'explore_cmd received' in ${EXPLORE_PROOF_S} s on $TRANSPORT) - stopping exploration and the stack"+ $PY tools/explore_ctl.py stop 2>&1 | tail -1+ stack_down >/dev/null 2>&1+ exit 1+ fi+ echo " exploration armed: the explorer heard explore_cmd"+fi++echo "== READY: moss-dimos.$BP =="+if [ "$BP" = "moss-nav" ]; then+ echo " pad: hold R1 (deadman), left stick forward/back, right stick turn - the map builds as you drive"+elif [ "${EXPLORE:-0}" != "1" ]; then+ echo " exploration NOT armed: the cockpit's 'Start exploration', or a goal click on its map"+fi+# MOSS_RECORD read as the stack reads it: 0 / false / no / off = no recorder+case "$(printf '%s' "$MOSS_RECORD" | tr -d '[:space:]' | tr '[:upper:]' '[:lower:]')" in+ 0|false|no|off) echo " not recording (MOSS_RECORD=$MOSS_RECORD)" ;;+ *) echo " recording into $MOSS_STATE_DIR/recordings/" ;;+esac+echo " cockpit: $(cockpit_url)"+echo " stop: tools/moss-fly.sh stop"diff --git a/tools/moss-teleop.sh b/tools/moss-teleop.shnew file mode 100755index 0000000..89bfce9--- /dev/null+++ b/tools/moss-teleop.sh@@ -0,0 +1,127 @@+#!/usr/bin/env bash+# MOSS at the gamepad: gates -> stack -> "you can drive". Runs ON THE ROVER+# (the onboard computer the pad receiver and the QT Py are plugged into), from+# this checkout, with its .venv:+#+# tools/moss-teleop.sh gates, then the stack (pad -> tracks)+# MOCK=1 tools/moss-teleop.sh same stack on the fake firmware: no motor+# tools/moss-teleop.sh stop stack down (the firmware's own 300 ms+# deadman has already stopped the tracks)+# tools/moss-teleop.sh log what the base and the pad said+#+# From another machine: ssh <user>@<rover> 'cd ~/moss && tools/moss-teleop.sh'+#+# Every gate REFUSES rather than guesses: a stack already up,+# no pad, no board, a port held by a bench, a mute firmware, a battery at the+# BMS floor - each is a refusal with the fix on the same line. Nothing moves+# until the deadman is held AND a stick leaves the deadzone; the stack can+# stay up with the pad on the table.+set -u+cd "$(dirname "$0")/.." || exit 1+DIMOS=.venv/bin/dimos+PY=.venv/bin/python+LAUNCH_LOG=/tmp/moss_teleop_launch.log+export GLIBC_TUNABLES=glibc.rtld.optional_static_tls=4194304 # same as tools/moss-fly.sh (static TLS reserve)+# same bus as tools/moss-fly.sh, under both names (dimOS reads DIMOS_TRANSPORT first)+BUS="${DIMOS_TRANSPORT:-${TRANSPORT:-lcm}}"+export DIMOS_TRANSPORT="$BUS" TRANSPORT="$BUS"++case "${1:-}" in+ stop)+ $DIMOS stop 2>&1 | tail -2+ sleep 1+ pgrep -f "[b]in/dimos( [^ ]+)* run( |$)" >/dev/null && { echo "dimos still up - killing"; pkill -f "[b]in/dimos( [^ ]+)* run( |$)"; }+ echo "stack down - the tracks stopped on the firmware deadman (300 ms) before this line"+ exit 0 ;;+ log)+ $DIMOS log 2>/dev/null | grep -a "MOSS\|[Gg]amepad\|Traceback\|Error" | tail -${2:-30}+ exit 0 ;;+ "") ;;+ *) echo "usage: tools/moss-teleop.sh [stop|log]"; exit 1 ;;+esac++echo "== 0/5 no dimOS stack already up =="+# the real launch line is 'bin/dimos --rerun-open none run ...': root options sit between+# 'dimos' and 'run', so the pattern must allow them (a plain 'bin/dimos run' never matches)+if pgrep -f "[b]in/dimos( [^ ]+)* run( |$)" >/dev/null; then+ echo "A DIMOS STACK IS ALREADY RUNNING - tools/moss-teleop.sh stop first"; exit 1+fi+echo " none"++echo "== 1/5 gamepad =="+if [ ! -e /dev/input/js0 ]; then+ echo "NO PAD (no /dev/input/js0) - receiver plugged into the rover? if plugged but invisible: replug it, else sudo reboot"; exit 1+fi+echo " js0 present (if the pad is mute: button UNDER the pad, then move a stick)"++echo "== 2/5 MOSS board =="+PORT=$(ls /dev/serial/by-id/*QT_Py* 2>/dev/null | head -1)+if [ "${MOCK:-0}" = "1" ]; then+ echo " MOCK=1: fake firmware, the real board is not touched"+else+ [ -n "$PORT" ] || { echo "NO QT PY on USB (/dev/serial/by-id/*QT_Py*) - cable, power, or the board is in bootloader"; exit 1; }+ echo " $PORT"+ HOLDER=$(fuser "$PORT" 2>/dev/null)+ [ -z "$HOLDER" ] || { echo "PORT HELD by pid $HOLDER (a bench or watcher) - stop it first"; exit 1; }+fi++echo "== 3/5 firmware answers =="+if [ "${MOCK:-0}" != "1" ]; then+ $PY - "$PORT" <<'EOF' || exit 1+import json, sys, time, serial+port = sys.argv[1]+try:+ s = serial.Serial(port, 115200, timeout=0.05)+except Exception as exc:+ print(f"CANNOT OPEN {port}: {exc!r} - dialout group? another process?"); sys.exit(1)+time.sleep(0.3); s.reset_input_buffer()+t0 = time.time(); last = None; buf = b""+while time.time() - t0 < 1.5:+ buf += s.read(4096)+ while b"\n" in buf:+ line, buf = buf.split(b"\n", 1)+ try:+ last = json.loads(line)+ except Exception:+ pass+s.close()+if not last:+ print("FIRMWARE MUTE (no telemetry in 1.5 s) - power-cycle the board, or reflash it: firmware/MOSS_QTPY_MDD3A/bench/flash_qtpy.sh"+ " (esptool from the 'firmware' extra, and a compiled build: see that folder's README.md)"); sys.exit(1)+mode, reason, mv = last.get("mode"), last.get("reason"), last.get("bus_mV")+print(f" firmware {last.get('fw')} mode={mode} ({reason}) battery={mv} mV ina_ok={last.get('ina_ok')}")+if mode not in (0,):+ print(f"FIRMWARE NOT DISARMED (mode {mode}) - something else is driving it"); sys.exit(1)+if last.get("ina_ok") and mv is not None and int(mv) < 10800:+ print(f"BATTERY {mv} mV - under 10.8 V the BMS cuts at 10.2 V mid-drive: recharge first"); sys.exit(1)+EOF+else+ echo " (mock)"+fi++echo "== 4/5 stack =="+: > "$LAUNCH_LOG"+if [ "${MOCK:-0}" = "1" ]; then export MOSS_MOCK_BUS=1; else export MOSS_PORT="$PORT"; fi+export MOSS_MAX_PCT="${MOSS_MAX_PCT:-100}" # firmware limit 100; lower it here for a cautious lap+export MOSS_RAMP_MS="${MOSS_RAMP_MS:-5 5}" # firmware ramp, ms per % up/down (the pad slew still takes 0.75 s to full)+nohup $DIMOS --rerun-open none run moss-dimos.moss-gamepad --daemon > "$LAUNCH_LOG" 2>&1 < /dev/null &+OK_BASE=0; OK_PAD=0+for i in $(seq 1 40); do+ sleep 1+ L=$($DIMOS log 2>/dev/null | grep -a "MOSS base: connected\|MOSS base: MOCK BUS\|Gamepad connected\|MOSS base: cannot open\|MOSS base: no telemetry\|Traceback")+ echo "$L" | grep -q "MOSS base: connected\|MOSS base: MOCK BUS" && OK_BASE=1+ echo "$L" | grep -q "Gamepad connected" && OK_PAD=1+ echo "$L" | grep -q "cannot open\|no telemetry\|Traceback" && break+ [ $OK_BASE = 1 ] && [ $OK_PAD = 1 ] && break+done+$DIMOS log 2>/dev/null | grep -a "MOSS\|[Gg]amepad\|Traceback\|Error" | tail -8+if [ $OK_BASE != 1 ] || [ $OK_PAD != 1 ]; then+ echo "STACK NOT READY (base=$OK_BASE pad=$OK_PAD) - see above and $LAUNCH_LOG; stopping"+ tail -5 "$LAUNCH_LOG"; $DIMOS stop >/dev/null 2>&1; exit 1+fi++echo "== 5/5 YOU CAN DRIVE =="+echo " deadman = R1 (right shoulder, SDL button 7) held the whole time"+echo " left stick up/down = forward/back right stick left/right = turn R2 = boost (capped at ${MOSS_MAX_PCT} %)"+echo " release R1 = 0.5 s of zeros then silence; the firmware stops 300 ms after the last drive"+echo " stop: tools/moss-teleop.sh stop watch: tools/moss-teleop.sh log"diff --git a/tools/moss_cockpit.py b/tools/moss_cockpit.pynew file mode 100644index 0000000..0ccf620--- /dev/null+++ b/tools/moss_cockpit.py@@ -0,0 +1,1930 @@+#!/usr/bin/env python3+"""MOSS cockpit: one small web server on the rover, one page for a whole run.++ http://<rover>:8904/ the page (tools/moss_cockpit/index.html)+ .venv/bin/python tools/moss_cockpit.py live: listens to the stack's LCM bus+ python3 tools/moss_cockpit.py --demo --port 8905 synthetic rover, no bus, no dimOS++Why a separate process instead of a dimOS web module: dimOS's own viewers either+need a secure context, git LFS (the command center) or cost a core with no+client connected (inflate + distance transform on every costmap). This server+imports lcm, dimos_lcm and numpy only - never dimos, torch or open3d - and does+nothing until a page asks: LCM callbacks keep the latest bytes and a timestamp,+decoding happens on request and is cached by that timestamp (map at most 2 Hz,+scan at most 4 Hz). A camera channel is subscribed only while an MJPEG client+is watching it.++What it touches: the LCM bus (explicit channels, listen; publish only on a POST+/cmd), ~/.local/state/dimos/runs/*.json, the running stack's main.jsonl and+/proc/<stack pid>/environ (read: the bus the stack chose), $MOSS_STATE_DIR+(default ~/.local/state/moss: preflight.json and the recording, read;+last_launch.log, written by the launcher it starts). It NEVER+opens a serial port: battery and arming state come from /base_telemetry, which+the stack's coordinator publishes.++Endpoints (all Cache-Control: no-store; a failing request never kills the server):+ GET / /static/<file> the page and its assets (tools/moss_cockpit/)+ GET /state.json stack, pose, topic rates, base, nav, recording, system+ GET /map.png /map_meta latest /global_costmap as a PNG + the pixel <-> metre contract+ GET /scan.json latest lidar points, world frame (fallback: lidar frame)+ GET /path.json planner path+ GET /log.json?n=40 tail of the running stack's main.jsonl+ 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+ GET /camera.mjpg?cam=wrist the arm's camera, same framing (cam=nose is the default)+ POST /cmd/explore|goal|stop|episode|stack publish on the bus / run tools/moss-fly.sh+ (header 'X-Requested-With: moss-cockpit' required, else 403;+ explore and goal only on a moss-* stack, else 409;+ bus commands only on an LCM run, else 503: the stack's own+ DIMOS_TRANSPORT, then TRANSPORT, as dimOS reads them)++Standard library + numpy (+ lcm and dimos_lcm in live mode). No build step,+no CDN: the field has no internet.+"""++from __future__ import annotations++import argparse+import base64+import datetime as _dt+import glob+import json+import math+import os+import re+import select+import signal+import socket+import struct+import subprocess+import sys+import threading+import time+import zlib+from collections import deque+from http.server import BaseHTTPRequestHandler, ThreadingHTTPServer+from urllib.parse import parse_qs, unquote, urlsplit++import numpy as np++TOOLS_DIR = os.path.dirname(os.path.abspath(__file__))+REPO_DIR = os.path.dirname(TOOLS_DIR)+if TOOLS_DIR not in sys.path:+ sys.path.insert(0, TOOLS_DIR) # explore_ctl, the shared out-of-stack publisher++try:+ import lcm # the bus; absent on a laptop: live mode says so, --demo does not need it+except ImportError:+ lcm = None++try:+ from dimos_lcm.geometry_msgs import Point as LcmPoint+ from dimos_lcm.geometry_msgs import Pose as LcmPose+ from dimos_lcm.geometry_msgs import PoseStamped as LcmPoseStamped+ from dimos_lcm.geometry_msgs import Quaternion as LcmQuaternion+ from dimos_lcm.geometry_msgs import Twist as LcmTwist+ from dimos_lcm.geometry_msgs import Vector3 as LcmVector3+ from dimos_lcm.nav_msgs import OccupancyGrid as LcmOccupancyGrid+ from dimos_lcm.nav_msgs import Path as LcmPath+ from dimos_lcm.sensor_msgs import Image as LcmImage+ from dimos_lcm.sensor_msgs import PointCloud2 as LcmPointCloud2+ from dimos_lcm.std_msgs import Bool as LcmBool+ from dimos_lcm.std_msgs import Header as LcmHeader+ from dimos_lcm.std_msgs import String as LcmString+ from dimos_lcm.std_msgs import Time as LcmTime+ HAVE_MSGS = True+except Exception: # noqa: BLE001 - pure-python codecs, but not installed everywhere+ HAVE_MSGS = False++DEFAULT_PORT = 8904+DEFAULT_BIND = "0.0.0.0"+PAGE_DIR = os.path.join(TOOLS_DIR, "moss_cockpit")+FLY_SCRIPT = os.path.join(TOOLS_DIR, "moss-fly.sh")++# LCM channels as the stack names them: "/<stream>#<dimOS msg_name>". Spelled+# out instead of derived from dimos_lcm classes, whose msg_name is not always+# the one dimOS uses (dimos_lcm's PoseStamped says just "PoseStamped").+CHANNELS = {+ "global_costmap": "/global_costmap#nav_msgs.OccupancyGrid",+ "odom": "/odom#geometry_msgs.PoseStamped",+ "lidar": "/lidar#sensor_msgs.PointCloud2", # world frame, from LidarOdometry+ "pointcloud": "/pointcloud#sensor_msgs.PointCloud2", # lidar frame, straight from the C1 driver+ "path": "/path#nav_msgs.Path",+ "goal_request": "/goal_request#geometry_msgs.PoseStamped",+ "goal_reached": "/goal_reached#std_msgs.Bool",+ "explore_done": "/explore_done#std_msgs.Bool",+ "explore_cmd": "/explore_cmd#std_msgs.Bool",+ "stop_explore_cmd": "/stop_explore_cmd#std_msgs.Bool",+ "stop_movement": "/stop_movement#std_msgs.Bool", # MovementManager on a pad touch: ends exploration+ "cmd_vel": "/cmd_vel#geometry_msgs.Twist",+ "base_telemetry": "/base_telemetry#std_msgs.String",+ "episode_marker": "/episode_marker#std_msgs.String",+}+CAMERAS = { # cockpit name -> (bus topic, LCM channel); subscribed only while watched+ "nose": ("color_image", "/color_image#sensor_msgs.Image"),+ "wrist": ("wrist_image", "/wrist_image#sensor_msgs.Image"),+}+IMAGE_NAME, IMAGE_CHANNEL = CAMERAS["nose"]+STATE_TOPICS = ("odom", "global_costmap", "lidar", "pointcloud", "path", "cmd_vel",+ "base_telemetry") + tuple(name for name, _ in CAMERAS.values())++MAP_MIN_INTERVAL_S = 0.5 # decode + PNG the costmap at most 2 Hz, whoever asks+SCAN_MIN_INTERVAL_S = 0.25 # scan at most 4 Hz+SCAN_FALLBACK_S = 5.0 # /lidar silent this long -> show /pointcloud (lidar frame)+MAX_SCAN_POINTS = 1500+HZ_WINDOW_S = 5.0+MJPEG_PERIOD_S = 0.2 # 5 fps: enough to see where the nose points, cheap on a small board+MAX_CAMERA_CLIENTS = 4+FIRST_FRAME_WAIT_S = 2.0+LCM_HANDLE_MS = 250 # the listener loop also (un)subscribes the camera between handles+SYSTEM_CACHE_S = 1.0+MAX_BODY = 65536+GOAL_LIMIT_M = 500.0 # MovementManager ignores clicks beyond 500 m: refuse them here, loudly+LAUNCH_TAIL = 60+LAUNCH_KILL_GRACE_S = 3.0 # 'stop' during a launch: SIGTERM its process group, SIGKILL after this+LAUNCH_ENV_DROP = ("EXPLORE", "MOCK", "MOSS_MOCK_BUS") # per-run switches of a CLI run: never inherited+GOAL_AFTER_STOP_S = 3.0 # the explorer's own 'stay here' goal lands ~2 s after its stop: ours goes after+MJPEG_WRITE_TIMEOUT_S = 5.0 # a viewer that vanished without FIN/RST frees its slot after this+CMD_HEADER = ("X-Requested-With", "moss-cockpit") # every POST /cmd/* carries it: a cross-site form cannot++# 3S pack: 12.6 V full, the BMS cuts at 10.2 V (linear, clamped; a voltage+# gauge, not a coulomb counter).+BATTERY_EMPTY_V = 10.2+BATTERY_FULL_V = 12.6+MODE_NAMES = {0: "off", 1: "ready", 2: "drive", 3: "test"}++# Map colours, UI-only (the brand has no occupancy palette): unknown = STUDIO,+# free = a darker studio, cost 1..99 ramps from free to a light slate, lethal =+# brand WHITE, so obstacles read first on the dark page.+COLOR_UNKNOWN = (0x3D, 0x57, 0x6A)+COLOR_FREE = (0x2E, 0x45, 0x56)+COLOR_COST_MAX = (0x8F, 0xA6, 0xB5)+COLOR_OCCUPIED = (0xEA, 0xF0, 0xEF)++_ISO = "%Y-%m-%dT%H:%M:%S"+++def state_dir() -> str:+ """MOSS's own state dir (recordings, preflight.json, last_launch.log)."""+ return os.path.expanduser(os.environ.get("MOSS_STATE_DIR") or "~/.local/state/moss")+++def runs_dir() -> str:+ """dimOS's run registry, resolved the way dimos.constants does on Linux."""+ base = os.environ.get("XDG_STATE_HOME") or os.path.expanduser("~/.local/state")+ return os.path.join(base, "dimos", "runs")+++# --- pictures ----------------------------------------------------------------++def png_bytes(rgb: np.ndarray, level: int = 3) -> bytes:+ """An (h, w, 3) uint8 array as a PNG, with zlib and struct only (level 3 instead+ of 6: ~2x faster on a 480x480 costmap, a few % bigger)."""+ h, w, _ = rgb.shape+ rows = np.zeros((h, w * 3 + 1), dtype=np.uint8)+ rows[:, 1:] = rgb.reshape(h, w * 3) # filter byte 0 = None, per row+ raw = zlib.compress(rows.tobytes(), level)++ def chunk(tag: bytes, data: bytes) -> bytes:+ return (struct.pack(">I", len(data)) + tag + data+ + struct.pack(">I", zlib.crc32(tag + data) & 0xFFFFFFFF))++ return (b"\x89PNG\r\n\x1a\n"+ + chunk(b"IHDR", struct.pack(">IIBBBBB", w, h, 8, 2, 0, 0, 0))+ + chunk(b"IDAT", raw)+ + chunk(b"IEND", b""))+++def _build_lut() -> np.ndarray:+ """256-entry colour table indexed by the int8 cell value seen as uint8."""+ lut = np.empty((256, 3), dtype=np.uint8)+ lut[:] = COLOR_UNKNOWN # -128..-1 (as 128..255): unknown+ lut[0] = COLOR_FREE+ free = np.array(COLOR_FREE, dtype=float)+ top = np.array(COLOR_COST_MAX, dtype=float)+ for v in range(1, 100):+ lut[v] = np.rint(free + (top - free) * (v / 99.0)).astype(np.uint8)+ lut[100:128] = COLOR_OCCUPIED # 100 lethal (and anything above)+ return lut+++MAP_LUT = _build_lut()+++def map_meta(width: int, height: int, res: float, ox: float, oy: float,+ stamp: float, frame: str) -> dict:+ """The /map_meta contract: extent and pixel <-> metre formulas, plus the costmap stamp and frame."""+ return {+ "res": res, "ox": ox, "oy": oy,+ "width": int(width), "height": int(height),+ "x_min": ox, "x_max": ox + width * res,+ "y_min": oy, "y_max": oy + height * res,+ "stamp": stamp, "frame": frame,+ # px -> metres and back. Row 0 of the PNG is the highest y.+ "x_of_px": "x = ox + (px + 0.5) * res",+ "y_of_py": "y = oy + (height - py - 0.5) * res",+ "px_of_x": "px = (x - ox) / res",+ "py_of_y": "py = height - (y - oy) / res",+ }+++def render_grid(cells: np.ndarray, res: float, ox: float, oy: float,+ stamp: float = 0.0, frame: str = "world") -> tuple[bytes, dict]:+ """An OccupancyGrid's cells (rows = y from min y up, int8 -1/0/1..99/100) as (PNG, meta).+ Flipped so PNG row 0 is the highest y."""+ grid = np.ascontiguousarray(cells, dtype=np.int8)+ h, w = grid.shape+ rgb = MAP_LUT[grid.view(np.uint8)]+ return png_bytes(np.flipud(rgb)), map_meta(w, h, float(res), float(ox), float(oy), stamp, frame)+++# --- message decoding (bytes off the bus -> plain values) -----------------------++def _stamp(header) -> float:+ return float(header.stamp.sec) + float(header.stamp.nsec) * 1e-9+++def yaw_of(qx: float, qy: float, qz: float, qw: float) -> float:+ """Heading (rad) of a quaternion, z-up."""+ return math.atan2(2.0 * (qw * qz + qx * qy), 1.0 - 2.0 * (qy * qy + qz * qz))+++def decode_costmap(raw: bytes) -> tuple[bytes, dict]:+ m = LcmOccupancyGrid.lcm_decode(raw)+ w, h, n = int(m.info.width), int(m.info.height), int(m.data_length)+ # the int8 cells are the message's last field: view them straight off the+ # raw bytes instead of rebuilding a 230k-element tuple+ cells = np.frombuffer(raw, dtype=np.int8, count=n, offset=len(raw) - n)+ if n != w * h:+ raise ValueError(f"costmap {w}x{h} carries {n} cells")+ o = m.info.origin.position+ return render_grid(cells.reshape(h, w), float(m.info.resolution), float(o.x), float(o.y),+ _stamp(m.header), m.header.frame_id or "world")+++def decode_pose(raw: bytes) -> dict:+ m = LcmPoseStamped.lcm_decode(raw)+ p, q = m.pose.position, m.pose.orientation+ return {"x": float(p.x), "y": float(p.y), "z": float(p.z),+ "yaw": yaw_of(q.x, q.y, q.z, q.w), "stamp": _stamp(m.header),+ "frame": m.header.frame_id}+++def cloud_xy(raw: bytes) -> tuple[str, float, np.ndarray]:+ """(frame, stamp, Nx2 float32 x/y) of a PointCloud2, NaN/inf rows dropped.+ Field offsets read from the message (dimOS writes x,y,z float32 at 0/4/8 with a+ 16-byte step, more when per-point extras are attached)."""+ m = LcmPointCloud2.lcm_decode(raw)+ n = int(m.width) * int(m.height)+ step = int(m.point_step)+ offs = {f.name: int(f.offset) for f in m.fields}+ if n == 0 or step < 8:+ return m.header.frame_id, _stamp(m.header), np.zeros((0, 2), np.float32)+ end = ">" if m.is_bigendian else "<"+ dt = np.dtype({"names": ["x", "y"], "formats": [end + "f4", end + "f4"],+ "offsets": [offs.get("x", 0), offs.get("y", 4)], "itemsize": step})+ data = m.data if isinstance(m.data, (bytes, bytearray)) else bytes(m.data)+ arr = np.frombuffer(data, dtype=dt, count=min(n, len(data) // step))+ xy = np.column_stack((arr["x"], arr["y"])).astype(np.float32)+ return m.header.frame_id, _stamp(m.header), xy[np.isfinite(xy).all(axis=1)]+++def downsample(points: np.ndarray, limit: int = MAX_SCAN_POINTS) -> np.ndarray:+ """At most `limit` rows, taken evenly across the array (keeps the angular spread)."""+ if len(points) <= limit:+ return points+ return points[np.linspace(0, len(points) - 1, limit).astype(int)]+++def decode_path(raw: bytes) -> list[list[float]]:+ m = LcmPath.lcm_decode(raw)+ return [[round(float(p.pose.position.x), 3), round(float(p.pose.position.y), 3)] for p in m.poses]+++def decode_twist(raw: bytes) -> tuple[float, float]:+ m = LcmTwist.lcm_decode(raw)+ return float(m.linear.x), float(m.angular.z)+++def decode_bool(raw: bytes) -> bool:+ return bool(LcmBool.lcm_decode(raw).data)+++def decode_string(raw: bytes) -> str:+ return str(LcmString.lcm_decode(raw).data)+++# --- messages to publish ----------------------------------------------------------+# Every nested field is built fresh: the generated dimos_lcm constructors take+# their defaults as shared instances (`pose=Pose()` evaluated once), so mutating+# a default-built message would silently change every later one in the process.++def goal_message(x: float, y: float, now: float | None = None):+ """A navigation goal the way dimOS's own web module sends a click: frame world,+ z 0, identity orientation, stamped now."""+ now = time.time() if now is None else now+ header = LcmHeader(seq=0, stamp=LcmTime(sec=int(now), nsec=int((now % 1.0) * 1e9)), frame_id="world")+ pose = LcmPose(position=LcmPoint(x=float(x), y=float(y), z=0.0),+ orientation=LcmQuaternion(x=0.0, y=0.0, z=0.0, w=1.0))+ return LcmPoseStamped(header=header, pose=pose)+++def zero_twist():+ return LcmTwist(linear=LcmVector3(x=0.0, y=0.0, z=0.0), angular=LcmVector3(x=0.0, y=0.0, z=0.0))+++# --- the base, from its telemetry JSON ------------------------------------------++def battery_pct(volts: float | None) -> int | None:+ """3S linear gauge: 10.2 V -> 0 %, 12.6 V -> 100 %, clamped."""+ if volts is None:+ return None+ frac = (volts - BATTERY_EMPTY_V) / (BATTERY_FULL_V - BATTERY_EMPTY_V)+ return int(round(100.0 * min(1.0, max(0.0, frac))))+++def base_view(t: dict, age_s: float | None) -> dict:+ """The /base_telemetry JSON (MossControlCoordinator, every 0.5 s) as the page shows it."""+ mv = t.get("bus_mV")+ volts = round(mv / 1000.0, 2) if isinstance(mv, (int, float)) else None+ mode = t.get("mode")+ return {+ "fw": t.get("fw"), "driver": t.get("driver"),+ "mode": mode, "mode_name": MODE_NAMES.get(mode, "unknown") if mode is not None else None,+ "reason": t.get("reason"),+ "bus_mV": mv, "battery_v": volts,+ "battery_pct": battery_pct(volts) if t.get("ina_ok", True) else None,+ "ina_ok": t.get("ina_ok"),+ "ticks": t.get("ticks"), "target_pct": t.get("target_pct"), "output_pct": t.get("output_pct"),+ "ramp_ms": t.get("ramp_ms"),+ "firmware_age_s": t.get("firmware_age_s"), "mock": t.get("mock"),+ "age_s": None if age_s is None else round(age_s, 2),+ }+++# --- the machine (stdlib, sysfs, /proc) ---------------------------------------------++_prev_cpu = None+_prev_cores = None+_gpu_samples: list = []+_sys_lock = threading.Lock()+_sys_cache = [0.0, None]+GPU_LOAD_PATHS = ("/sys/devices/platform/bus@0/17000000.gpu/load", "/sys/devices/gpu.0/load")+++def _read(path):+ try:+ with open(path) as f:+ return f.read().strip()+ except Exception: # noqa: BLE001 - sysfs raises odd errors on absent sensors+ return None+++def cpu_percent():+ """CPU usage since the previous call (first call: 0.2 s sample)."""+ global _prev_cpu++ def sample():+ vals = [int(x) for x in _read("/proc/stat").splitlines()[0].split()[1:]]+ return vals[3] + (vals[4] if len(vals) > 4 else 0), sum(vals)++ if _prev_cpu is None:+ _prev_cpu = sample()+ time.sleep(0.2)+ idle0, total0 = _prev_cpu+ idle1, total1 = sample()+ _prev_cpu = (idle1, total1)+ dt = total1 - total0+ return round(100.0 * (1 - (idle1 - idle0) / dt), 1) if dt > 0 else 0.0+++def cpu_per_core():+ """Per-core usage since the previous call: one saturated worker reads differently+ from a spread load."""+ global _prev_cores++ def sample():+ out = []+ for line in _read("/proc/stat").splitlines()[1:]:+ if not line.startswith("cpu"):+ break+ vals = [int(x) for x in line.split()[1:]]+ out.append((vals[3] + (vals[4] if len(vals) > 4 else 0), sum(vals)))+ return out++ if _prev_cores is None:+ _prev_cores = sample()+ time.sleep(0.2)+ prev, cur = _prev_cores, sample()+ _prev_cores = cur+ res = []+ for (i0, t0), (i1, t1) in zip(prev, cur):+ dt = t1 - t0+ res.append(round(100.0 * (1 - (i1 - i0) / dt), 1) if dt > 0 else 0.0)+ return res+++def meminfo():+ info = {}+ for line in _read("/proc/meminfo").splitlines():+ k, v = line.split(":", 1)+ info[k] = int(v.strip().split()[0]) # kB+ total = info["MemTotal"]+ used = total - info.get("MemAvailable", info.get("MemFree", 0))+ return {"ram_used_gb": round(used / 1024**2, 1), "ram_total_gb": round(total / 1024**2, 1),+ "ram_percent": round(100.0 * used / total, 1)}+++def _gpu_read_once():+ """Jetson Orin GPU load (sysfs gives tenths of a percent); None on a board without that file."""+ for path in GPU_LOAD_PATHS:+ raw = _read(path)+ if raw is not None:+ try:+ return int(raw) / 10.0+ except ValueError:+ pass+ return None+++def _gpu_sampler():+ """10 Hz, 3 s average: the mapper works in 4 ms CUDA bursts that a single+ read misses (10 Hz is plenty for a page that polls at 1 Hz)."""+ while True:+ v = _gpu_read_once()+ now = time.time()+ if v is not None:+ _gpu_samples.append((now, v))+ while _gpu_samples and now - _gpu_samples[0][0] > 3.0:+ _gpu_samples.pop(0)+ time.sleep(0.1)+++def gpu_percent():+ if not _gpu_samples:+ return _gpu_read_once()+ return round(sum(v for _, v in _gpu_samples) / len(_gpu_samples), 1)+++def temps():+ zones = {}+ for z in glob.glob("/sys/class/thermal/thermal_zone*"):+ name = _read(os.path.join(z, "type")) or "?"+ raw = _read(os.path.join(z, "temp"))+ if raw is None:+ continue+ try:+ t = int(raw) / 1000.0+ except ValueError:+ continue+ if t <= 0:+ continue+ zones[name.replace("-thermal", "").replace("_thermal", "")] = round(t, 1)+ return zones+++def disks():+ seen, out = set(), []+ for line in (_read("/proc/mounts") or "").splitlines():+ dev, mnt, fs = line.split()[:3]+ if fs not in ("ext4", "vfat", "xfs", "btrfs", "f2fs") or mnt in seen or mnt.startswith("/boot"):+ continue+ seen.add(mnt)+ try:+ st = os.statvfs(mnt)+ except OSError:+ continue+ total = st.f_blocks * st.f_frsize+ free = st.f_bavail * st.f_frsize+ if total == 0:+ continue+ out.append({"mountpoint": mnt, "fstype": fs,+ "used_gb": round((total - free) / 1024**3, 1), "total_gb": round(total / 1024**3, 1),+ "free_gb": round(free / 1024**3, 1), "percent": round(100.0 * (total - free) / total, 1)})+ return sorted(out, key=lambda d: d["mountpoint"])+++def system_metrics() -> dict:+ """The machine, cached 1 s: several pages polling at 1 Hz cost one /proc pass."""+ with _sys_lock:+ stamp, cached = _sys_cache+ if cached is not None and time.monotonic() - stamp < SYSTEM_CACHE_S:+ return cached+ out = {"cpu_percent": cpu_percent(), "cpu_per_core": cpu_per_core(), "gpu_percent": gpu_percent()}+ try:+ out.update(meminfo())+ except Exception: # noqa: BLE001+ pass+ out["temps"] = temps()+ out["disks"] = disks()+ out["load_1m"] = round(os.getloadavg()[0], 2)+ out["uptime_s"] = int(float((_read("/proc/uptime") or "0 0").split()[0]))+ _sys_cache[:] = [time.monotonic(), out]+ return out+++# --- the stack, without asking dimOS -----------------------------------------------++def _pid_alive(pid: int) -> bool:+ try:+ os.kill(pid, 0)+ return True+ except ProcessLookupError:+ return False+ except PermissionError:+ return True # exists, not ours to signal+ except (OSError, TypeError, ValueError):+ return False+++def _parse_iso(text: str) -> float | None:+ try:+ return _dt.datetime.fromisoformat(str(text).replace("Z", "+00:00")).timestamp()+ except (TypeError, ValueError):+ return None+++def stack_status(now: float | None = None) -> dict:+ """The live run from dimOS's registry (dead pids skipped, never `dimos status`:+ every dimOS CLI call writes a log file and it would also prune the registry)."""+ now = time.time() if now is None else now+ best = None+ for path in glob.glob(os.path.join(runs_dir(), "*.json")):+ try:+ with open(path) as fh:+ entry = json.load(fh)+ pid = int(entry.get("pid"))+ except Exception: # noqa: BLE001 - half-written or foreign file+ continue+ if not _pid_alive(pid):+ continue+ started = _parse_iso(entry.get("started_at"))+ if best is None or (started or 0) > (best[0] or 0):+ best = (started, pid, entry)+ if best is None:+ return {"running": False}+ started, pid, entry = best+ blueprint = str(entry.get("blueprint", ""))+ return {"running": True, "run_id": entry.get("run_id"),+ "blueprint": blueprint.rsplit(".", 1)[-1], "pid": pid,+ "started_at": entry.get("started_at"),+ "uptime_s": round(now - started, 1) if started else None,+ "log_dir": entry.get("log_dir")}+++def _dimos_run_pid() -> int | None:+ """A `dimos run` process even without a registry entry (a foreground run, a+ registry swept by someone's `dimos status`). TOCTOU-safe: a process that exits+ between the listing and the read is skipped."""+ try:+ pids = os.listdir("/proc")+ except OSError:+ return None+ for pid in pids:+ if not pid.isdigit():+ continue+ try:+ with open(f"/proc/{pid}/cmdline", "rb") as fh:+ argv = fh.read().split(b"\0")+ except OSError:+ continue+ if any(a.endswith(b"bin/dimos") for a in argv) and b"run" in argv:+ return int(pid)+ return None+++def proc_environ(pid) -> dict | None:+ """The environment process `pid` started with (/proc/<pid>/environ), or None+ when it cannot be read (gone, another user's, no pid)."""+ try:+ with open(f"/proc/{int(pid)}/environ", "rb") as fh:+ raw = fh.read()+ except (OSError, TypeError, ValueError):+ return None+ env = {}+ for item in raw.split(b"\0"):+ key, sep, value = item.partition(b"=")+ if sep:+ env[key.decode("utf-8", "replace")] = value.decode("utf-8", "replace")+ return env+++BUS_VARS = ("DIMOS_TRANSPORT", "TRANSPORT") # what dimOS picks its bus from (explore_ctl reads them)+++def bus_env(pid) -> tuple[dict, str]:+ """The variables that chose the bus, and whose they are. A running stack read+ DIMOS_TRANSPORT / TRANSPORT from its own environment when it started, so that+ one decides: a cockpit run by systemd shares nothing with a stack started from+ a shell. With no stack, or one whose environment cannot be read, the cockpit's+ own: what a stack launched from the page inherits."""+ env = proc_environ(pid) if pid else None+ whose = f"the running stack's environment, pid {pid}"+ if env is None:+ env = os.environ+ whose = "the cockpit's own environment" + (", the stack's is unreadable" if pid else "")+ return {k: env[k] for k in BUS_VARS if k in env}, whose+++def log_tail(log_dir: str | None, n: int) -> list[dict]:+ """The last n records of <log_dir>/main.jsonl as {ts, level, event, logger}."""+ if not log_dir:+ return []+ path = os.path.join(log_dir, "main.jsonl")+ try:+ with open(path, "rb") as fh:+ fh.seek(0, os.SEEK_END)+ size = fh.tell()+ fh.seek(max(0, size - 262144))+ chunk = fh.read()+ except OSError:+ return []+ out = []+ for line in reversed(chunk.splitlines()): # newest first, until n records parse+ if len(out) >= n:+ break+ try:+ rec = json.loads(line)+ except ValueError:+ continue # a half-written last line, a stray print+ if isinstance(rec, dict):+ out.append({"ts": rec.get("timestamp"), "level": rec.get("level"),+ "event": rec.get("event"), "logger": rec.get("logger")})+ return out[::-1]+++def preflight_doc() -> dict:+ """The last flight check moss-fly.sh saved (moss_preflight.py --json)."""+ path = os.path.join(state_dir(), "preflight.json")+ try:+ with open(path) as fh:+ doc = json.load(fh)+ except FileNotFoundError:+ return {"available": False}+ except (OSError, ValueError) as exc:+ return {"available": False, "error": f"{path}: {exc}"}+ if not isinstance(doc, dict):+ return {"available": False, "error": f"{path}: not a JSON object"}+ doc["available"] = True+ doc["mtime"] = os.path.getmtime(path)+ return doc+++def recording_view(episode: dict | None, closed: dict | None = None) -> dict:+ """`closed`: the episode the cockpit closed itself because the stack stopped under it."""+ db = os.path.join(state_dir(), "recordings", "moss.db")+ size = 0+ for p in (db, db + "-wal", db + "-shm"):+ try:+ size += os.path.getsize(p)+ except OSError:+ pass+ return {"db": db, "exists": os.path.isfile(db), "size_mb": round(size / 1e6, 1), "episode": episode,+ "closed": closed}+++_EPISODE_NAME = re.compile(r"^[A-Za-z0-9][A-Za-z0-9 ._-]{0,59}$")+++def episode_name(raw) -> str:+ """A safe episode name: letters, digits, space . _ -, 60 max; default ep-HHMMSS."""+ name = str(raw or "").strip()+ if not name:+ return time.strftime("ep-%H%M%S")+ if not _EPISODE_NAME.match(name):+ raise ValueError("episode name: letters, digits, space . _ - only, 60 characters max")+ return name+++def _finite_xy(body: dict) -> tuple[float, float]:+ try:+ x, y = float(body.get("x")), float(body.get("y"))+ except (TypeError, ValueError):+ raise ValueError("goal needs numbers x and y (metres, world frame)") from None+ if not (math.isfinite(x) and math.isfinite(y)):+ raise ValueError("goal x/y must be finite")+ if abs(x) > GOAL_LIMIT_M or abs(y) > GOAL_LIMIT_M:+ raise ValueError(f"goal beyond {GOAL_LIMIT_M:.0f} m: the movement manager would drop it")+ return x, y+++def _later(delay_s: float, fn) -> threading.Timer:+ """Run fn once, delay_s from now, on a daemon thread (the bench swaps this out)."""+ def run():+ try:+ fn()+ except Exception as exc: # noqa: BLE001 - a timer thread has nobody to raise to+ print(f"moss_cockpit: deferred command failed: {exc!r}", file=sys.stderr, flush=True)+ timer = threading.Timer(delay_s, run)+ timer.daemon = True+ timer.start()+ return timer+++# --- the launcher, as a child process -----------------------------------------------++class Launcher:+ """Runs tools/moss-fly.sh <action> without waiting for it; output appended to+ $MOSS_STATE_DIR/last_launch.log (a `stop` may run while a `map` is still in+ its gates, so both write to the same file, each under a header line)."""++ def __init__(self, script: str = FLY_SCRIPT):+ self.script = script+ self._lock = threading.Lock()+ self._procs: list[tuple[str, float, subprocess.Popen]] = []+ self.last: dict = {}++ def log_path(self) -> str:+ return os.path.join(state_dir(), "last_launch.log")++ def _running(self) -> list:+ return [(a, t, p) for a, t, p in self._procs if p.poll() is None]++ def _cancel(self, fh) -> None:+ """Stop pressed during a launch: end every launcher still in its gates first,+ or it would bring the stack up after the operator's stop. Each one leads its+ own process group (start_new_session), so its preflight child goes too."""+ for action, _, proc in self._running():+ if action == "stop":+ continue+ how = "SIGTERM"+ try:+ os.killpg(proc.pid, signal.SIGTERM)+ try:+ proc.wait(timeout=LAUNCH_KILL_GRACE_S)+ except subprocess.TimeoutExpired:+ how = "SIGKILL"+ os.killpg(proc.pid, signal.SIGKILL)+ proc.wait(timeout=LAUNCH_KILL_GRACE_S)+ except (ProcessLookupError, subprocess.TimeoutExpired):+ pass+ fh.write(f"=== moss-fly.sh {action} (pid {proc.pid}) cancelled by stop: {how} ===\n".encode())++ def start(self, action: str) -> dict:+ with self._lock:+ busy = self._running()+ if busy and action != "stop":+ return {"ok": False, "error": f"moss-fly.sh {busy[-1][0]} is still running"}+ if not os.path.isfile(self.script):+ return {"ok": False, "error": f"launcher missing: {self.script}"}+ path = self.log_path()+ os.makedirs(os.path.dirname(path), exist_ok=True)+ # a cockpit started by `EXPLORE=1 moss-fly.sh explore` must not arm the next page launch+ env = {k: v for k, v in os.environ.items() if k not in LAUNCH_ENV_DROP}+ with open(path, "ab" if busy else "wb") as fh:+ if action == "stop":+ self._cancel(fh)+ fh.write(f"=== moss-fly.sh {action} {time.strftime(_ISO)} ===\n".encode())+ fh.flush()+ proc = subprocess.Popen(["bash", self.script, action], cwd=REPO_DIR, env=env,+ stdin=subprocess.DEVNULL, stdout=fh, stderr=subprocess.STDOUT,+ start_new_session=True) # its own process group: Stop can end it+ self._procs = self._running() + [(action, time.time(), proc)]+ self.last = {"action": action, "started_at": time.time(), "pid": proc.pid}+ return {"ok": True, "sent": f"moss-fly.sh {action}", "pid": proc.pid}++ def status(self, lines: bool = True) -> dict:+ with self._lock:+ running = self._running()+ last = self._procs[-1] if self._procs else None+ rc = last[2].poll() if last else None+ head = {"running": bool(running), "action": last[0] if last else None,+ "started_at": last[1] if last else None, "returncode": rc}+ if not lines:+ return head+ try:+ with open(self.log_path(), "rb") as fh:+ fh.seek(0, os.SEEK_END)+ fh.seek(max(0, fh.tell() - 65536))+ tail = fh.read().decode("utf-8", "replace").splitlines()[-LAUNCH_TAIL:]+ except OSError:+ tail = []+ return dict(head, lines=tail)+++# --- bus bookkeeping ---------------------------------------------------------------++class _Latest:+ __slots__ = ("raw", "mono", "arrivals")++ def __init__(self):+ self.raw = None+ self.mono = None+ self.arrivals = deque(maxlen=64)+++class BusCache:+ """Latest bytes + arrival times per stream. Callbacks only store; decoding is+ the request's job, cached by the arrival stamp."""++ def __init__(self):+ self._lock = threading.Lock()+ self._topics = {name: _Latest() for name in list(CHANNELS) + [n for n, _ in CAMERAS.values()]}+ self._by_channel = {ch: name for name, ch in CHANNELS.items()}+ for name, ch in CAMERAS.values():+ self._by_channel[ch] = name+ self._decoded: dict = {}++ def ingest(self, channel: str, data: bytes, mono: float | None = None) -> None:+ name = self._by_channel.get(channel)+ if name is None:+ return+ mono = time.monotonic() if mono is None else mono+ with self._lock:+ t = self._topics[name]+ t.raw, t.mono = data, mono+ t.arrivals.append(mono)++ def clear(self) -> None:+ """Forget every message: a new run must not show the previous run's map, scan or goal."""+ with self._lock:+ self._topics = {name: _Latest() for name in self._topics}+ self._decoded = {}++ def latest(self, name: str):+ with self._lock:+ t = self._topics[name]+ return t.raw, t.mono++ def stats(self, name: str, now: float) -> dict:+ with self._lock:+ t = self._topics[name]+ recent = [a for a in t.arrivals if now - a <= HZ_WINDOW_S]+ mono = t.mono+ hz = (len(recent) - 1) / (recent[-1] - recent[0]) if len(recent) >= 2 and recent[-1] > recent[0] else 0.0+ return {"age_s": None if mono is None else round(now - mono, 2), "hz": round(hz, 1)}++ def decoded(self, name: str, fn):+ """fn(raw) for the latest message of `name`, computed once per arrival.+ Returns (value, mono) or (None, None); a decode error caches None."""+ raw, mono = self.latest(name)+ if raw is None:+ return None, None+ hit = self._decoded.get(name)+ if hit is not None and hit[0] == mono:+ return hit[1], mono+ try:+ value = fn(raw)+ except Exception: # noqa: BLE001 - one bad message must not poison the page+ value = None+ self._decoded[name] = (mono, value)+ return value, mono+++# --- the cockpit: shared part ---------------------------------------------------------++class CameraUnavailable(RuntimeError):+ """The camera stream cannot be served (and why, for a 503)."""+++class Cockpit:+ mode = "live"++ def __init__(self, fly_script: str = FLY_SCRIPT):+ self.launcher = Launcher(fly_script)+ self.episode: dict | None = None+ self.episode_closed: dict | None = None # closed by the cockpit when the stack stopped under it+ self._ep_lock = threading.Lock()+ self._cam_lock = threading.Lock()+ self._cam_clients = {cam: 0 for cam in CAMERAS}+ self.started = time.time()++ @property+ def camera_clients(self) -> int:+ """Viewers on all cameras together."""+ return sum(self._cam_clients.values())++ def camera_viewers(self, cam: str) -> int:+ return self._cam_clients.get(cam, 0)++ def camera_state(self) -> dict:+ doc = {"clients": self.camera_clients, "subscribed": self.camera_clients > 0}+ for cam in CAMERAS:+ n = self._cam_clients[cam]+ doc[cam] = {"clients": n, "subscribed": n > 0}+ return doc++ # episode bookkeeping, shared by both modes+ def _episode(self, body: dict) -> dict:+ action = str(body.get("action", ""))+ if action not in ("start", "stop"):+ raise ValueError('episode needs "action": "start" or "stop"')+ with self._ep_lock:+ if action == "start":+ name = episode_name(body.get("name"))+ self.episode = {"name": name, "started_at": time.time()}+ self.episode_closed = None+ else:+ name = (self.episode or {}).get("name") or episode_name(body.get("name"))+ self.episode = None+ return {"action": action, "name": name, "ts": time.time()}++ def camera_open(self, cam: str = "nose") -> None:+ if cam not in CAMERAS:+ raise CameraUnavailable(f"no camera called {cam!r} (nose or wrist)")+ with self._cam_lock:+ if self._cam_clients[cam] >= MAX_CAMERA_CLIENTS:+ raise CameraUnavailable(f"{MAX_CAMERA_CLIENTS} viewers already on the {cam} camera")+ self._cam_clients[cam] += 1++ def camera_close(self, cam: str = "nose") -> None:+ with self._cam_lock:+ if cam in self._cam_clients:+ self._cam_clients[cam] = max(0, self._cam_clients[cam] - 1)++ def launch(self) -> dict:+ return self.launcher.status()+++# --- live: the stack's bus ----------------------------------------------------------++class LiveCockpit(Cockpit):+ mode = "live"++ def __init__(self, fly_script: str = FLY_SCRIPT):+ super().__init__(fly_script)+ self.bus = BusCache()+ self.bus_error: str | None = None if lcm is not None else "python module lcm not importable"+ if not HAVE_MSGS:+ self.bus_error = "python module dimos_lcm not importable"+ self._pub_lock = threading.Lock()+ self._pub_lcm = None+ self._map_cache = (None, None, 0.0) # (mono, (png, meta), decoded_at)+ self._scan_cache = (None, None, 0.0)+ self._cam_cache = {cam: (None, None) for cam in CAMERAS} # cam -> (mono, jpeg)+ self._encoder = None+ self._ep_seen = None+ 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++ # the listener ------------------------------------------------------------+ def start_listener(self) -> None:+ if lcm is None:+ return+ threading.Thread(target=self._listen, name="lcm-listener", daemon=True).start()++ def _listen(self) -> None:+ """One LCM handle, one thread, explicit channels only; a camera is+ subscribed while someone watches it."""+ while True:+ try:+ lc = lcm.LCM()+ for ch in CHANNELS.values():+ lc.subscribe(ch, self._on_message)+ image_subs = {cam: None for cam in CAMERAS}+ self.bus_error = None+ while True:+ for cam, (_, channel) in CAMERAS.items():+ want = self._cam_clients[cam] > 0+ if want and image_subs[cam] is None:+ image_subs[cam] = lc.subscribe(channel, self._on_message)+ elif not want and image_subs[cam] is not None:+ lc.unsubscribe(image_subs[cam])+ image_subs[cam] = None+ lc.handle_timeout(LCM_HANDLE_MS)+ except Exception as exc: # noqa: BLE001 - multicast not up yet, etc.: retry+ self.bus_error = f"{type(exc).__name__}: {exc}"+ time.sleep(3.0)++ def _on_message(self, channel: str, data: bytes) -> None:+ self.bus.ingest(channel, data)++ # reads -------------------------------------------------------------------+ def _pose(self, now: float) -> dict | None:+ pose, mono = self.bus.decoded("odom", decode_pose)+ if pose is None:+ return None+ return {"x": round(pose["x"], 3), "y": round(pose["y"], 3), "yaw": round(pose["yaw"], 4),+ "age_s": round(now - mono, 2)}++ def _base(self, now: float) -> dict | None:+ t, mono = self.bus.decoded("base_telemetry", lambda raw: json.loads(decode_string(raw)))+ if not isinstance(t, dict):+ return None+ return base_view(t, now - mono)++ def _bool(self, name: str):+ value, mono = self.bus.decoded(name, decode_bool)+ return value, mono++ def _nav(self, now: float, stack: dict | None = None) -> dict:+ start, t_start = self._bool("explore_cmd")+ _, t_stop = self._bool("stop_explore_cmd")+ done, t_done = self._bool("explore_done")+ # a pad touch on moss-explore: MovementManager publishes stop_movement and the+ # explorer stops on it (WavefrontFrontierExplorer._on_stop_movement)+ halt, t_halt = self._bool("stop_movement")+ t_start = t_start if start else None+ exploring = t_start is not None and t_start > max(t_stop or -1.0, (t_done if done else None) or -1.0,+ (t_halt if halt else None) or -1.0)+ # moss-nav has no explorer: an explore_cmd there only opens map writing (the costmap+ # mission gate), nothing drives. Only an explore stack can be exploring.+ if stack is not None and stack.get("running") and "explore" not in str(stack.get("blueprint") or ""):+ exploring = False+ explore_done = bool(done) and (t_start is None or (t_done or 0) > t_start)+ goal_pose, t_goal = self.bus.decoded("goal_request", decode_pose)+ goal = None+ if goal_pose is not None and math.isfinite(goal_pose["x"]) and math.isfinite(goal_pose["y"]):+ goal = {"x": round(goal_pose["x"], 3), "y": round(goal_pose["y"], 3)}+ reached, t_reached = self._bool("goal_reached")+ goal_reached = reached if (t_reached is not None and (t_goal is None or t_reached > t_goal)) else None+ tw, t_tw = self.bus.decoded("cmd_vel", decode_twist)+ last_cmd = None if tw is None else {"vx": round(tw[0], 3), "wz": round(tw[1], 3),+ "age_s": round(now - t_tw, 2)}+ return {"exploring": exploring, "explore_done": explore_done, "goal": goal,+ "goal_reached": goal_reached, "last_cmd_vel": last_cmd}++ def _episode_from_bus(self) -> None:+ """Markers published by anyone (another page, a script) move the episode too."""+ marker, mono = self.bus.decoded("episode_marker", lambda raw: json.loads(decode_string(raw)))+ if not isinstance(marker, dict) or mono == self._ep_seen:+ return+ self._ep_seen = mono+ with self._ep_lock:+ if marker.get("action") == "start":+ self.episode = {"name": marker.get("name"), "started_at": marker.get("ts")}+ self.episode_closed = None+ elif marker.get("action") == "stop":+ self.episode = None++ def _sync_run(self, stack: dict) -> None:+ """A new run_id in the registry: drop the previous run's messages (its costmap+ is in another odometry frame). A stop alone keeps them: the last map stays readable."""+ run_id = stack.get("run_id") if stack.get("running") else None+ if run_id is None or run_id == self._run_id:+ return+ if self._run_id is not None:+ self.bus.clear()+ self._map_cache = (None, None, 0.0)+ self._scan_cache = (None, None, 0.0)+ self._cam_cache = {cam: (None, None) for cam in CAMERAS}+ self._run_id = run_id++ def _close_orphan_episode(self) -> None:+ """The stack stopped with an episode open: nothing records it any more, close it here."""+ with self._ep_lock:+ if self.episode is None:+ return+ self.episode_closed = dict(self.episode, ts=time.time(),+ reason="the stack stopped: the episode was closed")+ self.episode = None++ def _moss_stack(self) -> dict | None:+ """The running stack if it is a MOSS one. Another dimOS stack may share the+ computer and the bus: it never takes a goal or an explore_cmd from this page."""+ st = stack_status()+ return st if st.get("running") and str(st.get("blueprint") or "").startswith("moss") else None++ def state(self) -> dict:+ now = time.monotonic()+ stack = stack_status()+ self._sync_run(stack)+ self._episode_from_bus()+ if not stack["running"]:+ pid = _dimos_run_pid()+ if pid:+ stack = {"running": True, "run_id": None, "blueprint": None, "pid": pid,+ "started_at": None, "uptime_s": None, "log_dir": None}+ if not stack["running"]:+ self._close_orphan_episode()+ env, _ = bus_env(stack.get("pid") if stack["running"] else None)+ try:+ import explore_ctl+ transport = explore_ctl.transport(env)+ except Exception: # noqa: BLE001 - no dimos_lcm: the same order as explore_ctl.transport+ transport = (env.get("DIMOS_TRANSPORT") or env.get("TRANSPORT") or "lcm").strip().lower()+ return {+ "ts": time.time(), "mode": self.mode,+ "stack": stack,+ "pose": self._pose(now),+ "topics": {name: self.bus.stats(name, now) for name in STATE_TOPICS},+ "base": self._base(now),+ "nav": self._nav(now, stack),+ "recording": recording_view(self.episode, self.episode_closed),+ "system": system_metrics(),+ "bus": {"transport": transport, "listening": lcm is not None and HAVE_MSGS,+ "error": self.bus_error},+ "camera": self.camera_state(),+ "launch": self.launcher.status(lines=False),+ }++ def map(self):+ self._sync_run(stack_status())+ raw, mono = self.bus.latest("global_costmap")+ if raw is None:+ return None+ cached_mono, cached, at = self._map_cache+ if cached is not None and (cached_mono == mono or time.monotonic() - at < MAP_MIN_INTERVAL_S):+ return cached+ result = decode_costmap(raw)+ self._map_cache = (mono, result, time.monotonic())+ return result++ def scan(self) -> dict | None:+ self._sync_run(stack_status())+ now = time.monotonic()+ raw, mono = self.bus.latest("lidar")+ name = "lidar"+ if raw is None or now - mono > SCAN_FALLBACK_S:+ fb_raw, fb_mono = self.bus.latest("pointcloud")+ if fb_raw is not None:+ raw, mono, name = fb_raw, fb_mono, "pointcloud"+ if raw is None:+ return None+ cached_key, cached, at = self._scan_cache+ if cached is not None and (cached_key == (name, mono) or now - at < SCAN_MIN_INTERVAL_S):+ return cached+ frame, stamp, xy = cloud_xy(raw)+ pts = np.round(downsample(xy).astype(float), 3).tolist()+ doc = {"frame": frame or ("world" if name == "lidar" else "lidar_link"), "stamp": stamp,+ "source": name, "age_s": round(now - mono, 2), "points": pts}+ self._scan_cache = ((name, mono), doc, now)+ return doc++ def path(self) -> dict:+ self._sync_run(stack_status())+ pts, mono = self.bus.decoded("path", decode_path)+ return {"points": pts or [], "age_s": None if mono is None else round(time.monotonic() - mono, 2)}++ def log(self, n: int) -> list[dict]:+ return log_tail(stack_status().get("log_dir"), n)++ def preflight(self) -> dict:+ return preflight_doc()++ # camera ------------------------------------------------------------------+ def _encode(self, arr: np.ndarray, encoding: str) -> bytes:+ if self._encoder is None:+ try:+ from turbojpeg import TJPF_BGR, TJPF_GRAY, TJPF_RGB, TurboJPEG+ tj = TurboJPEG()+ fmts = {"rgb8": TJPF_RGB, "bgr8": TJPF_BGR, "mono8": TJPF_GRAY}++ def enc(a, e):+ return tj.encode(a, quality=70, pixel_format=fmts[e])+ self._encoder = ("turbojpeg", enc)+ except Exception: # noqa: BLE001+ try:+ import cv2++ def enc(a, e):+ a = a[..., ::-1] if e == "rgb8" else a+ ok, buf = cv2.imencode(".jpg", np.ascontiguousarray(a), [cv2.IMWRITE_JPEG_QUALITY, 70])+ if not ok:+ raise CameraUnavailable("cv2 could not encode the frame")+ return buf.tobytes()+ self._encoder = ("cv2", enc)+ except Exception: # noqa: BLE001+ self._encoder = ("none", None)+ name, enc = self._encoder+ if enc is None:+ raise CameraUnavailable("raw camera frames and no JPEG encoder here (turbojpeg or cv2); "+ "put the stack's color_image on JpegLcmTransport")+ return enc(arr, encoding)++ def camera_frame(self, cam: str = "nose"):+ """(key, jpeg) of the latest frame of that camera, or None before the first one."""+ topic = CAMERAS[cam][0]+ raw, mono = self.bus.latest(topic)+ if raw is None:+ return None+ cached = self._cam_cache[cam]+ if cached[0] == mono:+ return mono, cached[1]+ m = LcmImage.lcm_decode(raw)+ enc = str(m.encoding).lower()+ data = m.data if isinstance(m.data, (bytes, bytearray)) else bytes(m.data)+ if enc == "jpeg":+ jpeg = bytes(data) # JpegLcmTransport: the bus already carries the JPEG+ else:+ chans = {"rgb8": 3, "bgr8": 3, "mono8": 1, "rgba8": 4, "bgra8": 4}.get(enc)+ if chans is None:+ raise CameraUnavailable(f"camera encoding {m.encoding!r} not handled")+ arr = np.frombuffer(data, np.uint8)[: m.height * m.width * chans]+ arr = arr.reshape(m.height, m.width, chans) if chans > 1 else arr.reshape(m.height, m.width)+ if chans == 4:+ arr, enc = arr[..., :3], ("rgb8" if enc == "rgba8" else "bgr8")+ jpeg = self._encode(arr, enc)+ self._cam_cache[cam] = (mono, jpeg)+ return mono, jpeg++ # commands ----------------------------------------------------------------+ def _publisher(self):+ """One LCM handle for every command publish (None: explore_ctl opens its own)."""+ if lcm is None:+ return None+ with self._pub_lock:+ if self._pub_lcm is None:+ self._pub_lcm = lcm.LCM()+ return self._pub_lcm++ def command(self, name: str, body: dict) -> dict:+ if name == "stack":+ return self._stack(body)+ try:+ import explore_ctl # needs dimos_lcm: imported where it is used+ from dimos_lcm.std_msgs import Bool, String+ except ImportError as exc:+ return {"ok": False, "error": f"cannot publish on the bus here: {exc}", "_code": 503}+ # the bus is the one the running stack chose (the listener is LCM-only too): say so, publish nothing+ st = stack_status()+ env, whose = bus_env(st.get("pid") if st.get("running") else _dimos_run_pid())+ try:+ explore_ctl.require_lcm(env)+ except explore_ctl.TransportError as exc:+ return {"ok": False, "error": f"cannot publish on the bus here: {exc} (read from {whose})",+ "_code": 503}+ bus = self._publisher()++ def pub(stream, msg, msg_name=None):+ with self._pub_lock:+ return explore_ctl.publish_msg(stream, msg, msg_name=msg_name, bus=bus, env=env)++ def not_moss():+ st = stack_status()+ what = f"the running stack is {st.get('blueprint')!r}" if st.get("running") else "no stack is running"+ return {"ok": False, "error": f"refused: {what}, not a MOSS stack", "_code": 409}++ if name == "explore":+ action = body.get("action")+ if action not in ("start", "stop"):+ raise ValueError('explore needs "action": "start" or "stop"')+ if self._moss_stack() is None:+ return not_moss()+ with self._goal_lock:+ self._goal_gen += 1+ return {"ok": True, "sent": pub(explore_ctl.CHANNELS[action], Bool(data=True))}+ if name == "goal":+ x, y = _finite_xy(body)+ st = self._moss_stack()+ if st is None:+ return not_moss()+ exploring = self._nav(time.monotonic(), st)["exploring"]+ with self._goal_lock:+ self._goal_gen += 1+ gen = self._goal_gen+ if not exploring:+ sent = pub("goal_request", goal_message(x, y), msg_name="geometry_msgs.PoseStamped")+ return {"ok": True, "sent": sent, "goal": {"x": x, "y": y}}+ # Explorer2 publishes its own goal_request: stop it first. Its stop ends with a+ # 'stay here' goal about 2 s later, which would replace ours: ours goes after it.+ first = pub(explore_ctl.CHANNELS["stop"], Bool(data=True))++ def deferred():+ with self._goal_lock:+ if gen != self._goal_gen or self._moss_stack() is None:+ return # a stop, an explore, a newer goal or no stack: drop it+ pub("goal_request", goal_message(x, y), msg_name="geometry_msgs.PoseStamped")+ _later(GOAL_AFTER_STOP_S, deferred)+ return {"ok": True, "sent": first,+ "queued": explore_ctl.channel("goal_request", "geometry_msgs.PoseStamped"),+ "queued_s": GOAL_AFTER_STOP_S, "goal": {"x": x, "y": y}}+ if name == "stop":+ with self._goal_lock:+ self._goal_gen += 1 # a goal still queued behind an explore stop is dropped+ # soft stop: the firmware deadman (300 ms) and `moss-fly.sh stop` are the hard ones+ a = pub(explore_ctl.CHANNELS["stop"], Bool(data=True))+ b = pub("stop_movement", Bool(data=True))+ c = pub("tele_cmd_vel", zero_twist())+ also = [a, b]+ if stack_status().get("blueprint") != "moss-explore":+ # moss-nav has no MovementManager: the pad's tele_cmd_vel is remapped+ # onto cmd_vel and nobody listens on /tele_cmd_vel, so zero cmd_vel itself+ also.append(c)+ c = pub("cmd_vel", zero_twist())+ return {"ok": True, "sent": c, "also": also}+ if name == "episode":+ marker = self._episode(body)+ sent = pub("episode_marker", String(data=json.dumps(marker)))+ return {"ok": True, "sent": sent, "episode": self.episode}+ return {"ok": False, "error": f"unknown command {name!r}", "_code": 404}++ def _stack(self, body: dict) -> dict:+ action = body.get("action")+ if action not in ("map", "explore", "stop", "check"):+ raise ValueError('stack needs "action": "map", "explore", "stop" or "check"')+ if action in ("map", "explore"):+ st = stack_status()+ if st["running"] and not str(st.get("blueprint") or "").startswith("moss"):+ return {"ok": False, "error": f"a dimOS stack is already running ({st.get('blueprint')!r}, "+ "not MOSS): the MOSS cockpit leaves it alone", "_code": 409}+ if st["running"] or _dimos_run_pid():+ return {"ok": False, "error": "a dimOS stack is already running: stop it first"}+ return self.launcher.start(action)+++# --- demo: a synthetic rover, for the page without a robot ------------------------------++# 320x240 JPEG (made once with cv2; studio-to-navy gradient, a mint square, "DEMO CAMERA")+_DEMO_JPEG_B64 = (+ "/9j/4AAQSkZJRgABAQAAAQABAAD/2wBDAA0JCgsKCA0LCgsODg0PEyAVExISEyccHhcgLikxMC4pLSwzOko+MzZGNywtQFdB"+ "RkxOUlNSMj5aYVpQYEpRUk//2wBDAQ4ODhMREyYVFSZPNS01T09PT09PT09PT09PT09PT09PT09PT09PT09PT09PT09PT09P"+ "T09PT09PT09PT09PT0//wAARCADwAUADASIAAhEBAxEB/8QAHwAAAQUBAQEBAQEAAAAAAAAAAAECAwQFBgcICQoL/8QAtRAA"+ "AgEDAwIEAwUFBAQAAAF9AQIDAAQRBRIhMUEGE1FhByJxFDKBkaEII0KxwRVS0fAkM2JyggkKFhcYGRolJicoKSo0NTY3ODk6"+ "Q0RFRkdISUpTVFVWV1hZWmNkZWZnaGlqc3R1dnd4eXqDhIWGh4iJipKTlJWWl5iZmqKjpKWmp6ipqrKztLW2t7i5usLDxMXG"+ "x8jJytLT1NXW19jZ2uHi4+Tl5ufo6erx8vP09fb3+Pn6/8QAHwEAAwEBAQEBAQEBAQAAAAAAAAECAwQFBgcICQoL/8QAtREA"+ "AgECBAQDBAcFBAQAAQJ3AAECAxEEBSExBhJBUQdhcRMiMoEIFEKRobHBCSMzUvAVYnLRChYkNOEl8RcYGRomJygpKjU2Nzg5"+ "OkNERUZHSElKU1RVVldYWVpjZGVmZ2hpanN0dXZ3eHl6goOEhYaHiImKkpOUlZaXmJmaoqOkpaanqKmqsrO0tba3uLm6wsPE"+ "xcbHyMnK0tPU1dbX2Nna4uPk5ebn6Onq8vP09fb3+Pn6/9oADAMBAAIRAxEAPwDBopM0ZroOcWikzRmgBaKTNGaAFopM0ZoA"+ "WikzRmgBaKTNGaAFopM0ZoAWikzRmgBaKTNGaAFopM0ZoAWikzRmgBaKTNGaAFopM0ZoAWikzRmgBaKTNGaAFopM0ZoAWikz"+ "RmgBaKTNGaAFopM0ZoAWikzRmgBM0ZpuaM0ASxxSy58uN3x12qTikkjkjOJI2Qn+8MV13geRo9O1ORcbkAYfUBqk0DX5tau2"+ "07U4YZI5EJGFxyOx/DNTcqxx0cUsufLjd8ddqk4pxtrgAkwSgDqdhrtvDVu9lLrdvbDLRSYiB78Nt/pVS6v/ABYtrK09nGsQ"+ "Ql22rwMcnrRzBY5ExyCMSFGCHo2OD+NIqs7BUUsx6ADJr0DTLSG/8G21nOwXzlKoT2bJI/lVTwfpDWU0l3fLsmLtDCp9RncR"+ "+R/I0uYOU4pgysVYEEdQe1PaGZYxI0UgQ9GKnB/Guz0uxiuvGWpzzIri3YFQezHof0NQ6V4pur/XFtZ44jazsUCbeQOcfWnz"+ "BY47NKAWICgknoBWj4ks47DXLiCFQsWQyqOwIzit7QPL0nwpcawsSPcMSFZh0GQoH0zzRfQVjkpYZYcebE6Z6blIpgySAAST"+ "0Arq7PxVDeW09tr8QkjcfKY0/wA/gap+DrJbnWvPbmG1UyEn16D/AB/Ci/cdjCkjkjOJEZCezDFAjkMZkCMUHBbHA/Guv8RS"+ "Ra54dTU7dfmtpWUjuFzj/wCJNWvCMcE3heWK5x5UkrI2ffA/rRzaBy6nCDJIABJPQCn+VL5nl+W+/wDu7Tn8q6zw9oDWetTz"+ "X2BHauEiY9HZvun9R+JFXJP+SiR/9cP/AGU0cwcpxX2W5/595v8Avg01YJmZlWKQleoCniu5v7vxQl9MtnaI1uHPlsQOR+dR"+ "+F57uQ6zNOuLsONygfxANxj6ilzBynEyRSx/6yN0/wB5SKZmvQdBudV1Lz4NbsgLcpwZIimT6YPWuU0WwjvfEq28fzW8cpcn"+ "1RTx+fA/GmpBYzHiljAMkboD03AjNMzXfa1LFrukalFAMy2EuV99o5P4/MPwrz/NCdxNWHZozTc0ZqhDs0ZpuaM0AOzRmm5o"+ "zQA7NGabmjNADs0ZpuaM0AOzRmm5ozQA7NGabmjNADs0ZpuaM0AJmjNNzRmkM7TwMhm0/U4lIDOFUZ9w1S6LoX9gXjX+p3ts"+ "qxodoVj3+oFcNmjNKw7nfeGJ21B9bmhbY00mYyeNuQ2P6VWn0DxK0Egl1dHjKncpnfBGOR0ris0ZpWC52dxO9v4E06eFtskc"+ "ysp9wWpdO1+XWvFGnBohDHEH+QNnLFDzXF5ozT5Qudzp2owWXjLUobiRUS4bCsem4dB+ppdO8MHTdYGoT3UH2OJi6fNg98Z7"+ "cVwuaXJxjPApWC5qeIr6PUNbuLiE5iJCocdQBjNb3h6W31TwzPorzpFPk7A3GRkMD781xmaM07aBc7K38NWWl201z4gmicAf"+ "u0jcjP8AIk+1T6O1povhVru+jcreuQUT7xU5AHUdsn8a4csT1OaTNFguegaHfaHfLcaVYWs1us6MWWQ8Nxg45POP5VTtontP"+ "BGpwucSQzspx6grXF5ozS5QudUniS41S70m0kjEYS4jMrBs+YQRzjtWrIf8Ai4sf/XD/ANlNcBmjNPlC56BqGj+Ip7+eW11U"+ "Rwu5KJ5zjaPTAFM8NxXFgNbjnlD3ERDM4OcttJzzXBZozS5QudzoWpzeINNvNMvbgi5ZSUkGFJH4eh/MGmeF4F0XTtQ1O/Uq"+ "Y2MWB1+U8gfU8fhXE5ozRyhc73QdX0E6h9nsLOeCS5+UtIcq3U4PzGuR1mz/ALP1a5tf4Uf5f908j9DVDNGaaVgbuOzRmm5o"+ "zTEOzRmm5ozQA7NGabmjNADs0ZpuaM0AOzRmm5ozQA7NGabmjNADs0ZpuaM0AOzRmm5ozQAlFJRQAtFJRQAtFJRQAtFJRQAt"+ "FJRQAtFJRQAtFJRQAtFJRQAtFJRQAtFJRQAtFJRQAtFJRQAtFJRQAtFJRQAtFJRQAtFJRQAtFJRQAtFJRQAtFJRQAtFJRQA3"+ "NGaTNGaAFzRmkzRmgBc0ZpM0ZoAXNGaTNGaAFzRmkzRmgBc0ZpM0ZoAXNGaTNGaAFzRmkzRmgBc0ZpM0ZoAXNGaTNGaAFzRm"+ "kzRmgBc0ZpM0ZoAXNGaTNGaAFzRmkzRmgBc0ZpM0ZoAXNGaTNGaAFzRmkzRmgBc0ZpM0ZoAXNGaTNGaAFzRmkzRmgBtFJmjN"+ "IYtFJmjNAC0UmaM0ALRSZozQAtFJmjNAC0UmaM0ALRSZozQAtFJmjNAC0UmaM0ALRSZozQAtFJmjNAC0UmaM0ALRSZozQAtF"+ "JmjNAC0UmaM0ALRSZozQAtFJmjNAC0UmaM0ALRSZozQAtFJmjNACZozTaKAHZozTaKAHZozTaKAHZozTaKAHZozTaKAHZozT"+ "aKAHZozTaKAHZozTaKAHZozWhaaFqd5bJcW1tvifO1vMUZwcdz7VN/wjOs/8+f8A5FT/ABpXRDqwWjkjJzRmtb/hGdZ/58//"+ "ACKn+NH/AAjOs/8APn/5FT/Gi6F7an/MvvMnNGa1v+EZ1n/nz/8AIqf40f8ACM6z/wA+f/kVP8aLoPbU/wCZfeZOaM1rf8Iz"+ "rP8Az5/+RU/xpk3h3VoIXmltNscalmPmIcAcnvRdB7Wn/MvvMzNGabRTNB2aM02igB2aM02igB2aM02igB2aM02igB2aM02i"+ "gB2aM02igB2aM02igBKKSigYtFJRQAtFJRQAtFJRQAtFJRQAtFJRQAtFJRQAtFJRQB6R4U/5Fy0/4H/6G1a9Y/hP/kW7T/gf"+ "/obVsVi9zwq38SXqwooopGYUUUUAFU9X/wCQNff9e8n/AKCauVT1j/kDX3/XvJ/6CaaKh8SPLaKSitj3xaKSigBaKSigBaKS"+ "igBaKSigBaKSigBaKSigBaKSigBuaM0maM0ALmjNJmjNAC5ozSZozQAuaM0maM0ALmjNJmjNAC5ozSZozQAuaM0maM0ALmjN"+ "JmjNAHpXhL/kW7T/AIH/AOhtWxWN4S/5Fq0/4H/6G1bNYvc8Kt/El6sKKKKRmFFFFABVPWP+QLff9e8n/oJq5VPWf+QLff8A"+ "XvJ/6CaaKh8SPKs0ZpM0ZrY98XNGaTNGaAFzRmkzRmgBc0ZpM0ZoAXNGaTNGaAFzRmkzRmgBc0ZpM0ZoAXNGaTNGaAG5ozSZ"+ "ozSAXNGaTNGaAFzRmkzRmgBc0ZpM0ZoAXNGaTNGaAFzRmkzRmgBc0ZpM0ZoAXNGaTNGaAPTPCP8AyLVp/wAD/wDQ2rZrG8If"+ "8izaf8D/APQ2rZrJ7nhVv4kvVhRRRSMwooooAKpaz/yBb/8A69pP/QTV2qWs/wDIEv8A/r2k/wDQTTRUPiR5RmjNJmjNanvi"+ "5ozSZozQAuaM0maM0ALmjNJmjNAC5ozSZozQAuaM0maM0ALmjNJmjNAC5ozSZozQAmaM1e/tG1/6Ath/33P/APHKP7Rtf+gL"+ "Yf8Afc//AMcoGUc0Zq9/aNr/ANAWw/77n/8AjlH9o2v/AEBbD/vuf/45QBRzRmr39o2v/QFsP++5/wD45R/aNr/0BbD/AL7n"+ "/wDjlAFHNGavf2ja/wDQFsP++5//AI5R/aNr/wBAWw/77n/+OUAUc0Zq9/aNr/0BbD/vuf8A+OUf2ja/9AWw/wC+5/8A45QB"+ "RzRmr39o2v8A0BbD/vuf/wCOUf2ja/8AQFsP++5//jlAFHNGavf2ja/9AWw/77n/APjlH9o2v/QFsP8Avuf/AOOUAUc0Zq9/"+ "aNr/ANAWw/77n/8AjlH9o2v/AEBbD/vuf/45QB33hD/kWbT/AIH/AOhtW1XnFp4vvLK2S3tLOyihTO1AHOMnJ5LE9TU3/Cc6"+ "p/zws/8Avhv/AIqocWeXUwlSU213PQaK8+/4TnVP+eFn/wB8N/8AFUf8Jzqn/PCz/wC+G/8AiqXKyPqdU9Borz7/AITnVP8A"+ "nhZ/98N/8VR/wnOqf88LP/vhv/iqOVh9TqnoNUta/wCQJf8A/XtJ/wCgmuL/AOE51T/nhZ/98N/8VTJvGmoTwvDNa2bxyKVd"+ "drjIPBHDUcrHHB1U0znc0Zq9/aNr/wBAWw/77n/+OUf2ja/9AWw/77n/APjlaHrFHNGavf2ja/8AQFsP++5//jlH9o2v/QFs"+ "P++5/wD45QBRzRmr39o2v/QFsP8Avuf/AOOUf2ja/wDQFsP++5//AI5QBRzRmr39o2v/AEBbD/vuf/45R/aNr/0BbD/vuf8A"+ "+OUAUc0Zq9/aNr/0BbD/AL7n/wDjlH9o2v8A0BbD/vuf/wCOUAUc0Zq9/aNr/wBAWw/77n/+OUf2ja/9AWw/77n/APjlAFHN"+ "Gavf2ja/9AWw/wC+5/8A45R/aNr/ANAWw/77n/8AjlAFHNGavf2ja/8AQFsP++5//jlH9o2v/QFsP++5/wD45QBnZozTc0Zo"+ "AdmjNNzRmgB2aM03NGaAHZozTc0ZoAdmjNNzRmgB2aM03NGaAHZozTc0ZoAdmjNNzRmgB2aM03NGaAHZozTc0ZoAdmjNNzRm"+ "gB2aM03NGaAHZozTc0ZoAdmjNNzRmgB2aM03NGaAHZozTc0ZoAdmjNNzRmgB2aM03NGaAHZozTc0ZoAdmjNNzRmgBM0ZpKKB"+ "i5ozSUUALmjNJRQAuaM0lFAC5ozSUUALmjNJRQAuaM0lFAC5ozSUUALmjNJRQAuaM0lFAC5ozSUUALmjNJRQAuaM0lFAC5oz"+ "SUUALmjNJRQAuaM0lFAC5ozSUUALmjNJRQAuaM0lFAC5ozSUUANopM0ZpALRSZozQAtFJmjNAC0UmaM0ALRSZozQAtFJmjNA"+ "C0UmaM0ALRSZozQAtFJmjNAC0UmaM0ALRSZozQAtFJmjNAC0UmaM0ALRSZozQAtFJmjNAC0UmaM0ALRSZozQAtFJmjNAC0Um"+ "aM0ALRSZozQAmaM03NGaAHZozTc0ZoAdmjNNzRmgB2aM03NGaAHZozTc0ZoAdmjNNzRmgB2aM03NGaAHZozTc0ZoAdmjNNzR"+ "mgB2aM03NGaAHZozTc0ZoAdmjNNzRmgB2aM03NGaAHZozTc0ZoAdmjNNzRmgB2aM03NGaAHZozTc0ZoAdmjNNzRmgB2aM03N"+ "GaAHZozTc0ZoA//Z"+)+DEMO_JPEG = base64.b64decode("".join(_DEMO_JPEG_B64))+++class DemoWorld:+ """A 12 x 12 m square corridor around a central block, mapped as the pose drives+ its loop at 0.2 m/s. Lethal walls, a cost ramp near them, unknown outside."""++ RES = 0.05+ N = 240+ OX = OY = -6.0+ SPEED = 0.2+ LOOP = ((-4.25, -4.25), (4.25, -4.25), (4.25, 4.25), (-4.25, 4.25))+ SEEN_RADIUS_M = 3.0++ def __init__(self, now: float):+ c = self.OX + (np.arange(self.N) + 0.5) * self.RES+ X, Y = np.meshgrid(c, c) # rows = y, from min y up+ ax, ay = np.abs(X), np.abs(Y)+ inside = (ax < 5.6) & (ay < 5.6)+ outer = inside & ((ax >= 5.4) | (ay >= 5.4))+ block = (ax <= 3.0) & (ay <= 3.0)+ block_wall = block & ((ax >= 2.85) | (ay >= 2.85))+ bin_box = (X >= -5.25) & (X <= -4.95) & (Y >= 0.3) & (Y <= 0.7) # off the loop line+ truth = np.full((self.N, self.N), -1, dtype=np.int8)+ free = inside & ~outer & ~block+ truth[free] = 0+ d_outer = 5.4 - np.maximum(ax, ay)+ d_block = np.hypot(np.maximum(ax - 3.0, 0), np.maximum(ay - 3.0, 0))+ d_bin = np.hypot(np.maximum(np.abs(X + 5.1) - 0.15, 0), np.maximum(np.abs(Y - 0.5) - 0.2, 0))+ d = np.minimum(np.minimum(d_outer, d_block), d_bin)+ ramp = free & (d < 0.4)+ truth[ramp] = np.clip(np.rint(99 * (1 - d[ramp] / 0.4)), 1, 99).astype(np.int8)+ truth[outer | block_wall | bin_box] = 100+ self.X, self.Y, self.truth = X, Y, truth+ self.seen = np.zeros_like(truth, dtype=bool)+ self.x, self.y, self.yaw = self.LOOP[0][0], self.LOOP[0][1], 0.0+ self.corner = 1+ self.goal = None+ self.goal_active = False+ self.goal_reached = None+ self.hold_until = 0.0+ self.moving = False+ self.travelled = 0.0+ self.last = now+ self.map_stamp = now+ self._reveal(now)++ def _reveal(self, now: float) -> None:+ self._revealed_at = (self.x, self.y)+ near = np.hypot(self.X - self.x, self.Y - self.y) <= self.SEEN_RADIUS_M+ if not (near & ~self.seen).any():+ return+ self.seen |= near+ self.map_stamp = now++ def cells(self) -> np.ndarray:+ return np.where(self.seen, self.truth, np.int8(-1)).astype(np.int8)++ def target(self):+ return self.goal if self.goal_active else self.LOOP[self.corner]++ def step(self, now: float, may_patrol: bool) -> None:+ dt = min(max(now - self.last, 0.0), 1.0)+ self.last = now+ if not self.goal_active and (not may_patrol or now < self.hold_until):+ self.moving = False+ return+ tx, ty = self.target()+ dist = math.hypot(tx - self.x, ty - self.y)+ stride = self.SPEED * dt+ if dist > 1e-9:+ self.yaw = math.atan2(ty - self.y, tx - self.x)+ if dist <= stride:+ self.x, self.y = tx, ty+ if self.goal_active:+ self.goal_active, self.goal_reached = False, True+ self.hold_until = now + 5.0+ self.corner = min(range(4), key=lambda i: math.hypot(self.LOOP[i][0] - tx, self.LOOP[i][1] - ty))+ else:+ self.corner = (self.corner + 1) % 4+ else:+ self.x += stride * (tx - self.x) / dist+ self.y += stride * (ty - self.y) / dist+ self.travelled += min(dist, stride)+ self.moving = stride > 0+ if math.hypot(self.x - self._revealed_at[0], self.y - self._revealed_at[1]) > 0.25:+ self._reveal(now)++ def scan(self, max_range_m: float = 8.0) -> np.ndarray:+ """360 rays at 1 deg, ray-marched on the true walls (what a lidar would see)."""+ a = np.radians(np.arange(0.0, 360.0, 1.0))+ r = np.arange(1, int(max_range_m / self.RES)) * self.RES+ px = self.x + np.cos(a)[:, None] * r+ py = self.y + np.sin(a)[:, None] * r+ col = np.floor((px - self.OX) / self.RES).astype(int)+ row = np.floor((py - self.OY) / self.RES).astype(int)+ inb = (col >= 0) & (col < self.N) & (row >= 0) & (row < self.N)+ occ = np.zeros(px.shape, dtype=bool)+ occ[inb] = self.truth[row[inb], col[inb]] == 100+ hit = occ.any(axis=1)+ first = occ.argmax(axis=1)+ idx = np.nonzero(hit)[0]+ return np.column_stack((px[idx, first[idx]], py[idx, first[idx]])).astype(np.float32)+++class DemoCockpit(Cockpit):+ """Same endpoints, synthetic sources, never touches LCM or the launcher; the+ machine metrics are real."""++ mode = "demo"++ def __init__(self, fly_script: str = FLY_SCRIPT):+ super().__init__(fly_script)+ now = time.time()+ self._lock = threading.Lock()+ self.world = DemoWorld(now)+ self.stack = {"running": True, "blueprint": "moss-explore", "started": now}+ self.exploring = True+ self.explore_done = False+ self.halted = False+ self.stack_stopped_at = None+ self._map_cache = (None, None)+ self._log: deque = deque(maxlen=200)+ self._launch_lines: deque = deque(maxlen=LAUNCH_TAIL)+ self._launch_action = None+ self._preflight_ts = now+ 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)")++ def _say(self, level: str, event: str) -> None:+ self._log.append({"ts": time.strftime(_ISO), "level": level, "event": event, "logger": "demo"})++ def _tick(self) -> float:+ now = time.time()+ with self._lock:+ if not self.stack["running"]: # no stack, no motion, not even toward a goal+ self.world.last, self.world.moving = now, False+ return now+ may = not self.halted and (self.exploring or self.stack["blueprint"] == "moss-nav")+ self.world.step(now, may)+ return now++ def state(self) -> dict:+ now = self._tick()+ 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(),+ "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)+ nominal = {"odom": 10.0, "global_costmap": 1.0, "lidar": 10.0, "pointcloud": 10.0, "path": 1.0,+ "cmd_vel": 10.0, "base_telemetry": 2.0}+ for topic, _ in CAMERAS.values():+ nominal[topic] = 5.0+ topics = {k: {"age_s": round(silent + 1.0 / hz / 2, 2), "hz": hz if running else 0.0}+ for k, hz in nominal.items()}+ volts = max(11.0, 11.6 - 0.0005 * (now - self.started))+ drive = w.moving and running+ pct = 40 if drive else 0+ telemetry = {"fw": "qtpy-mdd3a-v3", "driver": "mdd3a", "mode": 2 if drive else 0,+ "reason": "drive" if drive else "boot", "bus_mV": int(volts * 1000), "ina_ok": True,+ "ticks": [int(w.travelled * 1000), int(w.travelled * 1000)],+ "target_pct": [pct, pct], "output_pct": [pct, pct], "ramp_ms": [5, 5],+ "firmware_age_s": 0.1, "mock": True}+ return {+ "ts": now, "mode": self.mode, "stack": stack,+ "pose": ({"x": round(w.x, 3), "y": round(w.y, 3), "yaw": round(w.yaw, 4), "age_s": 0.05}+ if running else None),+ "topics": topics,+ "base": base_view(telemetry, 0.3) if running else None,+ "nav": {"exploring": self.exploring and running, "explore_done": self.explore_done,+ "goal": None if w.goal is None else {"x": round(w.goal[0], 3), "y": round(w.goal[1], 3)},+ "goal_reached": w.goal_reached,+ "last_cmd_vel": {"vx": w.SPEED if drive else 0.0, "wz": 0.0, "age_s": 0.1}},+ "recording": {"db": os.path.join(state_dir(), "recordings", "moss.db") + " (demo)",+ "exists": True, "size_mb": round(0.5 + 0.01 * up, 1), "episode": self.episode},+ "system": system_metrics(),+ "bus": {"transport": "demo", "listening": False, "error": None},+ "camera": self.camera_state(),+ "launch": {"running": False, "action": self._launch_action},+ }++ def map(self):+ self._tick()+ with self._lock:+ stamp = self.world.map_stamp+ if self._map_cache[0] != stamp:+ self._map_cache = (stamp, render_grid(self.world.cells(), DemoWorld.RES, DemoWorld.OX,+ DemoWorld.OY, stamp, "world"))+ return self._map_cache[1]++ def scan(self) -> dict | None:+ now = self._tick()+ if not self.stack["running"]:+ return None+ with self._lock:+ pts = downsample(self.world.scan())+ return {"frame": "world", "stamp": now, "source": "demo", "age_s": 0.05,+ "points": np.round(pts.astype(float), 3).tolist()}++ def path(self) -> dict:+ self._tick()+ w = self.world+ if not self.stack["running"] or not (w.goal_active or w.moving):+ return {"points": [], "age_s": 0.5}+ tx, ty = w.target()+ return {"points": [[round(w.x, 3), round(w.y, 3)], [round(tx, 3), round(ty, 3)]], "age_s": 0.5}++ def log(self, n: int) -> list[dict]:+ return list(self._log)[-n:]++ 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)"),+ ("firmware", "battery", "11.62 V (INA219)"), ("lidar", "C1 spins", "1480 pts/s, 36/36 sectors"),+ ("camera", "RealSense D455 on USB 3", "demo"), ("camera", "wrist camera", "demo"),+ ("gamepad", "gamepad js0", "present"), ("system", "free space", "79 GB"),+ ("system", "memory available", "3.1 GB"), ("software", "entry points", "moss-nav, moss-explore")]+ rows = [{"ok": True, "label": label, "detail": detail, "section": section}+ for section, label, detail in checks]+ return {"available": True, "demo": True, "ts": self._preflight_ts, "checks": rows,+ "ok_count": len(rows), "ko_count": 0, "summary": f"{len(rows)}/{len(rows)} OK - ready to fly"}++ def launch(self) -> dict:+ return {"running": False, "action": self._launch_action, "started_at": None, "returncode": 0,+ "lines": list(self._launch_lines)}++ def camera_frame(self, cam: str = "nose"):+ # a new key every period, so the stream really runs at 5 fps; both demo cameras+ # show the demo frame+ return int(time.time() / MJPEG_PERIOD_S), DEMO_JPEG++ def command(self, name: str, body: dict) -> dict:+ self._tick()+ with self._lock:+ w = self.world+ if name == "explore":+ action = body.get("action")+ if action not in ("start", "stop"):+ raise ValueError('explore needs "action": "start" or "stop"')+ 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'}")+ return {"ok": True, "sent": "/explore_cmd#std_msgs.Bool" if self.exploring+ else "/stop_explore_cmd#std_msgs.Bool"}+ if name == "goal":+ x, y = _finite_xy(body)+ self.exploring = False+ 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})")+ 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")+ 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":+ marker = self._episode(body)+ self._say("info", f"demo: episode {marker['action']} {marker['name']}")+ return {"ok": True, "sent": "/episode_marker#std_msgs.String", "episode": self.episode}+ if name == "stack":+ action = body.get("action")+ if action not in ("map", "explore", "stop", "check"):+ raise ValueError('stack needs "action": "map", "explore", "stop" or "check"')+ now = time.time()+ self._launch_action = action+ self._launch_lines.append(f"=== moss-fly.sh {action} {time.strftime(_ISO)} === (demo)")+ if action in ("map", "explore"):+ if self.stack["running"]:+ return {"ok": False, "error": "a dimOS stack is already running: stop it first"}+ self.stack = {"running": True, "started": now,+ "blueprint": "moss-nav" if action == "map" else "moss-explore"}+ self.exploring, self.halted = False, False+ elif action == "stop":+ self.stack = dict(self.stack, running=False)+ self.stack_stopped_at = now+ self.exploring = False+ else:+ self._preflight_ts = now+ self._say("info", f"demo: moss-fly.sh {action}")+ return {"ok": True, "sent": f"moss-fly.sh {action}"}+ return {"ok": False, "error": f"unknown command {name!r}", "_code": 404}+++# --- HTTP ---------------------------------------------------------------------------++CONTENT_TYPES = {+ ".html": "text/html; charset=utf-8", ".css": "text/css; charset=utf-8",+ ".js": "text/javascript; charset=utf-8", ".json": "application/json; charset=utf-8",+ ".svg": "image/svg+xml", ".png": "image/png", ".jpg": "image/jpeg", ".jpeg": "image/jpeg",+ ".ttf": "font/ttf", ".otf": "font/otf", ".woff": "font/woff", ".woff2": "font/woff2",+ ".txt": "text/plain; charset=utf-8", ".ico": "image/x-icon",+}+PLACEHOLDER_PAGE = (b"<!doctype html><meta charset=utf-8><title>MOSS cockpit</title>"+ b"<p>MOSS cockpit server is up; the page (tools/moss_cockpit/index.html) is not installed."+ b" Try <a href=/state.json>/state.json</a>.</p>")+++def json_safe(obj):+ """NaN/inf -> null (JSON.parse rejects NaN; a NaN goal is how dimOS cancels one),+ numpy scalars -> Python numbers."""+ if isinstance(obj, dict):+ return {str(k): json_safe(v) for k, v in obj.items()}+ if isinstance(obj, (list, tuple)):+ return [json_safe(v) for v in obj]+ if isinstance(obj, np.generic):+ obj = obj.item()+ if isinstance(obj, float) and not math.isfinite(obj):+ return None+ return obj+++class Handler(BaseHTTPRequestHandler):+ server_version = "moss-cockpit/1.0"+ protocol_version = "HTTP/1.1"++ @property+ def cockpit(self) -> Cockpit:+ return self.server.cockpit++ def _send(self, code: int, body: bytes, ctype: str) -> None:+ self.send_response(code)+ self.send_header("Content-Type", ctype)+ self.send_header("Content-Length", str(len(body)))+ self.send_header("Cache-Control", "no-store")+ self.end_headers()+ if self.command != "HEAD":+ self.wfile.write(body)++ def _json(self, code: int, payload) -> None:+ self._send(code, json.dumps(json_safe(payload), allow_nan=False).encode("utf-8"),+ "application/json; charset=utf-8")++ def _static(self, rel: str) -> None:+ root = os.path.realpath(self.server.page_dir)+ full = os.path.realpath(os.path.join(root, unquote(rel)))+ if not full.startswith(root + os.sep) or not os.path.isfile(full):+ self._json(404, {"error": "nothing at this address"})+ return+ ctype = CONTENT_TYPES.get(os.path.splitext(full)[1].lower(), "application/octet-stream")+ with open(full, "rb") as fh:+ self._send(200, fh.read(), ctype)++ def do_GET(self) -> None: # noqa: N802+ url = urlsplit(self.path)+ path = url.path+ try:+ if path in ("/", "/index.html"):+ index = os.path.join(self.server.page_dir, "index.html")+ if os.path.isfile(index):+ with open(index, "rb") as fh:+ self._send(200, fh.read(), "text/html; charset=utf-8")+ else:+ self._send(200, PLACEHOLDER_PAGE, "text/html; charset=utf-8")+ elif path.startswith("/static/"):+ self._static(path[len("/static/"):])+ elif path == "/state.json":+ self._json(200, self.cockpit.state())+ elif path in ("/map.png", "/map_meta"):+ got = self.cockpit.map()+ if got is None:+ self._json(404, {"error": "no costmap yet: is a mapping stack running?"})+ elif path == "/map.png":+ self._send(200, got[0], "image/png")+ else:+ self._json(200, got[1])+ elif path == "/scan.json":+ doc = self.cockpit.scan()+ self._json(200, doc if doc is not None else {"frame": None, "stamp": None, "points": []})+ elif path == "/path.json":+ self._json(200, self.cockpit.path())+ elif path == "/log.json":+ try:+ n = int(parse_qs(url.query).get("n", ["40"])[0])+ except ValueError:+ n = 40+ self._json(200, {"lines": self.cockpit.log(max(1, min(n, 500)))})+ elif path == "/preflight.json":+ self._json(200, self.cockpit.preflight())+ elif path == "/launch.json":+ self._json(200, self.cockpit.launch())+ elif path == "/camera.mjpg":+ cam = parse_qs(url.query).get("cam", ["nose"])[0]+ if cam not in CAMERAS:+ self._json(404, {"error": f"no camera called {cam!r}: " + " or ".join(CAMERAS)})+ else:+ self._mjpeg(cam)+ else:+ self._json(404, {"error": f"nothing at this address: {path}"})+ except (BrokenPipeError, ConnectionResetError):+ pass+ except Exception as exc: # noqa: BLE001 - a page reload must never kill the server+ self.log_error("GET %s failed: %r", path, exc)+ try:+ self._json(500, {"error": f"{type(exc).__name__}: {exc}"})+ except OSError:+ pass++ do_HEAD = do_GET++ def _peer_gone(self) -> bool:+ """The viewer closed its socket. Checked while no frame is due: with a silent+ camera nothing is written, so a write error would never tell us, and the+ camera would stay subscribed for a viewer long gone."""+ try:+ readable, _, _ = select.select([self.connection], [], [], 0)+ return bool(readable) and self.connection.recv(1, socket.MSG_PEEK) == b""+ except (OSError, ValueError):+ return True++ def _mjpeg(self, cam: str = "nose") -> None:+ c = self.cockpit+ try:+ c.camera_open(cam)+ except CameraUnavailable as exc:+ self._json(503, {"error": str(exc)})+ return+ try:+ # a viewer that vanished without FIN/RST (phone off the WiFi, laptop asleep) blocks+ # the write once the buffers fill: time out and free its slot instead of waiting for TCP+ self.connection.settimeout(MJPEG_WRITE_TIMEOUT_S)+ frame = None+ deadline = time.monotonic() + FIRST_FRAME_WAIT_S+ try:+ while frame is None and time.monotonic() < deadline and not self.server.stopping:+ frame = c.camera_frame(cam)+ if frame is None:+ time.sleep(0.05)+ except CameraUnavailable as exc:+ self._json(503, {"error": str(exc)})+ return+ self.send_response(200)+ self.send_header("Content-Type", "multipart/x-mixed-replace; boundary=frame")+ self.send_header("Cache-Control", "no-store")+ self.send_header("Connection", "close")+ self.end_headers()+ self.close_connection = True+ if self.command == "HEAD":+ return+ sent_key, next_at = None, 0.0+ while not self.server.stopping:+ if frame is None or frame[0] == sent_key or time.monotonic() < next_at:+ if self._peer_gone():+ return+ time.sleep(0.03)+ try:+ frame = c.camera_frame(cam)+ except CameraUnavailable:+ return+ continue+ key, jpeg = frame+ self.wfile.write(b"--frame\r\nContent-Type: image/jpeg\r\nContent-Length: "+ + str(len(jpeg)).encode() + b"\r\n\r\n" + jpeg + b"\r\n")+ self.wfile.flush()+ sent_key, next_at = key, time.monotonic() + MJPEG_PERIOD_S+ except (BrokenPipeError, ConnectionResetError, OSError):+ pass # the viewer left+ finally:+ c.camera_close(cam)++ def do_POST(self) -> None: # noqa: N802+ path = urlsplit(self.path).path+ if not path.startswith("/cmd/"):+ self._json(404, {"ok": False, "error": f"nothing at this address: {path}"})+ return+ name = path[len("/cmd/"):]+ if self.headers.get(CMD_HEADER[0]) != CMD_HEADER[1]:+ # a cross-site page cannot set this header without a CORS preflight, which this+ # server never grants (no OPTIONS): only the cockpit page and deliberate scripts reach here+ self.close_connection = True # the body is not read: do not reuse the connection+ self._json(403, {"ok": False, "error": f"refused: POST /cmd/* needs the header "+ f"'{CMD_HEADER[0]}: {CMD_HEADER[1]}'"})+ return+ try:+ length = int(self.headers.get("Content-Length") or "0")+ if length < 0 or length > MAX_BODY:+ raise ValueError(f"body of {length} bytes refused")+ raw = self.rfile.read(length) if length else b""+ body = json.loads(raw.decode("utf-8")) if raw.strip() else {}+ if not isinstance(body, dict):+ raise ValueError("the body must be a JSON object")+ answer = self.cockpit.command(name, body)+ code = answer.pop("_code", 200)+ self._json(code, answer)+ except ValueError as exc: # includes a JSON decode error+ self._json(400, {"ok": False, "error": str(exc)})+ except Exception as exc: # noqa: BLE001+ self.log_error("POST %s failed: %r", path, exc)+ self._json(500, {"ok": False, "error": f"{type(exc).__name__}: {exc}"})++ def log_message(self, *args) -> None:+ pass # quiet: errors still go through log_error+++class CockpitServer(ThreadingHTTPServer):+ daemon_threads = True # an open MJPEG stream must not block exit+ allow_reuse_address = True++ def __init__(self, addr, cockpit: Cockpit, page_dir: str = PAGE_DIR):+ super().__init__(addr, Handler)+ self.cockpit = cockpit+ self.page_dir = page_dir+ self.stopping = False++ def shutdown(self) -> None:+ self.stopping = True+ super().shutdown()+++def make_server(bind: str = DEFAULT_BIND, port: int = DEFAULT_PORT, demo: bool = False,+ page_dir: str = PAGE_DIR, fly_script: str = FLY_SCRIPT, listen: bool = True) -> CockpitServer:+ """The server, built but not serving (port 0 = any free port: the bench uses that)."""+ cockpit = DemoCockpit(fly_script) if demo else LiveCockpit(fly_script)+ if not demo and listen:+ cockpit.start_listener()+ if any(os.path.exists(p) for p in GPU_LOAD_PATHS):+ threading.Thread(target=_gpu_sampler, name="gpu-sampler", daemon=True).start()+ return CockpitServer((bind, port), cockpit, page_dir)+++def main(argv=None) -> int:+ ap = argparse.ArgumentParser(description="MOSS cockpit web server")+ ap.add_argument("--port", type=int, default=int(os.environ.get("MOSS_COCKPIT_PORT", DEFAULT_PORT)))+ ap.add_argument("--bind", default=DEFAULT_BIND)+ ap.add_argument("--demo", action="store_true", help="synthetic rover: no bus, no stack, no launcher")+ args = ap.parse_args(argv)+ server = make_server(args.bind, args.port, demo=args.demo)+ cockpit = server.cockpit+ if not args.demo and isinstance(cockpit, LiveCockpit) and cockpit.bus_error:+ print(f"warning: {cockpit.bus_error} - the page will show no live data", flush=True)+ print(f"MOSS cockpit ({cockpit.mode}) on http://{args.bind}:{server.server_address[1]}/", flush=True)+ try:+ server.serve_forever()+ except KeyboardInterrupt:+ pass+ return 0+++if __name__ == "__main__":+ raise SystemExit(main())diff --git a/tools/moss_cockpit/app.js b/tools/moss_cockpit/app.jsnew file mode 100644index 0000000..7d1894b--- /dev/null+++ b/tools/moss_cockpit/app.js@@ -0,0 +1,914 @@+/* MOSS cockpit page.+ *+ * Talks only to the server that served it (relative paths, no other host: the+ * field has no internet). Contract: tools/moss_cockpit.py.+ *+ * GET /state.json 1 Hz GET /scan.json 3 Hz GET /path.json 1 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 /launch.json while a stack command runs+ * POST /cmd/<explore|goal|stop|episode|stack>+ *+ * Each poller waits for its answer before scheduling the next one, so a slow+ * Jetson never gets a queue of requests. Pollers sleep while the tab is hidden.+ */+"use strict";++var POLL = { state: 1000, meta: 1000, scan: 333, path: 1000, log: 2000, launch: 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+var LIVE_S = 2; // telemetry, scan and path older than this are not drawn or read as live+var EP_FULL_S = 300; // the bottom progress line is full after 5 min of episode+var RUN_FULL_S = 1800; // ... or after 30 min of run when no episode records+var CAM_RETRY_MS = 3000; // reconnect the MJPEG stream this long after it ended or failed, forever+var CAM_GRACE_MS = 6000; // a stream younger than this is not judged dead (first frame wait + a state poll)++var S = {+ online: false, state: null,+ meta: null, mapImg: null, mapStamp: null, scan: null, path: null,+ 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+};++function $(id) { return document.getElementById(id); }++// ---------------------------------------------------------------- utilities++function fetchJSON(url, opts) {+ var ctl = ("AbortController" in window) ? new AbortController() : null;+ var t = ctl ? setTimeout(function () { ctl.abort(); }, FETCH_TIMEOUT_MS) : null;+ var o = opts || {};+ if (ctl) o.signal = ctl.signal;+ o.cache = "no-store";+ return fetch(url, o).then(function (r) {+ if (t) clearTimeout(t);+ if (r.status === 404) return { __missing: true };+ return r.json().then(function (d) {+ if (!r.ok && d && typeof d === "object" && !("ok" in d)) d.ok = false;+ return d;+ });+ }, function (e) { if (t) clearTimeout(t); throw e; });+}++function post(name, body) {+ return fetchJSON("/cmd/" + name, {+ method: "POST", headers: { "Content-Type": "application/json", "X-Requested-With": "moss-cockpit" },+ body: JSON.stringify(body || {})+ });+}++function poller(fn, period) {+ function tick() {+ if (document.hidden) { setTimeout(tick, period); return; }+ var p;+ try { p = fn(); } catch (e) { p = null; }+ Promise.resolve(p).catch(function () {}).then(function () { setTimeout(tick, period); });+ }+ tick();+}++function num(v, d) { return (typeof v === "number" && isFinite(v)) ? v.toFixed(d) : "--"; }+function signed(v, d) { return (typeof v === "number" && isFinite(v)) ? ((v >= 0 ? "+" : "") + v.toFixed(d)) : "--"; }+function pad2(n) { return (n < 10 ? "0" : "") + n; }+function clock(s) {+ if (typeof s !== "number" || !isFinite(s) || s < 0) return "--:--";+ s = Math.floor(s);+ var h = Math.floor(s / 3600), m = Math.floor((s % 3600) / 60), sec = s % 60;+ return (h ? h + ":" + pad2(m) : m) + ":" + pad2(sec);+}+function span(s) { // long durations (machine uptime): days and hours read better than 254:08:52+ if (typeof s !== "number" || !isFinite(s) || s < 0) return "--";+ if (s < 86400) return clock(s);+ return Math.floor(s / 86400) + " d " + Math.floor((s % 86400) / 3600) + " h";+}+function dur(s) {+ if (typeof s !== "number" || !isFinite(s)) return "--";+ if (s < 90) return Math.round(s) + " s";+ if (s < 5400) return Math.round(s / 60) + " min";+ return Math.round(s / 3600) + " h";+}+function ago(s) {+ if (typeof s !== "number" || !isFinite(s)) return "--";+ return s < 1 ? "now" : dur(s) + " ago";+}+function setText(id, t) { var el = $(id); if (el && el.textContent !== t) el.textContent = t; }+function meter(id, frac, attn) {+ var el = $(id); if (!el) return;+ var f = (typeof frac === "number" && isFinite(frac)) ? Math.max(0, Math.min(1, frac)) : 0;+ el.firstElementChild.style.width = (f * 100).toFixed(1) + "%";+ el.classList.toggle("attn", !!attn);+}++var sayTimer = null;+function say(text, bad) {+ var el = $("say");+ el.textContent = text;+ el.className = bad ? "err" : "ok";+ clearTimeout(sayTimer);+ // a success line fades after a while; an error stays until the next action+ if (!bad) sayTimer = setTimeout(function () { if (el.textContent === text) el.textContent = ""; }, 8000);+}++function answer(d, okText) {+ if (d && d.ok) { say(okText || ("Sent on " + (d.sent || "the bus") + "."), false); return true; }+ say((d && d.error) ? d.error : "The cockpit refused the command.", true);+ return false;+}++// ---------------------------------------------------------------- state++function stack() { return (S.state && S.state.stack) || { running: false }; }+function blueprint() { var b = stack().blueprint || ""; return String(b); }+function isExploreStack() { return stack().running && /explore/.test(blueprint()); }+function isDemo() { return !!(S.state && S.state.mode === "demo"); }+function canGoal() { return S.online && (isExploreStack() || isDemo()); }+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); }++// 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];+ return t && typeof t.age_s === "number" ? t.age_s + offlineFor() : null;+}++function pollState() {+ return fetchJSON("/state.json").then(function (d) {+ if (!d || d.__missing) throw new Error("no state");+ var wasOffline = !S.online && S.stateAt > 0;+ S.state = d; S.online = true; S.stateAt = Date.now();+ if (wasOffline) camRestart("Cockpit back. Reconnecting the stream."); // its stream died with it+ render();+ }, function () {+ S.online = false;+ render();+ });+}++function render() {+ document.body.classList.toggle("offline", !S.online);+ renderHeader();+ renderBase();+ renderSystem();+ renderControls();+ renderCaption();+ renderCamera();+ renderPreflightWhen();+ requestDraw();+}++function renderHeader() {+ var st = stack(), chip = $("chip"), cls, label;+ if (!S.online) { cls = "off"; label = "OFFLINE"; }+ else if (st.running && exploring()) { cls = "on"; label = "EXPLORING"; }+ else if (st.running) { cls = "on"; label = "MAPPING"; }+ else { cls = "idle"; label = "IDLE"; }+ chip.className = "chip " + cls;+ setText("chip", label);+ var name = st.running ? (st.run_id || st.blueprint || "stack") : "no stack";+ if (isDemo()) name += " · demo data";+ setText("run-name", name);+}++function modeName(b) {+ if (b.mode_name) return b.mode_name;+ return ({ 0: "off", 1: "ready", 2: "drive", 3: "test" })[b.mode] || "--";+}++function setTrack(barId, valId, v) {+ var bar = $(barId);+ var p = (typeof v === "number" && isFinite(v)) ? Math.max(-100, Math.min(100, v)) : 0;+ var h = Math.abs(p) / 2; // 100 % fills half the bar+ bar.style.height = h + "%";+ bar.style.bottom = p >= 0 ? "50%" : (50 - h) + "%";+ bar.classList.toggle("rev", p < 0);+ setText(valId, (typeof v === "number") ? String(Math.round(v)) : "--");+}++function renderBase() {+ var b = S.state && S.state.base;+ var sec = $("mod-base");+ if (!b) {+ setText("base-title", S.online ? "No telemetry." : "Cockpit offline.");+ setText("b-volt", "--"); setText("b-pct", "--"); meter("b-meter", 0);+ setTrack("t-l", "t-lv", null); setTrack("t-r", "t-rv", null);+ ["b-mode", "b-ticks", "b-fw", "b-age"].forEach(function (id) { setText(id, "--"); });+ setText("t-target", "target -- / --");+ return;+ }+ var low = typeof b.battery_v === "number" && b.battery_v < 10.8;+ // age of the data = time since the cockpit got the message + its age when it was sent+ var age = (typeof b.age_s === "number" ? b.age_s : 0) + (typeof b.firmware_age_s === "number" ? b.firmware_age_s : 0)+ + offlineFor();+ var stale = age > LIVE_S;+ var title;+ if (stale) title = "No telemetry for " + dur(age) + ".";+ else if (b.ina_ok === false) title = "Battery sensor not seen.";+ else if (low) title = "Battery low. Land it.";+ else if (b.mode === 2) title = "Driving.";+ else if (b.mode === 1) title = "Armed, ready.";+ else title = "Disarmed.";+ if (b.mock && !stale) title = "Fake firmware. " + title;+ setText("base-title", title);+ $("base-title").classList.toggle("attn", stale || low || b.ina_ok === false);+ setText("b-volt", num(b.battery_v, 2));+ setText("b-pct", typeof b.battery_pct === "number" ? String(Math.round(b.battery_pct)) : "--");+ $("b-volt").parentNode.classList.toggle("attn", low);+ meter("b-meter", (b.battery_pct || 0) / 100, low);+ var o = b.output_pct || [], t = b.target_pct || [];+ setTrack("t-l", "t-lv", o[0]); setTrack("t-r", "t-rv", o[1]);+ setText("t-target", "target " + (t.length ? Math.round(t[0]) + " / " + Math.round(t[1]) : "-- / --"));+ setText("b-mode", modeName(b) + (b.reason && b.reason !== modeName(b) ? " · " + b.reason : ""));+ setText("b-ticks", b.ticks ? b.ticks[0] + " / " + b.ticks[1] : "--");+ setText("b-fw", (b.fw || "--") + (b.driver ? " · " + b.driver : ""));+ setText("b-age", stale ? "stale · " + ago(age) : ago(age));+ sec.classList.toggle("stale", stale);+}++var TOPIC_ORDER = ["odom", "lidar", "global_costmap", "path", "cmd_vel", "base_telemetry", "color_image", "wrist_image"];++function renderSystem() {+ var s = S.state && S.state.system;+ if (s) {+ setText("s-cpu", num(s.cpu_percent, 0) + " %");+ setText("s-ram", num(s.ram_used_gb, 1) + " / " + num(s.ram_total_gb, 1) + " GB");+ var tp = s.temps || {}, parts = [];+ if (typeof tp.cpu === "number") parts.push("CPU " + tp.cpu.toFixed(0) + "°");+ if (typeof tp.gpu === "number") parts.push("GPU " + tp.gpu.toFixed(0) + "°");+ setText("s-temp", parts.length ? parts.join(" · ") : "--");+ setText("s-load", num(s.load_1m, 2));+ setText("s-up", span(s.uptime_s));+ meter("s-cpu-m", (s.cpu_percent || 0) / 100, s.cpu_percent > 90);+ meter("s-ram-m", (s.ram_percent || 0) / 100, s.ram_percent > 90);+ var dl = $("s-disks"), html = "";+ (s.disks || []).forEach(function (d) {+ html += "<dt>Disk " + esc(d.mountpoint) + "</dt><dd>" + num(d.free_gb, 0) + " GB free</dd>";+ });+ if (dl.innerHTML !== html) dl.innerHTML = html;+ setText("sys-title", s.cpu_percent > 90 || s.ram_percent > 90 ? "Jetson under load." : "Jetson.");+ }+ var topics = (S.state && S.state.topics) || {}, rows = "";+ var names = TOPIC_ORDER.filter(function (n) { return n in topics; });+ Object.keys(topics).forEach(function (n) { if (names.indexOf(n) < 0) names.push(n); });+ names.forEach(function (n) {+ var t = topics[n] || {}, fresh = typeof t.age_s === "number" && t.age_s <= STALE_S;+ rows += '<span class="n">' + esc(n) + '</span><span class="r ' + (fresh ? "" : "dim") + '">'+ + (typeof t.hz === "number" ? t.hz.toFixed(1) + " Hz" : "--") + '</span><span class="r '+ + (fresh ? "" : "attn") + '">' + (typeof t.age_s === "number" ? ago(t.age_s) : "silent") + "</span>";+ });+ if (!names.length) rows = '<span class="n">no topic yet</span><span></span><span></span>';+ if ($("bus").innerHTML !== rows) $("bus").innerHTML = rows;+}++function esc(s) {+ return String(s == null ? "" : s).replace(/[&<>"]/g, function (c) {+ return { "&": "&", "<": "<", ">": ">", '"': """ }[c];+ });+}++function renderControls() {+ var st = stack(), on = S.online, busy = S.launchBusy;+ $("b-map").disabled = !on || st.running || busy;+ $("b-explore").disabled = !on || st.running || busy;+ $("b-stack-stop").disabled = !on || (!st.running && !busy);+ var exOk = on && (isExploreStack() || isDemo());+ $("b-ex-start").disabled = !exOk || exploring();+ $("b-ex-stop").disabled = !exOk || !exploring();+ if (!exOk) $("ex-confirm").hidden = true;+ setText("ex-note", exOk ? (exploring() ? "Exploring. A pad stick or Stop exploration takes over."+ : "Armed only by this button or EXPLORE=1 moss-fly.sh explore.")+ : "Needs the explore stack (Launch explore).");+ var rec = (S.state && S.state.recording) || {}, ep = rec.episode;+ var canRec = on && (st.running || isDemo());+ $("b-ep").disabled = !canRec;+ $("b-ep").textContent = ep ? "Stop" : "Start";+ $("b-ep").classList.toggle("primary", !ep && canRec);+ $("ep-name").disabled = !!ep;+ if (ep && document.activeElement !== $("ep-name")) $("ep-name").value = ep.name || "";+ var note;+ if (ep) note = "Recording " + ep.name + " · " + clock(epElapsed(ep));+ else if (rec.exists) note = "Run database " + num(rec.size_mb, 1) + " MB. An episode adds markers to it.";+ else note = "The run records to its database while the stack is up.";+ setText("rec-note", note);+ var title;+ if (!on) title = "Cockpit offline.";+ else if (st.running && exploring()) title = "Exploring on its own.";+ else if (st.running) title = "Stack up. Nothing moves by itself.";+ else title = "Nothing moves by itself.";+ setText("ctl-title", title);+}++function epElapsed(ep) {+ if (!ep || ep.started_at == null) return null;+ var t0 = typeof ep.started_at === "number" ? ep.started_at : Date.parse(ep.started_at) / 1000;+ var now = (S.state && S.state.ts) || Date.now() / 1000;+ return isFinite(t0) ? Math.max(0, now - t0) : null;+}++function renderCaption() {+ var st = stack(), nav = (S.state && S.state.nav) || {};+ var bp = (st.blueprint || "none").replace(/^[a-z0-9-]+\./, "");+ setText("cap-kicker", "RUN / " + bp.toUpperCase());+ var mins = st.running && typeof st.uptime_s === "number" ? Math.floor(st.uptime_s / 60) : 0;+ setText("cap-num", pad2(mins));+ var title, sub = "";+ if (!S.online) title = "Cockpit offline. Last values shown.";+ else if (!st.running) title = isDemo() ? "Demo data. Nothing is connected." : "No stack running.";+ else if (exploring()) title = "Exploring on its own.";+ else if (nav.goal && nav.goal_reached !== true) title = "Heading to a goal.";+ else if (/explore/.test(bp)) title = "Mapping. Exploration not armed.";+ else title = "Mapping. Drive with the pad.";+ if (nav.explore_done) sub = "Exploration finished: no frontier left.";+ else if (nav.goal) sub = "Goal " + signed(nav.goal.x, 2) + ", " + signed(nav.goal.y, 2) + " m"+ + (nav.goal_reached === true ? " · reached" : nav.goal_reached === false ? " · not reached" : "");+ var c = nav.last_cmd_vel;+ if (c && typeof c.age_s === "number" && c.age_s < 2) sub += (sub ? " · " : "") + "cmd " + num(c.vx, 2) + " m/s " + num(c.wz, 2) + " rad/s";+ setText("cap-title", title);+ setText("cap-sub", sub);+ setText("map-title", S.meta ? (st.running ? "Live costmap." : "Last costmap.") : "Waiting for a map.");+ // the costmap writes nothing until its mission gate opens: explore_cmd, or 5 cm from the boot pose+ setText("map-empty-why", !S.online ? "No map yet. The cockpit is offline."+ : !st.running ? "No map yet. Launch a stack to build one."+ : /explore/.test(bp) ? "No map yet. It opens when exploration starts or the rover moves 5 cm."+ : "No map yet. It opens once the rover has moved 5 cm.");++ var rec = (S.state && S.state.recording) || {}, ep = rec.episode, frac, left, right;+ if (ep) {+ var e = epElapsed(ep) || 0;+ frac = (e % EP_FULL_S) / EP_FULL_S;+ left = "EPISODE / " + String(ep.name || "").toUpperCase();+ right = clock(e);+ } else if (st.running) {+ frac = Math.min(1, (st.uptime_s || 0) / RUN_FULL_S);+ left = "no episode recording";+ right = "run " + clock(st.uptime_s);+ } else { frac = 0; left = "no episode"; right = ""; }+ $("cap-progress").style.width = (frac * 100).toFixed(2) + "%";+ setText("cap-left", left);+ setText("cap-right", right);+}++// ---------------------------------------------------------------- cameras++// Two MJPEG streams under the map: the nose (RealSense colour) and the wrist+// (the arm's camera). Each has its own state; one "Pause video" covers both.+var CAMS = {+ nose: { img: "cam-img", ph: "cam-ph", why: "cam-why", topic: "color_image", src: "/camera.mjpg?cam=nose" },+ wrist: { img: "wrist-img", ph: "wrist-ph", why: "wrist-why", topic: "wrist_image", src: "/camera.mjpg?cam=wrist" }+};+var camState = {};+Object.keys(CAMS).forEach(function (n) { camState[n] = { ok: false, at: 0, lost: null, timer: null, wasSilent: true }; });++function camSilent(n) {+ var t = S.state && S.state.topics && S.state.topics[CAMS[n].topic];+ return !(t && typeof t.age_s === "number" && t.age_s <= STALE_S);+}++function camConnect(n) {+ var c = CAMS[n], st = camState[n], img = $(c.img);+ if (S.camPaused || document.hidden) { img.removeAttribute("src"); st.ok = false; return; }+ if (!img.getAttribute("src")) { st.at = Date.now(); img.src = c.src + "&t=" + Date.now(); }+}+function camConnectAll() { Object.keys(CAMS).forEach(camConnect); }++// Drop the stream and ask again, with a fresh URL, CAM_RETRY_MS later. An MJPEG+// stream the server closes cleanly fires no error (the browser holds the last+// frame as a finished image), so this also runs when the state says it is gone.+function camRestart(n, why) {+ var st = camState[n];+ $(CAMS[n].img).removeAttribute("src");+ st.ok = false; st.lost = why;+ if (!st.timer) st.timer = setTimeout(function () { st.timer = null; camConnect(n); renderCamera(); }, CAM_RETRY_MS);+}++function camWatch(n) {+ var st = camState[n];+ if (S.camPaused || document.hidden || !S.online || st.timer) return;+ if (!$(CAMS[n].img).getAttribute("src")) { camConnect(n); return; }+ var silent = camSilent(n) && !isDemo();+ if (Date.now() - st.at < CAM_GRACE_MS) { st.wasSilent = silent; return; }+ // the topic is back after a silence (a stack restart): the stream we hold belongs to+ // the old run and may never show a new frame, ask for a fresh one+ if (st.wasSilent && !silent) { st.wasSilent = false; camRestart(n, "The camera is back. Reconnecting."); return; }+ st.wasSilent = silent;+ var cam = S.state && S.state.camera && S.state.camera[n];+ if (cam && cam.clients === 0) camRestart(n, "The stream ended. Reconnecting."); // the server holds no stream: ours is gone+ else if (st.ok && silent) camRestart(n, "The camera topic went silent. Reconnecting.");+}++function renderCamera() {+ Object.keys(CAMS).forEach(function (n) {+ camWatch(n);+ var st = camState[n], ph = $(CAMS[n].ph), why;+ if (S.camPaused) why = "Video paused in this browser.";+ else if (!S.online) why = "Cockpit offline.";+ else if (!st.ok && st.timer) why = st.lost || "Reconnecting.";+ else if (!st.ok && camSilent(n) && !isDemo() && Date.now() - st.at >= CAM_GRACE_MS) why = "The camera topic is silent. Waiting for frames.";+ else if (!st.ok) why = "Connecting to the stream.";+ else if (camSilent(n) && !isDemo()) why = "The camera topic is silent.";+ ph.hidden = !why;+ if (why) setText(CAMS[n].why, why);+ });+ setText("b-cam", S.camPaused ? "Resume video" : "Pause video");+}++(function () {+ Object.keys(CAMS).forEach(function (n) {+ var img = $(CAMS[n].img), st = camState[n];+ img.addEventListener("load", function () { st.ok = true; st.lost = null; renderCamera(); });+ img.addEventListener("error", function () {+ if (!img.getAttribute("src")) return; // our own removeAttribute, not a failure+ camRestart(n, "No stream from the cockpit. Retrying every " + CAM_RETRY_MS / 1000 + " s."); renderCamera();+ });+ });+ try { S.camPaused = localStorage.getItem("moss.cockpit.camPaused") === "1"; } catch (e) {}+ $("b-cam").addEventListener("click", function () {+ S.camPaused = !S.camPaused;+ try { localStorage.setItem("moss.cockpit.camPaused", S.camPaused ? "1" : "0"); } catch (e) {}+ camConnectAll(); renderCamera();+ });+ // MJPEG is the heaviest thing the Jetson serves: drop it while nobody looks.+ document.addEventListener("visibilitychange", camConnectAll);+})();++// ---------------------------------------------------------------- map++var cv = $("map"), ctx = cv.getContext("2d"), wrap = $("map-wrap");+var drawQueued = false;++function sx(x) { return (x - S.view.x) * S.view.scale; }+function sy(y) { return (S.view.y - y) * S.view.scale; }+function wx(px) { return S.view.x + px / S.view.scale; }+function wy(py) { return S.view.y - py / S.view.scale; }+function W() { return cv.clientWidth; }+function H() { return cv.clientHeight; }++function resize() {+ var r = window.devicePixelRatio || 1;+ cv.width = Math.max(1, Math.round(W() * r));+ cv.height = Math.max(1, Math.round(H() * r));+ ctx.setTransform(r, 0, 0, r, 0, 0);+ requestDraw();+}++function fitView() {+ var m = S.meta; if (!m) return;+ var w = m.width * m.res, h = m.height * m.res;+ // leave the lower third to the caption: fit into the top 74 % of the pane+ var avH = H() * 0.74, s = Math.min(W() / w, avH / h) * 0.96;+ if (!isFinite(s) || s <= 0) return;+ S.view.scale = s;+ S.view.x = m.ox - (W() / s - w) / 2;+ S.view.y = m.oy + h + (avH / s - h) / 2;+ S.fitted = true;+ requestDraw();+}++function requestDraw() {+ if (drawQueued) return;+ drawQueued = true;+ requestAnimationFrame(function () { drawQueued = false; draw(); });+}++function drawGrid() {+ // 1 m grid, only over the map itself: outside it the pane stays bare studio+ var m = S.meta;+ if (S.view.scale < 14 || !m) return;+ var x0 = Math.ceil(Math.max(wx(0), m.ox)), x1 = Math.floor(Math.min(wx(W()), m.ox + m.width * m.res));+ var y0 = Math.ceil(Math.max(wy(H()), m.oy)), y1 = Math.floor(Math.min(wy(0), m.oy + m.height * m.res));+ var gx0 = sx(m.ox), gx1 = sx(m.ox + m.width * m.res), gy0 = sy(m.oy + m.height * m.res), gy1 = sy(m.oy);+ ctx.strokeStyle = "rgba(234,240,239,.07)";+ ctx.lineWidth = 1;+ ctx.beginPath();+ for (var x = x0; x <= x1; x++) { var X = Math.round(sx(x)) + .5; ctx.moveTo(X, gy0); ctx.lineTo(X, gy1); }+ for (var y = y0; y <= y1; y++) { var Y = Math.round(sy(y)) + .5; ctx.moveTo(gx0, Y); ctx.lineTo(gx1, Y); }+ ctx.stroke();+}++function pose() { return S.state && S.state.pose; }++function scanWorld() {+ var sc = S.scan; if (!sc || !sc.points) return [];+ if (sc.frame === "world") return sc.points;+ var p = pose(); // lidar frame: place it at the robot+ var px = p ? p.x : 0, py = p ? p.y : 0, c = Math.cos(p ? p.yaw : 0), s = Math.sin(p ? p.yaw : 0);+ return sc.points.map(function (q) { return [px + c * q[0] - s * q[1], py + s * q[0] + c * q[1]]; });+}++// The server keeps the last scan and path however old they are: only fresh ones+// are drawn as live. The scan's own age_s is cached with it, so the bus age wins.+function scanLive() {+ var sc = S.scan; if (!sc) return false;+ var topics = (S.state && S.state.topics) || {};+ var a = sc.source in topics ? topicAge(sc.source)+ : (typeof sc.age_s === "number" ? sc.age_s + offlineFor() : null);+ return a !== null && a < LIVE_S;+}+// One /path message per plan, not a stream: a plan stays valid while the stack runs.+function pathLive() {+ var pa = S.path, a = pa && typeof pa.age_s === "number" ? pa.age_s + offlineFor() : null;+ if (a !== null && a < LIVE_S) return true;+ var odom = topicAge("odom");+ return stack().running && odom !== null && odom < LIVE_S;+}++function draw() {+ var w = W(), h = H();+ ctx.clearRect(0, 0, w, h);+ var m = S.meta;+ if (S.mapImg && m) {+ ctx.imageSmoothingEnabled = false;+ ctx.drawImage(S.mapImg, sx(m.ox), sy(m.oy + m.height * m.res), m.width * m.res * S.view.scale, m.height * m.res * S.view.scale);+ }+ drawGrid();++ // path: mint line, dimmed once the stack that planned it is gone+ var pts = S.path && S.path.points;+ if (pts && pts.length > 1) {+ ctx.strokeStyle = pathLive() ? "#6CF8C7" : "rgba(108,248,199,.3)";+ ctx.lineWidth = 2.25; ctx.lineJoin = "round"; ctx.lineCap = "round";+ ctx.beginPath(); ctx.moveTo(sx(pts[0][0]), sy(pts[0][1]));+ for (var i = 1; i < pts.length; i++) ctx.lineTo(sx(pts[i][0]), sy(pts[i][1]));+ ctx.stroke();+ }++ // scan: mint dots, only while the lidar speaks (an old scan is not where the robot is)+ var sc = scanLive() ? scanWorld() : [], r = Math.max(1.1, Math.min(2.4, S.view.scale / 40));+ ctx.fillStyle = "rgba(108,248,199,.9)";+ for (var k = 0; k < sc.length; k++) {+ var X = sx(sc[k][0]), Y = sy(sc[k][1]);+ if (X < -4 || Y < -4 || X > w + 4 || Y > h + 4) continue;+ ctx.fillRect(X - r / 2, Y - r / 2, r, r);+ }++ // goal: salmon ring (dashed while it waits for confirmation)+ var nav = S.state && S.state.nav;+ if (nav && nav.goal) ring(nav.goal.x, nav.goal.y, false);+ if (S.pendingGoal) ring(S.pendingGoal.x, S.pendingGoal.y, true);++ // pose: white triangle pointing along yaw+ var p = pose();+ if (p && typeof p.x === "number") {+ var cx = sx(p.x), cy = sy(p.y), a = -(p.yaw || 0), L = 15, B = 8;+ 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";+ ctx.strokeStyle = "rgba(8,15,26,.65)"; ctx.lineWidth = 1.2;+ ctx.fill(); ctx.stroke(); ctx.restore();+ }+ positionChip();+}++function ring(x, y, dashed) {+ ctx.save();+ ctx.strokeStyle = "#FF947E"; ctx.lineWidth = 2.25;+ if (dashed) ctx.setLineDash([4, 4]);+ ctx.beginPath(); ctx.arc(sx(x), sy(y), 9, 0, 2 * Math.PI); ctx.stroke();+ ctx.setLineDash([]);+ ctx.beginPath(); ctx.arc(sx(x), sy(y), 2, 0, 2 * Math.PI); ctx.fillStyle = "#FF947E"; ctx.fill();+ ctx.restore();+}++function pollMeta() {+ return fetchJSON("/map_meta").then(function (m) {+ if (!m || m.__missing || m.error || !(m.width > 0)) {+ $("map-empty").hidden = !!S.mapImg; return;+ }+ var stamp = String(m.stamp != null ? m.stamp : (m.map_mtime != null ? m.map_mtime : ""));+ if (stamp === S.mapStamp && S.mapImg) return;+ return new Promise(function (ok) {+ var img = new Image();+ img.onload = function () {+ // /map.png is a second request: the server may have decoded a newer, differently+ // cropped costmap in between. The crop only grows, so a size mismatch catches it;+ // keep the old pair and leave the stamp, the next poll fetches both again.+ if (img.naturalWidth !== m.width || img.naturalHeight !== m.height) { ok(); return; }+ // swap image and metadata together, so the transform always matches the picture+ S.mapImg = img; S.meta = m; S.mapStamp = stamp;+ $("map-empty").hidden = true;+ if (!S.fitted) fitView();+ renderCaption(); requestDraw(); ok();+ };+ img.onerror = function () { ok(); };+ img.src = "/map.png?stamp=" + encodeURIComponent(stamp);+ });+ }, function () {});+}++function pollScan() {+ return fetchJSON("/scan.json").then(function (d) {+ if (d && !d.__missing && d.points) { S.scan = d; requestDraw(); }+ }, function () {});+}++function pollPath() {+ return fetchJSON("/path.json").then(function (d) {+ S.path = (d && !d.__missing && d.points) ? d : null; requestDraw();+ }, function () {});+}++// pointer events: drag = pan, wheel / pinch = zoom around the cursor, tap = goal+var ptrs = {}, gesture = null;++function local(e) { var b = cv.getBoundingClientRect(); return { x: e.clientX - b.left, y: e.clientY - b.top }; }+function ptrList() { return Object.keys(ptrs).map(function (k) { return ptrs[k]; }); }++cv.addEventListener("pointerdown", function (e) {+ if (e.button !== undefined && e.button > 0) return;+ cv.setPointerCapture(e.pointerId);+ ptrs[e.pointerId] = local(e);+ var l = ptrList();+ if (l.length === 1) {+ gesture = { kind: "pan", start: l[0], vx: S.view.x, vy: S.view.y, moved: false };+ } else if (l.length === 2) {+ var mx = (l[0].x + l[1].x) / 2, my = (l[0].y + l[1].y) / 2;+ gesture = { kind: "pinch", d0: Math.hypot(l[0].x - l[1].x, l[0].y - l[1].y) || 1, s0: S.view.scale,+ wx: wx(mx), wy: wy(my), moved: true };+ }+});++cv.addEventListener("pointermove", function (e) {+ var p = local(e);+ setText("hud-xy", "x " + signed(wx(p.x), 2) + " m y " + signed(wy(p.y), 2) + " m");+ if (!(e.pointerId in ptrs)) return;+ ptrs[e.pointerId] = p;+ if (!gesture) return;+ var l = ptrList();+ if (gesture.kind === "pan" && l.length === 1) {+ var dx = p.x - gesture.start.x, dy = p.y - gesture.start.y;+ if (!gesture.moved && Math.abs(dx) + Math.abs(dy) > TAP_SLOP_PX) { gesture.moved = true; cv.classList.add("grabbing"); }+ if (gesture.moved) { S.view.x = gesture.vx - dx / S.view.scale; S.view.y = gesture.vy + dy / S.view.scale; requestDraw(); }+ } else if (gesture.kind === "pinch" && l.length >= 2) {+ var d = Math.hypot(l[0].x - l[1].x, l[0].y - l[1].y) || 1;+ var mx = (l[0].x + l[1].x) / 2, my = (l[0].y + l[1].y) / 2;+ zoomTo(gesture.s0 * d / gesture.d0, gesture.wx, gesture.wy, mx, my);+ }+});++function endPointer(e) {+ var was = gesture, p = ptrs[e.pointerId] || local(e);+ delete ptrs[e.pointerId];+ cv.classList.remove("grabbing");+ var left = ptrList().length;+ if (left === 0) {+ gesture = null;+ if (e.type === "pointerup" && was && was.kind === "pan" && !was.moved) tap(p);+ } else if (left === 1) {+ var q = ptrList()[0]; // pinch -> pan with the remaining finger, never a tap+ gesture = { kind: "pan", start: q, vx: S.view.x, vy: S.view.y, moved: true };+ }+}+cv.addEventListener("pointerup", endPointer);+cv.addEventListener("pointercancel", endPointer);+cv.addEventListener("pointerleave", function () { setText("hud-xy", "x -- m y -- m"); });++function zoomTo(scale, wxp, wyp, px, py) {+ S.view.scale = Math.max(4, Math.min(600, scale));+ S.view.x = wxp - px / S.view.scale;+ S.view.y = wyp + py / S.view.scale;+ requestDraw();+}++cv.addEventListener("wheel", function (e) {+ e.preventDefault();+ var p = local(e);+ zoomTo(S.view.scale * Math.pow(1.0015, -e.deltaY), wx(p.x), wy(p.y), p.x, p.y);+}, { passive: false });++$("b-fit").addEventListener("click", fitView);++// tap on the map -> inline confirm chip, never a modal+function tap(p) {+ var x = wx(p.x), y = wy(p.y);+ if (!canGoal()) {+ S.pendingGoal = null;+ showChip(p, "Goals need the explore stack running.", false);+ return;+ }+ S.pendingGoal = { x: x, y: y, px: p.x, py: p.y };+ showChip(p, "Send goal (" + signed(x, 2) + ", " + signed(y, 2) + ")? Stops exploration first.", true);+ requestDraw();+}++var chipTimer = null;+function showChip(p, text, ask) {+ var chip = $("goal-chip");+ setText("goal-text", text);+ $("b-goal-yes").hidden = !ask;+ $("b-goal-no").textContent = ask ? "Cancel" : "OK";+ chip.hidden = false;+ chip.dataset.px = p.x; chip.dataset.py = p.y;+ positionChip();+ clearTimeout(chipTimer);+ if (ask) $("b-goal-yes").focus({ preventScroll: true });+ else chipTimer = setTimeout(hideChip, 4000);+}++function positionChip() {+ var chip = $("goal-chip");+ if (chip.hidden) return;+ var px = S.pendingGoal ? sx(S.pendingGoal.x) : +chip.dataset.px;+ var py = S.pendingGoal ? sy(S.pendingGoal.y) : +chip.dataset.py;+ var cw = chip.offsetWidth, ch = chip.offsetHeight;+ var left = Math.max(0, Math.min(W() - cw, px - cw / 2));+ var top = py - ch - 18;+ if (top < 44) top = py + 18;+ chip.style.left = left + "px";+ chip.style.top = Math.max(0, Math.min(H() - ch, top)) + "px";+}++function hideChip() { $("goal-chip").hidden = true; S.pendingGoal = null; requestDraw(); }++$("b-goal-no").addEventListener("click", hideChip);+$("b-goal-yes").addEventListener("click", function () {+ var g = S.pendingGoal; if (!g) return;+ hideChip();+ post("goal", { x: +g.x.toFixed(3), y: +g.y.toFixed(3) }).then(function (d) {+ answer(d, "Goal sent: " + signed(g.x, 2) + ", " + signed(g.y, 2) + " m.");+ pollState();+ }, function () { say("Goal not sent: the cockpit did not answer.", true); });+});++window.addEventListener("keydown", function (e) {+ if (e.key === "Escape") { hideChip(); $("ex-confirm").hidden = true; }+});++// ---------------------------------------------------------------- commands++function stackCmd(action, okText) {+ S.launchBusy = true; S.launchAction = action; renderControls();+ post("stack", { action: action }).then(function (d) {+ if (!answer(d, okText)) { S.launchBusy = false; renderControls(); return; }+ watchLaunch();+ }, function () { S.launchBusy = false; renderControls(); say("The cockpit did not answer.", true); });+}++var launchWatching = false;+function watchLaunch() {+ if (launchWatching) return;+ launchWatching = true;+ (function step() {+ fetchJSON("/launch.json").then(function (d) {+ var lines = (d && (d.lines || d.tail)) || [];+ var ul = $("launch-tail"), html = "";+ lines.slice(-5).forEach(function (l) { html += "<li>" + esc(l) + "</li>"; });+ ul.innerHTML = html;+ if (d && d.running) { setTimeout(step, POLL.launch); return; }+ launchWatching = false;+ var was = S.launchAction;+ S.launchBusy = false; S.launchAction = null;+ renderControls();+ if (was) loadPreflight(); // check, map and explore all rewrite preflight.json+ pollState();+ }, function () { setTimeout(step, POLL.launch * 2); });+ })();+}++$("b-map").addEventListener("click", function () { stackCmd("map", "Launching the mapping stack."); });+$("b-explore").addEventListener("click", function () { stackCmd("explore", "Launching the explore stack. Exploration stays off."); });+$("b-stack-stop").addEventListener("click", function () { stackCmd("stop", "Stopping the stack."); });+$("b-check").addEventListener("click", function () { stackCmd("check", "Flight check running."); });+$("b-check2").addEventListener("click", function () { stackCmd("check", "Flight check running."); });++$("b-ex-start").addEventListener("click", function () { $("ex-confirm").hidden = false; $("b-ex-yes").focus(); });+$("b-ex-no").addEventListener("click", function () { $("ex-confirm").hidden = true; });+$("b-ex-yes").addEventListener("click", function () {+ $("ex-confirm").hidden = true;+ post("explore", { action: "start" }).then(function (d) { answer(d, "Exploration started."); pollState(); },+ function () { say("The cockpit did not answer.", true); });+});+$("b-ex-stop").addEventListener("click", function () {+ post("explore", { action: "stop" }).then(function (d) { answer(d, "Exploration stopped."); pollState(); },+ function () { say("The cockpit did not answer.", true); });+});++$("b-ep").addEventListener("click", function () {+ var rec = (S.state && S.state.recording) || {}, ep = rec.episode;+ var name = ep ? ep.name : ($("ep-name").value.trim() || "ep1");+ post("episode", { action: ep ? "stop" : "start", name: name }).then(function (d) {+ if (answer(d, ep ? "Episode " + name + " closed." : "Episode " + name + " started.") && ep) {+ var m = /^(.*?)(\d+)$/.exec(name);+ $("ep-name").value = m ? m[1] + (parseInt(m[2], 10) + 1) : name + "-2";+ }+ pollState();+ }, function () { say("The cockpit did not answer.", true); });+});++var stopTimer = null;+function stopShow(label, bad) { // #say may be scrolled away: the button that was pressed tells too+ ["b-stop", "stop-fab"].forEach(function (id) { var b = $(id); b.textContent = label; b.classList.toggle("fail", !!bad); });+ clearTimeout(stopTimer);+ stopTimer = setTimeout(function () { stopShow("STOP", false); }, bad ? 8000 : 2500);+}+function softStop() {+ hideChip();+ post("stop", {}).then(function (d) {+ stopShow(answer(d, "Soft stop sent: exploration, navigation, zero twist.") ? "STOP SENT" : "STOP NOT SENT", !(d && d.ok));+ pollState();+ }, function () {+ stopShow("STOP NOT SENT", true);+ say("STOP NOT SENT: the cockpit did not answer. Use the pad or moss-fly.sh stop.", true);+ });+}+$("b-stop").addEventListener("click", softStop);+$("stop-fab").addEventListener("click", softStop);++// ---------------------------------------------------------------- flight check++function loadPreflight() {+ return fetchJSON("/preflight.json").then(function (d) {+ if (!d || d.__missing || d.available === false || !d.checks) { S.preflight = null; }+ else S.preflight = d;+ renderPreflight();+ }, function () {});+}++function renderPreflight() {+ var d = S.preflight, list = $("pf-list");+ $("pf-empty").hidden = !!d;+ list.hidden = !d;+ $("pf-again").hidden = !d;+ if (!d) { setText("pf-title", "No flight check yet."); setText("pf-when", ""); return; }+ var ko = typeof d.ko_count === "number" ? d.ko_count : d.checks.filter(function (c) { return !c.ok; }).length;+ setText("pf-title", ko ? ko + (ko > 1 ? " checks" : " check") + " to fix." : "Ready to fly.");+ $("pf-title").classList.toggle("attn", ko > 0);+ renderPreflightWhen();+ // Brief by default: what needs fixing stays in view, the passing rows fold+ // away so the base and system modules are not pushed off the screen.+ var html = "", sec = null, pass = 0, skippedN = 0;+ d.checks.forEach(function (c) {+ if (c.section && c.section !== sec) {+ sec = c.section;+ html += '<li class="sec"><span class="kicker">' + esc(String(sec).toUpperCase()) + "</span></li>";+ }+ var skipped = c.ok && /skipp|held by the stack/i.test(c.detail || "");+ var mark = skipped ? '<span class="v na">--</span>' : c.ok ? '<span class="v ok">OK</span>' : '<span class="v ko">KO</span>';+ if (c.ok && !skipped) pass++;+ if (skipped) skippedN++;+ html += '<li class="' + (c.ok ? "pass" : "fail") + '">' + mark + '<span class="lb">' + esc(c.label) + "</span>"+ + (c.detail ? '<span class="dt">' + esc(c.detail) + "</span>" : "") + "</li>";+ });+ var skipTxt = skippedN ? ", " + skippedN + " skipped" : "";+ html += '<li class="passline">' + (pass + skippedN === d.checks.length ? pass + " checks pass" + skipTxt + "."+ : pass + " passing " + (pass === 1 ? "check" : "checks") + " folded" + skipTxt + ".") + "</li>";+ list.innerHTML = html;+ list.classList.toggle("brief", !S.pfAll);+ setText("b-pf-all", S.pfAll ? "Fold passing checks" : "Show passing checks");+ $("b-pf-all").setAttribute("aria-expanded", S.pfAll ? "true" : "false");+}++// the check's age on the Jetson's clock (no NTP in the field), refreshed by every state poll+function renderPreflightWhen() {+ var d = S.preflight;+ if (!d) return;+ var now = (S.state && typeof S.state.ts === "number") ? S.state.ts + offlineFor() : Date.now() / 1000;+ var when = typeof d.ts === "number" ? ago(now - d.ts) : "";+ setText("pf-when", (d.summary || "") + (when ? " · " + when : ""));+}++$("b-pf-all").addEventListener("click", function () { S.pfAll = !S.pfAll; renderPreflight(); });++// ---------------------------------------------------------------- log++function fmtTs(ts) {+ if (typeof ts === "number") { var d = new Date(ts * 1000); return pad2(d.getHours()) + ":" + pad2(d.getMinutes()) + ":" + pad2(d.getSeconds()); }+ // dimOS stamps main.jsonl in UTC ISO with a "Z": parse it, show local time+ var t = typeof ts === "string" && /^\d{4}-\d{2}-\d{2}T/.test(ts) ? new Date(ts) : null;+ if (t && !isNaN(t.getTime())) return pad2(t.getHours()) + ":" + pad2(t.getMinutes()) + ":" + pad2(t.getSeconds());+ var m = /(\d{2}:\d{2}:\d{2})/.exec(String(ts || ""));+ return m ? m[1] : "";+}++function pollLog() {+ return fetchJSON("/log.json?n=20").then(function (d) {+ var lines = Array.isArray(d) ? d : (d && (d.lines || d.log)) || [];+ var el = $("log"), html = "";+ lines.slice(-20).forEach(function (l) {+ var lv = String(l.level || "").toLowerCase(), warn = /warn|error|crit/.test(lv);+ html += '<li class="' + (warn ? "warn" : "") + '"><span class="t">' + esc(fmtTs(l.ts)) + '</span><span class="e">'+ + esc(l.event) + (l.logger ? ' <span class="lg">' + esc(String(l.logger).split(".").pop()) + "</span>" : "")+ + "</span></li>";+ });+ if (!html) html = '<li><span class="t"></span><span class="e dim">' + (stack().running ? "Log is empty." : "No stack, no log.") + "</span></li>";+ if (el.innerHTML !== html) el.innerHTML = html;+ }, function () {});+}++// ---------------------------------------------------------------- go++if ("ResizeObserver" in window) new ResizeObserver(function () { resize(); if (!S.fitted) fitView(); }).observe(wrap);+window.addEventListener("resize", resize);+resize();+camConnect();+renderCamera();+loadPreflight();+fetchJSON("/launch.json").then(function (d) { if (d && d.running) { S.launchBusy = true; watchLaunch(); } }, function () {});+poller(pollState, POLL.state);+poller(pollMeta, POLL.meta);+poller(pollScan, POLL.scan);+poller(pollPath, POLL.path);+poller(pollLog, POLL.log);diff --git a/tools/moss_cockpit/assets/mark-mint.svg b/tools/moss_cockpit/assets/mark-mint.svgnew file mode 100644index 0000000..edc26c1--- /dev/null+++ b/tools/moss_cockpit/assets/mark-mint.svg@@ -0,0 +1 @@+<svg xmlns="http://www.w3.org/2000/svg" width="393" height="353" viewBox="16.0 16.0 393.0 353.0" role="img"><title>mark-mint</title><polygon points="32.000000,352.000000 32.000000,32.000000 112.000000,32.000000 212.000000,212.000000 312.000000,32.000000 392.000000,32.000000 392.000000,352.000000 312.000000,352.000000 312.000000,172.000000 212.000000,332.000000 112.000000,172.000000 112.000000,352.000000" fill="#6cf8c7"/></svg>diff --git a/tools/moss_cockpit/assets/wordmark-white.svg b/tools/moss_cockpit/assets/wordmark-white.svgnew file mode 100644index 0000000..00e3313--- /dev/null+++ b/tools/moss_cockpit/assets/wordmark-white.svg@@ -0,0 +1 @@+<svg xmlns="http://www.w3.org/2000/svg" width="675" height="219" viewBox="33.0 13.0 675.0 219.0" role="img"><title>wordmark-white</title><path fill="#eaf0ef" d="M48.896 212.0V32.79999999999998H111.616L142.59199999999998 188.96H147.2L178.17600000000002 32.79999999999998H240.89600000000002V212.0H208.12800000000001V57.632000000000005H203.52L172.8 212.0H116.992L86.27199999999999 57.632000000000005H81.664V212.0Z"/><path fill="#eaf0ef" d="M330.528 215.584Q296.736 215.584 276.76800000000003 197.024Q256.8 178.464 256.8 143.904V100.896Q256.8 66.33599999999998 276.76800000000003 47.775999999999996Q296.736 29.216000000000008 330.528 29.216000000000008Q364.32 29.216000000000008 384.288 47.775999999999996Q404.256 66.33599999999998 404.256 100.896V143.904Q404.256 178.464 384.288 197.024Q364.32 215.584 330.528 215.584ZM330.528 185.376Q349.472 185.376 359.96799999999996 174.368Q370.464 163.36 370.464 144.928V99.872Q370.464 81.44 359.96799999999996 70.432Q349.472 59.42400000000001 330.528 59.42400000000001Q311.84000000000003 59.42400000000001 301.216 70.432Q290.592 81.44 290.592 99.872V144.928Q290.592 163.36 301.216 174.368Q311.84000000000003 185.376 330.528 185.376Z"/><path fill="#eaf0ef" d="M483.104 215.584Q462.368 215.584 446.496 208.16Q430.624 200.736 421.664 186.91199999999998Q412.704 173.088 412.704 153.632V146.464H445.984V153.632Q445.984 169.76 455.96799999999996 177.824Q465.952 185.888 483.104 185.888Q500.512 185.888 509.08799999999997 178.976Q517.664 172.064 517.664 161.312Q517.664 153.888 513.44 149.28Q509.216 144.672 501.15200000000004 141.728Q493.088 138.784 481.568 136.224L475.68 134.94400000000002Q457.248 130.848 444.06399999999996 124.57600000000001Q430.88 118.304 423.84000000000003 108.064Q416.8 97.824 416.8 81.44Q416.8 65.05599999999998 424.608 53.40799999999999Q432.416 41.75999999999999 446.624 35.488Q460.832 29.216000000000008 480.032 29.216000000000008Q499.23199999999997 29.216000000000008 514.208 35.744Q529.184 42.27199999999999 537.76 55.19999999999999Q546.336 68.12799999999999 546.336 87.584V95.264H513.056V87.584Q513.056 77.344 509.088 71.07199999999999Q505.12 64.79999999999998 497.696 61.855999999999995Q490.272 58.912000000000006 480.032 58.912000000000006Q464.672 58.912000000000006 457.376 64.672Q450.08 70.43199999999999 450.08 80.416Q450.08 87.072 453.536 91.68Q456.992 96.288 463.904 99.36Q470.81600000000003 102.432 481.568 104.736L487.456 106.01599999999999Q506.656 110.112 520.864 116.512Q535.072 122.91199999999999 543.008 133.408Q550.944 143.904 550.944 160.288Q550.944 176.672 542.624 189.088Q534.304 201.504 519.072 208.54399999999998Q503.84000000000003 215.584 483.104 215.584Z"/><path fill="#eaf0ef" d="M624.104 215.584Q603.368 215.584 587.4960000000001 208.16Q571.624 200.736 562.664 186.91199999999998Q553.704 173.088 553.704 153.632V146.464H586.984V153.632Q586.984 169.76 596.9680000000001 177.824Q606.952 185.888 624.104 185.888Q641.512 185.888 650.088 178.976Q658.664 172.064 658.664 161.312Q658.664 153.888 654.44 149.28Q650.216 144.672 642.152 141.728Q634.088 138.784 622.568 136.224L616.6800000000001 134.94400000000002Q598.248 130.848 585.0640000000001 124.57600000000001Q571.88 118.304 564.8399999999999 108.064Q557.8 97.824 557.8 81.44Q557.8 65.05599999999998 565.608 53.40799999999999Q573.416 41.75999999999999 587.624 35.488Q601.832 29.216000000000008 621.032 29.216000000000008Q640.232 29.216000000000008 655.208 35.744Q670.184 42.27199999999999 678.76 55.19999999999999Q687.336 68.12799999999999 687.336 87.584V95.264H654.056V87.584Q654.056 77.344 650.088 71.07199999999999Q646.12 64.79999999999998 638.696 61.855999999999995Q631.272 58.912000000000006 621.032 58.912000000000006Q605.672 58.912000000000006 598.376 64.672Q591.08 70.43199999999999 591.08 80.416Q591.08 87.072 594.5360000000001 91.68Q597.992 96.288 604.904 99.36Q611.816 102.432 622.568 104.736L628.456 106.01599999999999Q647.656 110.112 661.864 116.512Q676.072 122.91199999999999 684.008 133.408Q691.944 143.904 691.944 160.288Q691.944 176.672 683.624 189.088Q675.304 201.504 660.072 208.54399999999998Q644.84 215.584 624.104 215.584Z"/></svg>diff --git a/tools/moss_cockpit/fonts/SpaceGrotesk-OFL.txt b/tools/moss_cockpit/fonts/SpaceGrotesk-OFL.txtnew file mode 100644index 0000000..cb512b9--- /dev/null+++ b/tools/moss_cockpit/fonts/SpaceGrotesk-OFL.txt@@ -0,0 +1,93 @@+Copyright 2020 The Space Grotesk Project Authors (https://github.com/floriankarsten/space-grotesk)++This Font Software is licensed under the SIL Open Font License, Version 1.1.+This license is copied below, and is also available with a FAQ at:+http://scripts.sil.org/OFL+++-----------------------------------------------------------+SIL OPEN FONT LICENSE Version 1.1 - 26 February 2007+-----------------------------------------------------------++PREAMBLE+The goals of the Open Font License (OFL) are to stimulate worldwide+development of collaborative font projects, to support the font creation+efforts of academic and linguistic communities, and to provide a free and+open framework in which fonts may be shared and improved in partnership+with others.++The OFL allows the licensed fonts to be used, studied, modified and+redistributed freely as long as they are not sold by themselves. The+fonts, including any derivative works, can be bundled, embedded,+redistributed and/or sold with any software provided that any reserved+names are not used by derivative works. The fonts and derivatives,+however, cannot be released under any other type of license. The+requirement for fonts to remain under this license does not apply+to any document created using the fonts or their derivatives.++DEFINITIONS+"Font Software" refers to the set of files released by the Copyright+Holder(s) under this license and clearly marked as such. This may+include source files, build scripts and documentation.++"Reserved Font Name" refers to any names specified as such after the+copyright statement(s).++"Original Version" refers to the collection of Font Software components as+distributed by the Copyright Holder(s).++"Modified Version" refers to any derivative made by adding to, deleting,+or substituting -- in part or in whole -- any of the components of the+Original Version, by changing formats or by porting the Font Software to a+new environment.++"Author" refers to any designer, engineer, programmer, technical+writer or other person who contributed to the Font Software.++PERMISSION & CONDITIONS+Permission is hereby granted, free of charge, to any person obtaining+a copy of the Font Software, to use, study, copy, merge, embed, modify,+redistribute, and sell modified and unmodified copies of the Font+Software, subject to the following conditions:++1) Neither the Font Software nor any of its individual components,+in Original or Modified Versions, may be sold by itself.++2) Original or Modified Versions of the Font Software may be bundled,+redistributed and/or sold with any software, provided that each copy+contains the above copyright notice and this license. These can be+included either as stand-alone text files, human-readable headers or+in the appropriate machine-readable metadata fields within text or+binary files as long as those fields can be easily viewed by the user.++3) No Modified Version of the Font Software may use the Reserved Font+Name(s) unless explicit written permission is granted by the corresponding+Copyright Holder. This restriction only applies to the primary font name as+presented to the users.++4) The name(s) of the Copyright Holder(s) or the Author(s) of the Font+Software shall not be used to promote, endorse or advertise any+Modified Version, except to acknowledge the contribution(s) of the+Copyright Holder(s) and the Author(s) or with their explicit written+permission.++5) The Font Software, modified or unmodified, in part or in whole,+must be distributed entirely under this license, and must not be+distributed under any other license. The requirement for fonts to+remain under this license does not apply to any document created+using the Font Software.++TERMINATION+This license becomes null and void if any of the above conditions are+not met.++DISCLAIMER+THE FONT SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,+EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO ANY WARRANTIES OF+MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT+OF COPYRIGHT, PATENT, TRADEMARK, OR OTHER RIGHT. IN NO EVENT SHALL THE+COPYRIGHT HOLDER BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,+INCLUDING ANY GENERAL, SPECIAL, INDIRECT, INCIDENTAL, OR CONSEQUENTIAL+DAMAGES, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING+FROM, OUT OF THE USE OR INABILITY TO USE THE FONT SOFTWARE OR FROM+OTHER DEALINGS IN THE FONT SOFTWARE.diff --git a/tools/moss_cockpit/fonts/SpaceGrotesk.ttf b/tools/moss_cockpit/fonts/SpaceGrotesk.ttfnew file mode 100644index 0000000..a1b2e6cBinary files /dev/null and b/tools/moss_cockpit/fonts/SpaceGrotesk.ttf differdiff --git a/tools/moss_cockpit/index.html b/tools/moss_cockpit/index.htmlnew file mode 100644index 0000000..10632b5--- /dev/null+++ b/tools/moss_cockpit/index.html@@ -0,0 +1,456 @@+<!doctype html>+<html lang="en">+<head>+<meta charset="utf-8">+<meta name="viewport" content="width=device-width, initial-scale=1">+<meta name="color-scheme" content="dark">+<title>MOSS cockpit</title>+<link rel="icon" href="/static/assets/mark-mint.svg" type="image/svg+xml">+<link rel="stylesheet" href="/static/tokens.css">+<style>+ * { box-sizing: border-box; }+ [hidden] { display: none !important; } /* the page hides rows with the attribute; no rule may win over it */+ html, body { margin: 0; }+ body {+ background: var(--moss-studio);+ color: var(--moss-white);+ font-family: var(--moss-font);+ font-weight: var(--moss-w-body);+ font-size: var(--ui-fs-body);+ line-height: 1.4;+ -webkit-font-smoothing: antialiased;+ }+ .num, .tab { font-variant-numeric: tabular-nums; }+ h1, h2, h3, p { margin: 0; }+ button, input { font: inherit; color: inherit; }++ /* The studio background: flat blue plus three thin arcs cut by the frame. */+ #studio { position: fixed; inset: 0; width: 100%; height: 100%; pointer-events: none; z-index: 0; }++ .kicker { font-weight: var(--moss-w-kicker); font-size: var(--ui-fs-kicker); letter-spacing: var(--moss-track-kicker);+ color: var(--moss-mint); font-kerning: none; line-height: 1.2; }+ .title { font-weight: var(--moss-w-title); font-size: var(--ui-fs-title); line-height: 1.15; }+ .dim { color: var(--ui-dim); }+ .small { font-size: var(--ui-fs-small); }++ /* ---- page frame ---- */+ #app { position: relative; z-index: 1; display: grid; grid-template-rows: auto 1fr;+ height: 100vh; height: 100dvh; padding: 0 var(--moss-margin); }+ header { display: flex; align-items: center; gap: 40px; padding: 38px 0 22px; min-height: 96px; }+ .lockup { display: flex; align-items: flex-start; gap: 13px; text-decoration: none; flex: none; }+ .lockup .mark { width: 32.75px; height: auto; display: block; }+ .lockup .wm { height: 30.8px; width: auto; display: block; margin-top: -.7px; }+ .runline { display: flex; flex-direction: column; gap: 4px; min-width: 0; }+ .runline .row { display: flex; align-items: baseline; gap: 18px; min-width: 0; }+ #run-name { font-size: var(--ui-fs-body); white-space: nowrap; overflow: hidden; text-overflow: ellipsis; min-width: 0; }+ .chip { display: inline-flex; align-items: center; gap: 8px; font-weight: 600; font-size: var(--ui-fs-kicker);+ letter-spacing: var(--moss-track-kicker); white-space: nowrap; }+ .chip::before { content: ""; width: 7px; height: 7px; border-radius: 50%; background: currentColor; flex: none; }+ .chip.on { color: var(--moss-mint); }+ .chip.on::before { animation: breathe 2.4s var(--moss-ease) infinite; }+ .chip.idle { color: var(--ui-unknown); }+ .chip.off { color: var(--moss-salmon); }+ .chip.off::before { background: transparent; box-shadow: inset 0 0 0 1.5px currentColor; }+ @keyframes breathe { 50% { opacity: .35; } }++ main { display: grid; grid-template-columns: minmax(250px, 3fr) minmax(0, 6fr) minmax(290px, 3fr);+ column-gap: 36px; min-height: 0; padding-bottom: 28px; }+ .col { min-height: 0; overflow-y: auto; scrollbar-width: thin; scrollbar-color: var(--moss-hairline) transparent;+ padding-right: 4px; }+ .col::-webkit-scrollbar { width: 6px; }+ .col::-webkit-scrollbar-thumb { background: var(--moss-hairline); }+ #col-map { display: flex; flex-direction: column; overflow: hidden; padding-right: 0; }++ /* ---- a module: no box. A 70.5 px accent on a hairline, a kicker, a title. ---- */+ .mod { padding: 0 0 26px; }+ .mod-head { position: relative; border-top: 1px solid var(--moss-hairline); padding-top: 13px; margin-bottom: 14px; }+ .mod-head::before { content: ""; position: absolute; left: 0; top: -1px; width: var(--moss-accent-len);+ height: var(--moss-accent-w); background: var(--moss-mint); }+ .mod-head.alt::before { background: var(--moss-salmon); }+ .mod-head .title { margin-top: 5px; }+ .mod-head .aside { position: absolute; right: 0; top: 11px; }++ /* ---- buttons: hairline, mint fill for primary, 0-4 px radius ---- */+ .btn { appearance: none; background: transparent; border: 1px solid var(--ui-control-line); border-radius: 2px;+ padding: 8px 13px; font-weight: 600; font-size: 14px; line-height: 1.2; cursor: pointer;+ transition: background var(--moss-fade-in) var(--moss-ease), color var(--moss-fade-in) var(--moss-ease); }+ .btn:hover:not(:disabled) { background: rgba(234, 240, 239, .08); }+ .btn.primary { background: var(--moss-mint); border-color: var(--moss-mint); color: var(--moss-navy); }+ .btn.primary:hover:not(:disabled) { background: #8ffad5; }+ .btn:disabled { opacity: .38; cursor: default; }+ .btn.primary:disabled { background: transparent; color: var(--moss-white); border-color: var(--ui-control-line); }+ .btn.link { border: 0; padding: 2px 0; font-size: var(--ui-fs-small); color: var(--moss-mint); background: none;+ border-bottom: 1px solid currentColor; border-radius: 0; }+ .btn.link:hover:not(:disabled) { background: none; color: var(--moss-white); }+ :focus-visible { outline: 2px solid var(--moss-mint); outline-offset: 2px; }+ .btnrow { display: flex; flex-wrap: wrap; gap: 8px; }+ .btnrow .btn { flex: 1 1 auto; }++ /* ---- status marks (UI-only mapping documented in tokens.css) ---- */+ .v { display: inline-block; min-width: 28px; font-weight: 600; font-size: 11.5px; letter-spacing: .1em;+ text-align: center; padding: 2px 0; line-height: 1.25; }+ .v.ok { color: var(--ui-ok); }+ .v.ko { background: var(--ui-fail-bg); color: var(--ui-fail-fg); border-radius: 4px; }+ .v.na { color: var(--ui-unknown); }+ .attn { color: var(--ui-attention); }++ /* ---- flight check ---- */+ #pf-list { list-style: none; margin: 0; padding: 0; }+ #pf-list li { display: grid; grid-template-columns: 34px 1fr; column-gap: 8px; padding: 6px 0;+ border-bottom: 1px solid rgba(106, 135, 151, .45); }+ #pf-list li:last-child { border-bottom: 0; }+ #pf-list .lb { font-size: 14px; }+ #pf-list .dt { grid-column: 2; font-size: 12.5px; color: var(--ui-dim); overflow-wrap: anywhere; }+ #pf-list .sec { display: block; padding: 12px 0 2px; border: 0; }+ #pf-list .sec .kicker { font-size: 11.5px; color: var(--ui-dim); }+ #pf-empty p { margin-bottom: 12px; }+ #pf-list.brief li.pass, #pf-list.brief li.sec { display: none; }+ #pf-list .passline { display: block; grid-template-columns: none; font-size: 12.5px; color: var(--ui-dim); padding: 8px 0 0; border: 0; }+ #pf-list:not(.brief) .passline { display: none; }+ .pf-links { display: flex; gap: 18px; flex-wrap: wrap; margin-top: 12px; }++ /* ---- base ---- */+ .big { font-size: var(--ui-fs-big); font-weight: 600; line-height: 1; letter-spacing: -.01em; }+ .big .unit { font-size: 18px; font-weight: 500; margin-left: 3px; letter-spacing: 0; }+ .batt { display: flex; align-items: baseline; justify-content: space-between; gap: 12px; }+ .meter { position: relative; height: 1px; background: var(--moss-rule); margin: 12px 0 14px; }+ .meter > i { position: absolute; left: 0; top: -1px; height: var(--moss-progress-w); background: var(--moss-mint);+ width: 0; transition: width var(--moss-fade-in) var(--moss-ease); }+ .meter.attn > i { background: var(--moss-salmon); }+ .kv { display: grid; grid-template-columns: auto 1fr; column-gap: 14px; row-gap: 5px; font-size: 14px; }+ .kv dt { color: var(--ui-dim); }+ .kv dd { margin: 0; text-align: right; font-variant-numeric: tabular-nums; overflow-wrap: anywhere; }+ .tracks { display: grid; grid-template-columns: 1fr auto 1fr; align-items: center; gap: 14px; margin: 4px 0 16px; }+ .track { display: flex; align-items: center; gap: 10px; }+ .track.r { flex-direction: row-reverse; }+ .track .lbl { text-align: center; font-variant-numeric: tabular-nums; }+ .track .lbl b { display: block; font-size: 22px; font-weight: 600; }+ .track .lbl span { font-size: 11.5px; letter-spacing: .1em; color: var(--ui-dim); font-weight: 600; }+ .tbar { position: relative; width: 8px; height: 72px; background: rgba(234, 240, 239, .10); }+ .tbar::after { content: ""; position: absolute; left: -5px; right: -5px; top: 50%; height: 1px; background: var(--moss-hairline); }+ .tbar > i { position: absolute; left: 0; right: 0; background: var(--moss-mint); height: 0; bottom: 50%;+ transition: height 200ms var(--moss-ease), bottom 200ms var(--moss-ease); }+ .tbar > i.rev { background: var(--moss-white); }+ .tracks .mid { text-align: center; }+ .tracks .mid .kicker { font-size: 11.5px; }++ /* ---- map pane ---- */+ #map-wrap { position: relative; flex: 1 1 auto; min-height: 320px; overflow: hidden; }+ #map { position: absolute; inset: 0; width: 100%; height: 100%; display: block; touch-action: none; cursor: grab; }+ #map.grabbing { cursor: grabbing; }+ #map-tools { position: absolute; top: 12px; right: 0; display: flex; gap: 8px; align-items: center; z-index: 2; }+ #map-tools .btn { background: rgba(61, 87, 106, .7); backdrop-filter: blur(2px); }+ #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; }+ #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; }+ #goal-chip { position: absolute; z-index: 3; display: flex; align-items: center; gap: 10px; padding: 7px 8px 7px 12px;+ background: var(--moss-navy); border-left: var(--moss-vrule-w) solid var(--moss-salmon);+ font-size: 14px; white-space: nowrap; box-shadow: 0 6px 24px rgba(8, 15, 26, .45); }+ #goal-chip[hidden] { display: none; }+ #goal-chip .btn { padding: 5px 10px; font-size: 13px; }+ #goal-chip .btn.go { background: var(--moss-salmon); border-color: var(--moss-salmon); color: var(--moss-navy); }++ /* lower-third caption on the map pane */+ .lower { position: absolute; left: 0; right: 0; bottom: 0; height: 34%; min-height: 128px; background: var(--moss-fade);+ pointer-events: none; z-index: 1; }+ .lower-in { position: absolute; left: 0; right: 0; bottom: 22px; display: grid; grid-template-columns: auto auto 1fr;+ column-gap: 16px; align-items: start; padding-left: 0; }+ .lower .vrule { width: var(--moss-vrule-w); align-self: stretch; background: var(--moss-mint); margin-left: 0; }+ .lower .numeral { font-size: clamp(34px, 3.1vw, var(--moss-fs-numeral)); font-weight: 600; color: var(--moss-mint);+ line-height: .92; font-variant-numeric: tabular-nums; margin-top: 2px; }+ .lower .ttl { font-size: clamp(22px, 2.05vw, 34px); font-weight: 600; line-height: 1.12; margin-top: 6px; }+ .lower .sub { font-size: var(--ui-fs-small); color: var(--ui-dim); margin-top: 6px; font-variant-numeric: tabular-nums; }+ .brule { position: relative; height: 1px; background: var(--moss-rule); margin-top: 0; flex: none; }+ .brule > i { position: absolute; left: 0; bottom: 0; height: var(--moss-progress-w); width: 0; background: var(--moss-mint);+ transition: width 900ms linear; }+ .brule-cap { display: flex; justify-content: space-between; gap: 12px; padding-top: 8px; font-size: 12.5px;+ color: var(--ui-dim); font-variant-numeric: tabular-nums; flex: none; }++ /* ---- cameras, under the map: nose (RealSense) and wrist (arm), side by side ---- */+ .cams { display: grid; grid-template-columns: 1fr 1fr; column-gap: 16px; flex: none; margin-top: 14px;+ height: clamp(150px, 26vh, 300px); }+ .cam { position: relative; height: 100%; background: rgba(8, 15, 26, .35); overflow: hidden; }+ .cam img { position: absolute; inset: 0; width: 100%; height: 100%; object-fit: contain; display: block;+ color: transparent; } /* no alt text through the placeholder while the stream is down */+ .cam img:not([src]) { display: none; } /* no broken-image icon either: the page removes src between streams */+ .cam .tag { position: absolute; left: 10px; top: 8px; margin: 0; font-size: 11.5px; letter-spacing: .14em;+ color: var(--moss-white); opacity: .85; z-index: 2; }+ .cam-cap { display: flex; justify-content: space-between; align-items: center; gap: 12px; padding-top: 8px;+ font-size: 12.5px; color: var(--ui-dim); flex: none; }+ .cam .ph { position: absolute; inset: 0; display: flex; flex-direction: column; justify-content: flex-end; padding: 14px 16px;+ background: var(--moss-fade); }+ .cam .ph[hidden] { display: none; }++ /* ---- controls ---- */+ .ctl-group { padding: 4px 0 14px; }+ .ctl-group + .ctl-group { border-top: 1px solid rgba(106, 135, 151, .45); padding-top: 14px; }+ .ctl-group .kicker { font-size: 11.5px; color: var(--ui-dim); margin-bottom: 8px; display: block; }+ .ctl-note { font-size: 12.5px; color: var(--ui-dim); margin-top: 7px; }+ .field { display: flex; gap: 8px; }+ .field input { flex: 1 1 auto; min-width: 0; background: transparent; border: 0; border-bottom: 1px solid var(--ui-control-line);+ padding: 7px 0; font-size: 14px; font-weight: 500; border-radius: 0; }+ .field input:focus-visible { outline: none; border-bottom-color: var(--moss-mint); }+ #say { min-height: 1.4em; font-size: 13px; margin: -4px 0 10px; }+ #say.err { color: var(--ui-attention); }+ #say.ok { color: var(--moss-mint); }+ #launch-tail { margin: 8px 0 0; padding: 0; list-style: none; font-size: 12px; color: var(--ui-dim);+ font-variant-numeric: tabular-nums; overflow-wrap: anywhere; }+ .confirm { display: flex; align-items: center; gap: 8px; flex-wrap: wrap; margin-top: 8px; padding: 8px 0 8px 12px;+ border-left: var(--moss-vrule-w) solid var(--moss-salmon); font-size: 13.5px; }+ .confirm[hidden] { display: none; }++ .stop { display: block; width: 100%; appearance: none; border: 0; border-radius: 8px; cursor: pointer;+ background: var(--moss-salmon); color: var(--moss-navy); font-weight: 700; font-size: 30px;+ letter-spacing: .14em; padding: 18px 0 17px; line-height: 1; }+ .stop:hover { background: #ffa894; }+ .stop:active { transform: translateY(1px); }+ .stop-cap { font-size: 12.5px; color: var(--ui-dim); margin-top: 8px; }+ .stop.fail { background: var(--moss-navy); color: var(--moss-salmon); box-shadow: inset 0 0 0 2px var(--moss-salmon); }+ /* a STOP always in view, whatever the width and the scroll of the columns */+ #stop-fab { position: fixed; right: 16px; bottom: 16px; z-index: 6; width: auto;+ 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 */++ /* ---- 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; }+ #log .t { color: var(--ui-unknown); }+ #log .e { overflow-wrap: anywhere; }+ #log .warn .e { color: var(--ui-attention); }+ #log .lg { color: var(--ui-unknown); }++ .bus { display: grid; grid-template-columns: 1fr auto auto; column-gap: 12px; row-gap: 3px; font-size: 13px;+ font-variant-numeric: tabular-nums; }+ .bus .n { color: var(--ui-dim); }+ .bus .r { text-align: right; }++ /* offline: keep the last values, dimmed */+ body.offline [data-live] { opacity: .45; transition: opacity var(--moss-fade-out) var(--moss-ease); }+ .stale { opacity: .5; }++ /* ---- phone and narrow windows: one column, map first ---- */+ @media (max-width: 1099px) {+ #app { display: block; height: auto; padding: 0 16px; }+ header { padding: 24px 0 18px; gap: 18px; flex-wrap: wrap; min-height: 0; }+ .lockup .mark { width: 26px; }+ .lockup .wm { height: 24.5px; }+ .lockup { gap: 10px; }+ .runline { flex-basis: 100%; }+ #run-name { white-space: normal; overflow-wrap: anywhere; }+ main { display: flex; flex-direction: column; padding-bottom: 96px; }+ .col { overflow: visible; padding-right: 0; display: contents; }+ #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; }+ }++ @media (prefers-reduced-motion: reduce) {+ *, *::before, *::after { animation: none !important; transition: none !important; }+ }+</style>+</head>+<body>++<svg id="studio" viewBox="0 0 1920 1080" preserveAspectRatio="xMidYMid slice" aria-hidden="true">+ <g fill="none" stroke-linecap="butt">+ <path d="M 1166.67 -160.59 A 217 217 0 0 1 733.63 -171.93" stroke="#6CF8C7" stroke-width="7"/>+ <path d="M -256.16 783.84 A 206 206 0 1 1 -7.50 1107.90" stroke="#FF947E" stroke-width="7"/>+ <path d="M 2020.50 862.00 A 129.5 129.5 0 0 1 2009.21 603.49" stroke="#6CF8C7" stroke-width="6"/>+ </g>+</svg>++<div id="app">+ <header>+ <a class="lockup" href="/" aria-label="MOSS cockpit">+ <img class="mark" src="/static/assets/mark-mint.svg" alt="">+ <img class="wm" src="/static/assets/wordmark-white.svg" alt="MOSS">+ </a>+ <div class="runline">+ <span class="kicker">MOSS / COCKPIT</span>+ <div class="row">+ <span id="run-name" class="num" data-live>no stack</span>+ <span id="chip" class="chip idle" role="status" aria-live="polite">IDLE</span>+ </div>+ </div>+ </header>++ <main>+ <!-- column 1: flight check, base, system -->+ <div class="col" id="col-left">+ <section class="mod" id="mod-flight" aria-labelledby="pf-title">+ <div class="mod-head">+ <p class="kicker">FLIGHT CHECK</p>+ <h2 class="title" id="pf-title">No flight check yet.</h2>+ <p class="small dim" id="pf-when"></p>+ </div>+ <div id="pf-empty">+ <p class="dim small">The check reads the ports, the firmware, the battery, the lidar and the cameras. It never moves the robot.</p>+ <button class="btn primary" id="b-check" type="button">Run flight check</button>+ </div>+ <ul id="pf-list" hidden></ul>+ <p id="pf-again" class="pf-links" hidden>+ <button class="btn link" id="b-pf-all" type="button" aria-expanded="false">Show passing checks</button>+ <button class="btn link" id="b-check2" type="button">Run the check again</button>+ </p>+ </section>++ <section class="mod" id="mod-base" aria-labelledby="base-title" data-live>+ <div class="mod-head">+ <p class="kicker">BASE</p>+ <h2 class="title" id="base-title">No telemetry.</h2>+ </div>+ <div class="batt">+ <div class="big num"><span id="b-volt">--</span><span class="unit">V</span></div>+ <div class="big num" style="font-size:28px"><span id="b-pct">--</span><span class="unit">%</span></div>+ </div>+ <div class="meter" id="b-meter"><i></i></div>+ <div class="tracks" aria-label="Track outputs">+ <div class="track l"><div class="tbar"><i id="t-l"></i></div><div class="lbl"><b id="t-lv" class="num">0</b><span>LEFT %</span></div></div>+ <div class="mid"><p class="kicker">TRACKS</p><p class="small dim num" id="t-target">target 0 / 0</p></div>+ <div class="track r"><div class="tbar"><i id="t-r"></i></div><div class="lbl"><b id="t-rv" class="num">0</b><span>RIGHT %</span></div></div>+ </div>+ <dl class="kv">+ <dt>Mode</dt><dd id="b-mode">--</dd>+ <dt>Ticks L / R</dt><dd id="b-ticks">--</dd>+ <dt>Firmware</dt><dd id="b-fw">--</dd>+ <dt>Telemetry age</dt><dd id="b-age">--</dd>+ </dl>+ </section>++ <section class="mod" id="mod-system" aria-labelledby="sys-title" data-live>+ <div class="mod-head alt">+ <p class="kicker">SYSTEM</p>+ <h2 class="title" id="sys-title">Jetson.</h2>+ </div>+ <dl class="kv" id="sys-kv">+ <dt>CPU</dt><dd id="s-cpu">--</dd>+ <dt>RAM</dt><dd id="s-ram">--</dd>+ <dt>Temperature</dt><dd id="s-temp">--</dd>+ <dt>Load, 1 min</dt><dd id="s-load">--</dd>+ <dt>Uptime</dt><dd id="s-up">--</dd>+ </dl>+ <div class="meter" id="s-cpu-m" style="margin:14px 0 4px"><i></i></div>+ <p class="small dim">CPU</p>+ <div class="meter" id="s-ram-m" style="margin:10px 0 4px"><i></i></div>+ <p class="small dim">RAM</p>+ <dl class="kv" id="s-disks" style="margin-top:14px"></dl>+ <p class="kicker" style="margin:20px 0 8px;font-size:11.5px;color:var(--ui-dim)">BUS</p>+ <div class="bus" id="bus"></div>+ </section>+ </div>++ <!-- column 2: the map -->+ <div class="col" id="col-map">+ <div class="mod-head" style="flex:none">+ <p class="kicker">MAP</p>+ <h2 class="title" id="map-title">Waiting for a map.</h2>+ </div>+ <div id="map-wrap" data-live>+ <canvas id="map" aria-label="Map. Drag to pan, wheel or pinch to zoom, tap to send a goal when navigation runs."></canvas>+ <div id="hud"><span id="hud-xy">x -- m y -- m</span></div>+ <div id="map-tools"><button class="btn" id="b-fit" type="button">Fit</button></div>+ <div id="map-empty"><p class="dim" id="map-empty-why">No map yet. Launch a stack to build one.</p></div>+ <div id="goal-chip" hidden role="dialog" aria-label="Confirm goal">+ <span id="goal-text" class="num">Send goal?</span>+ <button class="btn go" id="b-goal-yes" type="button">Send</button>+ <button class="btn" id="b-goal-no" type="button">Cancel</button>+ </div>+ <div class="lower">+ <div class="lower-in">+ <div class="vrule"></div>+ <div class="numeral" id="cap-num">00</div>+ <div>+ <p class="kicker" id="cap-kicker">RUN / NONE</p>+ <p class="ttl" id="cap-title" aria-live="polite">No stack running.</p>+ <p class="sub" id="cap-sub"></p>+ </div>+ </div>+ </div>+ </div>+ <div class="brule" aria-hidden="true"><i id="cap-progress"></i></div>+ <div class="brule-cap"><span id="cap-left">no episode</span><span id="cap-right"></span></div>+ <div class="cams" id="cams" data-live>+ <div class="cam" id="cam-nose">+ <img id="cam-img" alt="Nose camera, live">+ <div class="ph" id="cam-ph"><p class="kicker">NO FRAMES</p><p class="small dim" id="cam-why">The camera topic is silent.</p></div>+ <p class="tag kicker">NOSE</p>+ </div>+ <div class="cam" id="cam-wrist">+ <img id="wrist-img" alt="Wrist camera, live">+ <div class="ph" id="wrist-ph"><p class="kicker">NO FRAMES</p><p class="small dim" id="wrist-why">The camera topic is silent.</p></div>+ <p class="tag kicker">WRIST</p>+ </div>+ </div>+ <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 -->+ <div class="col" id="col-right">+ <section class="mod" id="mod-controls" aria-labelledby="ctl-title">+ <div class="mod-head">+ <p class="kicker">CONTROLS</p>+ <h2 class="title" id="ctl-title">Nothing moves by itself.</h2>+ </div>+ <p id="say" role="status" aria-live="polite"></p>++ <div class="ctl-group">+ <span class="kicker">STACK</span>+ <div class="btnrow">+ <button class="btn" id="b-map" type="button">Launch map</button>+ <button class="btn" id="b-explore" type="button">Launch explore</button>+ <button class="btn" id="b-stack-stop" type="button">Stop stack</button>+ </div>+ <ul id="launch-tail"></ul>+ </div>++ <div class="ctl-group">+ <span class="kicker">EXPLORATION</span>+ <div class="btnrow">+ <button class="btn primary" id="b-ex-start" type="button">Start exploration</button>+ <button class="btn" id="b-ex-stop" type="button">Stop exploration</button>+ </div>+ <div class="confirm" id="ex-confirm" hidden>+ <span>The rover will drive on its own.</span>+ <button class="btn primary" id="b-ex-yes" type="button">Start</button>+ <button class="btn" id="b-ex-no" type="button">Cancel</button>+ </div>+ <p class="ctl-note" id="ex-note">Needs the explore stack.</p>+ </div>++ <div class="ctl-group">+ <span class="kicker">RECORD EPISODE</span>+ <div class="field">+ <input id="ep-name" type="text" autocomplete="off" spellcheck="false" aria-label="Episode name" value="ep1">+ <button class="btn" id="b-ep" type="button">Start</button>+ </div>+ <p class="ctl-note num" id="rec-note">The run records to its database while the stack is up.</p>+ </div>++ <div class="ctl-group">+ <button class="stop" id="b-stop" type="button">STOP</button>+ <p class="stop-cap">Soft stop: exploration + navigation + zero twist; the firmware stops the tracks 300 ms after the last command. The hard stops are the firmware deadman and <span class="num">moss-fly.sh stop</span>.</p>+ </div>+ </section>++ <section class="mod" id="mod-log" aria-labelledby="log-title">+ <div class="mod-head alt">+ <p class="kicker">LOG</p>+ <h2 class="title" id="log-title">Stack log.</h2>+ </div>+ <ol id="log" data-live><li><span class="t"></span><span class="e dim">No stack, no log.</span></li></ol>+ </section>+ </div>+ </main>+</div>++<button class="stop" id="stop-fab" type="button" aria-label="Soft stop">STOP</button>++<script src="/static/app.js"></script>+</body>+</html>diff --git a/tools/moss_cockpit/tokens.css b/tools/moss_cockpit/tokens.cssnew file mode 100644index 0000000..681256d--- /dev/null+++ b/tools/moss_cockpit/tokens.css@@ -0,0 +1,83 @@+/* MOSS brand tokens for the cockpit.+ *+ * Colours, font and spacing are the MOSS brand tokens: flat studio blue,+ * three thin arcs, mint accent, salmon as a counterpoint, Space Grotesk,+ * hairlines instead of boxes. Sizes are set for a 1440 px canvas. Lines+ * marked ADAPT are cockpit-only additions.+ */++/* Variable font, weight axis 300-700, file default 300: the weight RANGE is+ required, otherwise the browser synthesises 600 from the 300 master. */+@font-face {+ font-family: "Space Grotesk";+ src: url("fonts/SpaceGrotesk.ttf") format("truetype");+ font-weight: 300 700;+ font-style: normal;+ font-display: swap;+}++:root {+ /* palette */+ --moss-white: #EAF0EF;+ --moss-mint: #6CF8C7;+ --moss-salmon: #FF947E;+ --moss-navy: #080F1A;+ --moss-studio: #3D576A;++ /* lines */+ --moss-hairline: #6A8797; /* card and closing hairlines */+ --moss-rule: rgba(234, 240, 239, .235); /* caption bottom rule (WHITE alpha 60/255) */+ --moss-fade: linear-gradient(to bottom, rgba(8, 15, 26, 0), rgba(8, 15, 26, .902));++ /* type */+ --moss-font: "Space Grotesk", system-ui, sans-serif;+ --moss-w-body: 500;+ --moss-w-title: 600;+ --moss-w-display: 700;+ --moss-w-kicker: 600;+ --moss-track-kicker: .12em;++ /* display sizes at 1440 px */+ --moss-fs-numeral: 55.5px;+ --moss-fs-title: 49.5px;+ --moss-fs-sub: 23.25px;+ --moss-fs-kicker: 18.75px;+ --moss-fs-kicker-sm: 17.25px;+ --moss-fs-meta: 15.75px;+ --moss-fs-micro: 13.5px;++ /* ADAPT: a dense instrument tier for telemetry: a smaller scale with the+ same hierarchy (caps kicker > sentence-case title > body) and weights. */+ --ui-fs-kicker: 13.5px; /* = --moss-fs-micro, the smallest display size */+ --ui-fs-title: 22px;+ --ui-fs-body: 15px;+ --ui-fs-small: 13px;+ --ui-fs-big: 40px; /* battery and other headline numerals */++ /* grid at 1440: 84 px margin, 84 px columns, 24 px gutters */+ --moss-margin: clamp(16px, 5.834vw, 112px);+ --moss-gutter: 24px;+ --moss-accent-len: 70.5px;+ --moss-accent-w: 2.25px;+ --moss-vrule-w: 3px;+ --moss-progress-w: 2.25px;++ /* motion: ease(x) = x*x*(3-2x) */+ --moss-ease: cubic-bezier(.3333, 0, .6667, 1);+ --moss-fade-in: 300ms;+ --moss-fade-out: 280ms;++ /* ADAPT, UI-only status colours. In the brand tokens the mint/salmon+ alternation carries no status, so this mapping is cockpit-only:+ ok = mint text+ attention = salmon text+ failure = salmon badge with navy 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-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_preflight.py b/tools/moss_preflight.pynew file mode 100755index 0000000..ba87dab--- /dev/null+++ b/tools/moss_preflight.py@@ -0,0 +1,810 @@+#!/usr/bin/env python3+"""MOSS flight check: every organ the MOSS stack needs, probed read-only, one verdict each.++Runs on the robot, from the root of this checkout, before a stack is launched+(and by `tools/moss-fly.sh check` and the launcher's gate 1):++ .venv/bin/python tools/moss_preflight.py the whole check (the lidar spins ~5 s)+ .venv/bin/python tools/moss_preflight.py --no-spin everything but the lidar spin+ .venv/bin/python tools/moss_preflight.py --quick no lidar spin, no firmware listen+ .venv/bin/python tools/moss_preflight.py --json one JSON document on stdout instead of lines+ ... --save <file> also write that JSON document to <file>++Sections, in order: stack, ports, firmware, lidar, camera (RealSense), wrist+camera, gamepad, system, software, state dir. One line per probe,+" OK |KO label - detail", a summary "N/M OK - ready to fly" or+"N/M OK - KO: a, b", exit 1 on any KO.++Read-only by construction:+- the motor board (QT Py) is opened only when no dimOS stack runs and no other+ process holds it, exactly as tools/moss-teleop.sh gate 3 does: a plain open at+ 115200 (DTR asserted, which the firmware needs before it streams), 1.5 s of+ listening, not one byte written. The firmware stays disarmed;+- the lidar is opened under the same conditions; it spins for a few seconds and+ is stopped again (C1Scanner.close sends STOP);+- the RealSense is enumerated, never streamed;+- sudo is only asked whether it would run without a password (`sudo -n true`),+ and only when dimOS would need it at launch (loopback multicast, its route+ or the socket buffers not set yet). It runs nothing as root.+The one thing it may create is the MOSS state dir (and its recordings/).++The MOSS_* on/off switches (MOSS_CAMERA, MOSS_WRIST, MOSS_MOCK_BUS,+MOSS_REQUIRE_PAD) are read the way the stack reads them+(moss_dimos.robot_profile.env_on): unset or empty = the default, 0 / false /+no / off = off, anything else = on. A camera switched off is skipped here, as+the stack skips it.++A check that cannot run because a stack owns the device is reported as skipped+("held by the stack"), never as a failure and never by fighting the stack for+the port: two readers on one serial line corrupt each other's frames.++Every probe is a module-level function so benches/bench_moss_preflight_cold.py+can replace it; nothing runs at import.+"""+from __future__ import annotations++import argparse+import contextlib+import glob+import importlib+import json+import os+import re+import subprocess+import sys+import time+from pathlib import Path++ROOT = Path(__file__).resolve().parent.parent+if str(ROOT) not in sys.path:+ sys.path.insert(0, str(ROOT))++BYID_DIR = "/dev/serial/by-id"+V4L_DIR = "/dev/v4l/by-id"+JS_PATH = "/dev/input/js0"+QTPY_GLOB = "*QT_Py*"+C1_GLOB = "*CP2102N*"+WRIST_GLOBS = ("*KD-USB*-video-index0", "*Sonix*-video-index0")+FIRMWARE_BAUD = 115200+LISTEN_S = 1.5 # five telemetry lines at the firmware's 200 ms period+LIDAR_TURNS = 3+LIDAR_MIN_PPS = 200.0 # the C1 gives ~1500 points/s; 200 = a scan that is barely alive+BATTERY_FLOOR_MV = 10800 # launcher floor (moss-teleop.sh gate 3): margin above the cut under load+BMS_CUT_MV = 10200 # the 3S pack's BMS cuts here+DISK_MIN_GB = 5.0+RAM_MIN_KB = 2.5e6+RMEM_TARGET = 67_108_864 # what dimOS's LCM buffer configurator asks for+IFF_MULTICAST = 0x1000+MCAST_PREFIX = "224.0.0.0/4" # the route dimOS's LCM multicast configurator checks+PAD_SHANWAN = "2563:0526"+PAD_SWITCH_PRO = "057e:2009"+MOSS_BLUEPRINTS = ("moss-nav", "moss-explore", "moss-gamepad")+DISTRIBUTION = "moss-dimos" # the blueprint namespace: dimos run moss-dimos.<name>+# pyrealsense2 is not here: the camera section checks it, and only when the camera is on.+SOFTWARE_IMPORTS = ("dimos", "kiss_icp", "serial", "numpy", "pygame")+REINSTALL = ("uv pip install --python .venv/bin/python -e . --no-deps"+ " (a venv made with python -m venv: .venv/bin/pip install -e . --no-deps)")+LCM_FIX = ("install tools/lcm-loopback.service to set it at boot (docs/cockpit.md),"+ " or give this user passwordless sudo")++RESULTS: list[dict] = []+_SECTION = [""]+_QUIET = [False]+++# ── output ─────────────────────────────────────────────────────────────────+def _say(text: str) -> None:+ if not _QUIET[0]:+ print(text, flush=True)+++def section(name: str) -> None:+ _SECTION[0] = name+ _say(name)+++def verdict(ok: bool, label: str, detail: str = "") -> None:+ RESULTS.append({"ok": bool(ok), "label": label, "detail": detail, "section": _SECTION[0]})+ _say(f" {'OK ' if ok else 'KO '} {label}" + (f" - {detail}" if detail else ""))+++def skipped(label: str, why: str) -> None:+ """A probe deliberately not run: not a failure, not counted in N/M."""+ RESULTS.append({"ok": True, "skipped": True, "label": label,+ "detail": f"skipped: {why}", "section": _SECTION[0]})+ _say(f" -- {label} - skipped: {why}")+++# ── probes (each one replaced by the cold bench) ─────────────────────────────+def _ls(pattern: str) -> list[str]:+ return sorted(glob.glob(pattern))+++def _exists(path: str) -> bool:+ return os.path.exists(path)+++def _runs_dir() -> Path:+ """dimOS's run registry, where dimOS, tools/moss-fly.sh and the cockpit+ look for it (XDG_STATE_HOME when set and not empty)."""+ return Path(os.environ.get("XDG_STATE_HOME") or Path.home() / ".local/state") / "dimos" / "runs"+++def _pid_alive(pid: int) -> bool:+ return pid > 0 and os.path.exists(f"/proc/{pid}")+++def _proc_cmdlines() -> list[tuple[int, str]]:+ """(pid, cmdline with NULs as spaces) for every readable process. Each read+ is guarded: a process that exits between listdir and open is normal."""+ out = []+ try:+ pids = os.listdir("/proc")+ except OSError:+ return out+ for pid in pids:+ if not pid.isdigit():+ continue+ try:+ with open(f"/proc/{pid}/cmdline", "rb") as f:+ raw = f.read()+ except OSError:+ continue+ out.append((int(pid), raw.replace(b"\0", b" ").decode(errors="replace").strip()))+ return out+++def _port_holders(dev: str) -> list[int]:+ """PIDs with `dev` open, from /proc/*/fd (no sudo: fds of other users'+ processes are unreadable and skipped - every MOSS process runs as us)."""+ real = os.path.realpath(dev)+ me = os.getpid()+ holders = []+ try:+ pids = os.listdir("/proc")+ except OSError:+ return holders+ for pid in pids:+ if not pid.isdigit() or int(pid) == me:+ continue+ try:+ for fd in os.listdir(f"/proc/{pid}/fd"):+ try:+ if os.readlink(f"/proc/{pid}/fd/{fd}") == real:+ holders.append(int(pid))+ break+ except OSError:+ continue+ except OSError:+ continue+ return holders+++def _open_serial(port: str, baud: int):+ """A plain pyserial open, as moss-teleop.sh gate 3: DTR is asserted on open+ (the QT Py's USB CDC only streams to a host holding DTR), and this exact+ open does not reset the board."""+ import serial++ return serial.Serial(port, baud, timeout=0.05)+++def _usb_devices() -> set[str]:+ """'vvvv:pppp' of every enumerated USB device, from sysfs (no lsusb needed)."""+ out = set()+ for d in glob.glob("/sys/bus/usb/devices/*"):+ try:+ vid = Path(d, "idVendor").read_text().strip()+ pid = Path(d, "idProduct").read_text().strip()+ except OSError:+ continue+ out.add(f"{vid}:{pid}".lower())+ return out+++def _statvfs(path: str):+ return os.statvfs(path)+++def _meminfo() -> dict[str, int]:+ info = {}+ with open("/proc/meminfo") as f:+ for line in f:+ k, v = line.split(":", 1)+ info[k] = int(v.split()[0])+ return info+++def _sysctl(name: str) -> str | None:+ try:+ return Path("/proc/sys", *name.split(".")).read_text().strip()+ except OSError:+ return None+++def _lo_flags() -> int | None:+ try:+ return int(Path("/sys/class/net/lo/flags").read_text().strip(), 16)+ except (OSError, ValueError):+ return None+++def _mcast_route() -> bool | None:+ """Whether 224.0.0.0/4 has a route, as dimOS checks it (`ip -o route show`+ prints something). None when `ip` cannot run."""+ try:+ r = subprocess.run(["ip", "-o", "route", "show", MCAST_PREFIX],+ capture_output=True, text=True, timeout=10)+ except (OSError, subprocess.SubprocessError):+ return None+ return bool(r.stdout.strip()) if r.returncode == 0 else None+++def _sudo_ok() -> bool:+ """Whether dimOS could fix the system settings itself at launch: root, or+ sudo without a password. `sudo -n true` never prompts and changes nothing."""+ if os.geteuid() == 0:+ return True+ try:+ return subprocess.run(["sudo", "-n", "true"], stdin=subprocess.DEVNULL,+ capture_output=True, timeout=10).returncode == 0+ except (OSError, subprocess.SubprocessError):+ return False+++def _lidar_probe(port: str) -> dict:+ """Info, health and LIDAR_TURNS turns, then STOP (C1Scanner.close)."""+ from moss_dimos.c1_serial import C1Scanner++ lidar = C1Scanner(port, baudrate=460800, timeout=3)+ try:+ lidar.open()+ info = lidar.info()+ health = lidar.health()+ sectors: set[int] = set()+ n = turns = 0+ t0 = time.monotonic()+ for scan in lidar.scans(min_len=50):+ turns += 1+ for _q, ang, dist in scan:+ if dist > 0:+ sectors.add(int(ang // 10) % 36)+ n += 1+ if turns >= LIDAR_TURNS:+ break+ dt = max(time.monotonic() - t0, 1e-6)+ return {"info": info, "health": health, "turns": turns,+ "points_per_s": n / dt, "hz": turns / dt, "sectors": len(sectors)}+ finally:+ lidar.close()+++def _realsense_probe() -> list[dict]:+ """Enumeration only (no pipeline, no stream). Raises ImportError without pyrealsense2."""+ import pyrealsense2 as rs++ out = []+ for dev in rs.context().query_devices():+ def info(key, dev=dev):+ try:+ return dev.get_info(getattr(rs.camera_info, key))+ except Exception: # noqa: BLE001 - a field a device does not report+ return None+ out.append({"name": info("name"), "serial": info("serial_number"),+ "firmware": info("firmware_version"), "usb": info("usb_type_descriptor")})+ return out+++def _import(name: str):+ return importlib.import_module(name)+++def _canonical(name: str) -> str:+ """A distribution name as dimOS turns it into a namespace (PEP 503)."""+ return re.sub(r"[-_.]+", "-", name).lower()+++def _on_root(entry: str) -> bool:+ try:+ return Path(entry or ".").resolve() == ROOT+ except (OSError, RuntimeError, ValueError):+ return False+++def _entry_points(group: str = "dimos.blueprints") -> set[str]:+ """Local names of the blueprints registered by THIS distribution only:+ `dimos run moss-dimos.moss-nav` resolves through the moss-dimos namespace,+ so the same local name registered by another installed package proves+ nothing. Read from the installed distributions, as the stack's dimOS reads+ them: the checkout itself is left out, because the moss_dimos.egg-info an+ editable install leaves there is not on the stack's path."""+ from importlib.metadata import distributions++ path = [p for p in sys.path if not _on_root(p)]+ for dist in distributions(path=path):+ dist_name = dist.metadata.get("Name")+ if dist_name and _canonical(dist_name) == DISTRIBUTION:+ # the first one found is the one Python uses+ return {ep.name for ep in dist.entry_points if ep.group == group}+ return set()+++def _module_file() -> str | None:+ """Where `import moss_dimos` lands for the stack: a fresh interpreter of+ this venv, started from this checkout as the launcher starts dimOS, without+ the current directory on sys.path (dimOS runs as a console script, which+ does not add it). None when moss_dimos does not import there."""+ code = ("import sys\n"+ "if sys.path and sys.path[0] == '':\n"+ " del sys.path[0]\n"+ "import moss_dimos\n"+ "sys.stdout.write(moss_dimos.__file__ or '')\n")+ try:+ r = subprocess.run([sys.executable, "-c", code], cwd=str(ROOT), stdin=subprocess.DEVNULL,+ capture_output=True, text=True, timeout=60)+ except (OSError, subprocess.SubprocessError):+ return None+ return (r.stdout.strip() or None) if r.returncode == 0 else None+++def _robot_profile():+ """The geometry the MOSS stack will fly (MOSS with its MOSS_* overrides)."""+ from moss_dimos import robot_profile++ p = robot_profile.current()+ return p, robot_profile.derived(p)+++def _state_dir() -> Path:+ """MOSS_STATE_DIR resolved exactly as the recorder resolves it (absolute)."""+ from moss_dimos import robot_profile++ return robot_profile.moss_state_dir()+++# ── helpers ────────────────────────────────────────────────────────────────+def _env_on(name: str, default: bool) -> bool:+ """A MOSS_* switch, parsed by the stack's own parser."""+ from moss_dimos import robot_profile++ return robot_profile.env_on(name, default)+++def _raw(name: str) -> str:+ """A switch's value as written, for the messages."""+ return os.environ.get(name, "").strip()+++def _first(patterns) -> str | None:+ for pat in patterns:+ hits = _ls(pat)+ if hits:+ return hits[0]+ return None+++def _nearest_existing(path: Path) -> Path:+ p = path+ while not p.exists() and p != p.parent:+ p = p.parent+ return p+++def _is_dimos_run(cmdline: str) -> bool:+ """`<venv>/bin/dimos [root options] run <blueprint>`: root options such as+ `--rerun-open none` sit BETWEEN `dimos` and `run`, so a plain substring+ 'bin/dimos run' never matches a real launch."""+ toks = cmdline.split()+ for i, t in enumerate(toks):+ if t.endswith("bin/dimos"):+ return "run" in toks[i + 1:]+ return False+++# ── checks ─────────────────────────────────────────────────────────────────+def check_stack(ctx: dict) -> None:+ """A running stack owns the board and the lidar: the probes that would open+ them are skipped, and the launcher's own gate 0 refuses a second stack."""+ section("stack (dimOS)")+ me = os.getpid()+ cmdlines = dict(_proc_cmdlines())+ live, stale = None, 0+ for f in sorted(_runs_dir().glob("*.json")):+ try:+ entry = json.loads(f.read_text())+ pid = int(entry.get("pid", 0))+ except (OSError, ValueError, TypeError):+ stale += 1+ continue+ # A registry file outlives a reboot and its pid can be reused by any+ # process: only a live pid that IS a `dimos ... run` makes a stack.+ if _pid_alive(pid) and _is_dimos_run(cmdlines.get(pid, "")):+ live = (entry.get("run_id") or f.stem, pid)+ else:+ stale += 1+ procs = [(pid, cmd) for pid, cmd in cmdlines.items() if pid != me and _is_dimos_run(cmd)]+ note = f" ({stale} stale registry entr{'y' if stale == 1 else 'ies'})" if stale else ""+ if live:+ ctx["stack"] = f"run {live[0]} pid {live[1]}"+ verdict(True, "stack", f"{live[0]} pid {live[1]}{note}")+ elif procs:+ ctx["stack"] = f"unregistered dimos run pid {procs[0][0]}"+ verdict(True, "stack", f"unregistered dimos run pid {procs[0][0]} (a zombie? moss-fly.sh gate 0 sweeps it){note}")+ else:+ ctx["stack"] = None+ verdict(True, "stack", f"none{note}")+++def _resolve(env_name: str, pattern: str) -> tuple[str | None, str]:+ """The path the stack will open: the env override when set, else the by-id glob."""+ env = os.environ.get(env_name)+ if env:+ return env, f"{env_name}"+ hits = _ls(os.path.join(BYID_DIR, pattern))+ return (hits[0] if hits else None), f"by-id {pattern}"+++def check_ports(ctx: dict) -> None:+ section("ports (by-id)")+ ctx["qtpy"] = ctx["c1"] = None+ for key, label, env_name, pattern, missing in (+ ("qtpy", "motor board (QT Py)", "MOSS_PORT", QTPY_GLOB,+ "not on USB - cable, power, or the board sits in its bootloader (by-id 'Espressif')"),+ ("c1", "lidar (RPLidar C1 on CP2102N)", "MOSS_LIDAR_PORT", C1_GLOB,+ "not on USB - lidar cable or hub")):+ if key == "qtpy" and _env_on("MOSS_MOCK_BUS", False):+ verdict(True, label, f"MOSS_MOCK_BUS={_raw('MOSS_MOCK_BUS')}: fake firmware, the board is not needed")+ continue+ path, how = _resolve(env_name, pattern)+ if not path or not _exists(path):+ verdict(False, label, f"{path or pattern} {missing}")+ continue+ holders = [] if ctx.get("stack") else _port_holders(path)+ if holders:+ verdict(False, f"{label} free", f"held by pid {', '.join(map(str, holders))} - stop that process first")+ continue+ ctx[key] = path+ verdict(True, label, f"{os.path.basename(path)} ({how})"+ + (" - in use by the stack" if ctx.get("stack") else ", free"))+ # On MOSS /dev/ttyUSB0 IS the lidar: a MOSS_PORT naming it (or the CP2102N+ # by-id path) would send motor commands into the lidar.+ if ctx["qtpy"] and ctx["c1"] and os.path.realpath(ctx["qtpy"]) == os.path.realpath(ctx["c1"]):+ verdict(False, "motor port is not the lidar", f"MOSS_PORT and the lidar are both {os.path.realpath(ctx['c1'])}"+ " - unset MOSS_PORT (the QT Py by-id is found on its own)")+ ctx["qtpy"] = None+++def _battery(t: dict) -> None:+ mv = t.get("bus_mV")+ if t.get("ina_ok") is not True or mv is None:+ verdict(False, "battery", "INA219 not seen (ina_ok false, no bus_mV) - check its STEMMA cable")+ return+ v = int(mv) / 1000.0+ if mv >= BATTERY_FLOOR_MV:+ verdict(True, "battery", f"{v:.2f} V")+ elif mv >= BMS_CUT_MV:+ verdict(False, "battery", f"{v:.2f} V - under 10.8 V: the BMS cuts at 10.2 V under load, charge first")+ else:+ verdict(False, "battery", f"{v:.2f} V - under the BMS cut (10.2 V): charge before anything")+++def check_firmware(ctx: dict, quick: bool) -> None:+ section("firmware (QT Py, listen only)")+ if _env_on("MOSS_MOCK_BUS", False):+ skipped("firmware telemetry", f"MOSS_MOCK_BUS={_raw('MOSS_MOCK_BUS')} (fake firmware)")+ return+ if ctx.get("stack"):+ skipped("firmware telemetry", f"held by the stack ({ctx['stack']})")+ return+ if quick:+ skipped("firmware telemetry", "--quick")+ return+ port = ctx.get("qtpy")+ if not port:+ skipped("firmware telemetry", "no free QT Py port (see ports)")+ return+ try:+ ser = _open_serial(port, FIRMWARE_BAUD)+ except Exception as exc: # noqa: BLE001+ verdict(False, "firmware telemetry", f"cannot open {port}: {exc!r} - dialout group? another process?")+ return+ last, n, buf = None, 0, b""+ try:+ with contextlib.suppress(Exception):+ ser.reset_input_buffer() # a partial line from before the open+ deadline = time.monotonic() + LISTEN_S+ while time.monotonic() < deadline:+ buf += ser.read(4096) or b""+ while b"\n" in buf:+ line, buf = buf.split(b"\n", 1)+ line = line.strip()+ if not line.startswith(b"{"):+ continue+ try:+ doc = json.loads(line)+ except ValueError:+ continue+ if isinstance(doc, dict):+ last, n = doc, n + 1+ finally:+ with contextlib.suppress(Exception):+ ser.close()+ if last is None:+ verdict(False, "firmware telemetry", f"mute for {LISTEN_S:.1f} s - power-cycle the board, or reflash it")+ return+ verdict(True, "firmware telemetry", f"{n} line(s) in {LISTEN_S:.1f} s")+ fw = last.get("fw")+ verdict(isinstance(fw, str) and bool(fw), "firmware version", str(fw) if fw else "no 'fw' key - a build older than v3?")+ mode = last.get("mode")+ verdict(mode == 0, "firmware disarmed",+ "mode 0 (off)" if mode == 0 else f"mode {mode} ({last.get('reason')}) - something else is driving it")+ drv = last.get("driver")+ verdict(True, "motor driver", str(drv) if drv else "not reported (a firmware build older than the driver key)")+ _battery(last)+ ticks = last.get("ticks")+ good = isinstance(ticks, list) and len(ticks) == 2 and all(isinstance(x, int) for x in ticks)+ verdict(good, "encoder ticks", f"L {ticks[0]} R {ticks[1]}" if good else f"missing or malformed: {ticks!r}")+++def check_lidar(ctx: dict, spin: bool) -> None:+ section("lidar (RPLidar C1, ~5 s spin)")+ if ctx.get("stack"):+ skipped("lidar scan", f"held by the stack ({ctx['stack']})")+ return+ if not spin:+ skipped("lidar scan", "--quick/--no-spin")+ return+ port = ctx.get("c1")+ if not port:+ skipped("lidar scan", "no free lidar port (see ports)")+ return+ try:+ r = _lidar_probe(port)+ except Exception as exc: # noqa: BLE001+ verdict(False, "lidar answers", f"{type(exc).__name__}: {exc} - unplug/replug the lidar cable, then rerun")+ return+ info = r.get("info") or {}+ verdict(True, "lidar answers", f"model {info.get('model')} serial {info.get('serial')}")+ status, code = (r.get("health") or ("?", -1))+ verdict(status == "Good", "lidar health", f"{status} ({code})")+ pps, sectors = float(r.get("points_per_s", 0.0)), int(r.get("sectors", 0))+ verdict(pps > LIDAR_MIN_PPS and sectors == 36, "lidar scan",+ f"{r.get('turns')} turns, {pps:.0f} points/s, {sectors}/36 sectors seen"+ + ("" if sectors == 36 else " - something sits in the scan plane, or the spin is weak"))+++def check_realsense(ctx: dict) -> None:+ section("camera (RealSense, enumeration only)")+ if not _env_on("MOSS_CAMERA", True):+ skipped("RealSense", f"MOSS_CAMERA={_raw('MOSS_CAMERA')}: this flight builds no RealSense")+ return+ wanted = os.environ.get("MOSS_RS_SERIAL") or None+ try:+ devices = _realsense_probe()+ except ImportError as exc:+ verdict(False, "RealSense", f"pyrealsense2 not importable: {exc} - install it (Software.md),"+ " or fly without the camera: MOSS_CAMERA=0")+ return+ except Exception as exc: # noqa: BLE001+ verdict(False, "RealSense", f"enumeration failed: {exc}")+ return+ d455 = [d for d in devices if "D455" in str(d.get("name") or "")+ and (wanted is None or d.get("serial") == wanted)]+ if not d455:+ seen = ", ".join(f"{d.get('name')} {d.get('serial')}" for d in devices) or "nothing"+ verdict(False, "RealSense D455", f"no D455{' with serial ' + wanted if wanted else ''} (seen: {seen})"+ " - plug it on USB 3, or fly without the camera: MOSS_CAMERA=0")+ return+ d = d455[0]+ usb = str(d.get("usb") or "")+ desc = f"{d.get('name')} serial {d.get('serial')} fw {d.get('firmware')} USB {usb or '?'}"+ if usb.startswith("3"):+ verdict(True, "RealSense D455", desc)+ elif usb.startswith("2"):+ verdict(False, "RealSense D455", f"{desc} - on USB 2: 5 fps max, replug on a USB 3 port/cable")+ else:+ verdict(False, "RealSense D455", f"{desc} - unknown USB link")+++def check_wrist(ctx: dict) -> None:+ section("wrist camera (UVC)")+ if not _env_on("MOSS_WRIST", True):+ skipped("wrist camera", f"MOSS_WRIST={_raw('MOSS_WRIST')}: this flight builds no wrist camera")+ return+ path = _first(os.path.join(V4L_DIR, g) for g in WRIST_GLOBS)+ # The stack skips a missing wrist camera with a warning (no wrist_image):+ # its absence is a warning here too, not a refusal.+ verdict(True, "wrist camera", path or f"WARN not seen (no {' or '.join(WRIST_GLOBS)} under {V4L_DIR}); "+ "the stack runs without wrist_image")+++def check_gamepad(ctx: dict) -> None:+ section("gamepad")+ if _exists(JS_PATH):+ verdict(True, "gamepad", f"{JS_PATH} present (if mute: button under the pad, then move a stick)")+ return+ usb = _usb_devices()+ if PAD_SHANWAN in usb:+ why = f"receiver visible ({PAD_SHANWAN}), no js0: replug or reboot"+ elif PAD_SWITCH_PRO in usb:+ why = (f"pad enumerated as a Nintendo Pro Controller ({PAD_SWITCH_PRO}) without a driver: "+ "switch the pad to its Android/X-input mode and replug")+ else:+ why = "no pad (no js0, no known receiver on USB)"+ # A missing pad stops nothing from running: the map builds, the cockpit shows, exploration+ # can be armed; only hand driving is lost. MOSS_REQUIRE_PAD=1 turns this into a refusal.+ required = _env_on("MOSS_REQUIRE_PAD", False)+ verdict(not required, "gamepad", ("" if required else "WARN nothing drives by hand in moss-nav; ") + why)+++def check_system(ctx: dict) -> None:+ section("system")+ try:+ where = _nearest_existing(_state_dir())+ st = _statvfs(str(where))+ free_gb = st.f_bavail * st.f_frsize / 1e9+ verdict(free_gb > DISK_MIN_GB, "disk for recordings", f"{free_gb:.1f} GB free on {where}")+ except Exception as exc: # noqa: BLE001 - an unresolvable state dir, a failed statvfs+ verdict(False, "disk for recordings", f"{type(exc).__name__}: {exc}")+ try:+ avail = _meminfo().get("MemAvailable", 0)+ verdict(avail > RAM_MIN_KB, "RAM available", f"{avail / 1e6:.1f} GB")+ except (OSError, ValueError) as exc:+ verdict(False, "RAM available", str(exc))+ rmem = _sysctl("net.core.rmem_max")+ try:+ r = int(rmem) if rmem is not None else None+ except ValueError:+ r = None+ flags = _lo_flags()+ lo_on = flags is not None and bool(flags & IFF_MULTICAST)+ route = _mcast_route()+ # dimOS sets what is missing with sudo at every launch, and its multicast+ # setting is critical: a launch that cannot run sudo dies on it. The+ # launcher starts dimOS in the background (and the cockpit page has no+ # terminal at all), so only passwordless sudo, or the boot unit, works.+ needs_fix = not lo_on or not route or (r is not None and r < RMEM_TARGET)+ sudo = _sudo_ok() if needs_fix else True+ if r is None:+ verdict(True, "socket buffers", "net.core.rmem_max unreadable (warning only)")+ elif r >= RMEM_TARGET:+ verdict(True, "socket buffers", f"rmem_max {r}")+ elif sudo:+ verdict(True, "socket buffers", f"warning: rmem_max {r} < {RMEM_TARGET} - dimOS raises it with sudo at launch")+ else:+ verdict(True, "socket buffers", f"warning: rmem_max {r} < {RMEM_TARGET} and no passwordless sudo:"+ f" LCM runs with small buffers - {LCM_FIX}")+ if lo_on:+ verdict(True, "loopback multicast", "on")+ elif sudo:+ verdict(True, "loopback multicast", "off - dimOS turns it on with sudo at launch")+ else:+ verdict(False, "loopback multicast", f"off, and no passwordless sudo here: dimOS cannot turn it on at launch"+ f" - {LCM_FIX}")+ if route is None:+ verdict(False, "multicast route", "cannot run `ip` (iproute2): dimOS needs it to check and set LCM multicast")+ elif route:+ verdict(True, "multicast route", f"{MCAST_PREFIX} routed")+ elif sudo:+ verdict(True, "multicast route", f"no {MCAST_PREFIX} route - dimOS adds it on lo with sudo at launch")+ else:+ verdict(False, "multicast route", f"no {MCAST_PREFIX} route, and no passwordless sudo here: dimOS cannot"+ f" add it at launch - {LCM_FIX}")+++def check_software(ctx: dict) -> None:+ section("software")+ # pygame prints a banner on import unless told not to (as moss_dimos/gamepad.py does)+ os.environ.setdefault("PYGAME_HIDE_SUPPORT_PROMPT", "1")+ for name in SOFTWARE_IMPORTS:+ try:+ mod = _import(name)+ verdict(True, f"import {name}", str(getattr(mod, "__version__", "") or ""))+ except Exception as exc: # noqa: BLE001+ verdict(False, f"import {name}", f"{type(exc).__name__}: {exc}")+ try:+ names = _entry_points()+ except Exception as exc: # noqa: BLE001+ names = set()+ verdict(False, "blueprint entry points", f"cannot read: {exc}")+ for bp in MOSS_BLUEPRINTS:+ verdict(bp in names, f"blueprint {bp}",+ f"registered as {DISTRIBUTION}.{bp}" if bp in names+ else f"missing from {DISTRIBUTION} - reinstall in {ROOT}: {REINSTALL}")+ try:+ src = _module_file()+ except Exception: # noqa: BLE001 - reported as not importable+ src = None+ if src is None:+ verdict(False, "moss-dimos install", f"moss_dimos does not import in this venv ({sys.executable})"+ f" - install it in {ROOT}: {REINSTALL}")+ elif Path(src).resolve().is_relative_to(ROOT):+ verdict(True, "moss-dimos install", "the stack imports moss_dimos from this checkout")+ else:+ verdict(False, "moss-dimos install", f"the stack would import moss_dimos from {Path(src).resolve().parent},"+ f" not from this checkout - reinstall in {ROOT}: {REINSTALL}")+ try:+ p, d = _robot_profile()+ name = getattr(p, "name", None)+ lethal = d.get("lethal_clearance_m") if isinstance(d, dict) else getattr(d, "lethal_clearance_m", None)+ verdict(True, "robot profile",+ f"{name}: width {p.robot_width_m} m, pivot {p.pivot_diameter_m} m, "+ f"lidar {p.lidar_height_m} m up, lethal {lethal:.2f} m (placeholders until measured)")+ except Exception as exc: # noqa: BLE001+ verdict(False, "robot profile", f"{type(exc).__name__}: {exc}")+++def check_state_dir(ctx: dict) -> None:+ section("state dir")+ try:+ d = _state_dir()+ (d / "recordings").mkdir(parents=True, exist_ok=True)+ ok = os.access(d / "recordings", os.W_OK)+ verdict(ok, "MOSS state dir", f"{d} (recordings/ {'writable' if ok else 'NOT writable'})")+ except Exception as exc: # noqa: BLE001 - unresolvable or unwritable+ verdict(False, "MOSS state dir", f"{os.environ.get('MOSS_STATE_DIR') or '~/.local/state/moss'}: {exc}")+++# ── main ───────────────────────────────────────────────────────────────────+def document(flags: dict) -> dict:+ counted = [r for r in RESULTS if not r.get("skipped")]+ ko = [r["label"] for r in counted if not r["ok"]]+ n_ok = len(counted) - len(ko)+ summary = f"{n_ok}/{len(counted)} OK" + (f" - KO: {', '.join(ko)}" if ko else " - ready to fly")+ return {"ts": time.time(), "checks": list(RESULTS), "ok_count": n_ok, "ko_count": len(ko),+ "skipped_count": len(RESULTS) - len(counted), "summary": summary, "flags": flags}+++def _save(path: str, doc: dict) -> None:+ p = Path(os.path.expanduser(path))+ p.parent.mkdir(parents=True, exist_ok=True)+ tmp = p.with_name(p.name + ".tmp")+ tmp.write_text(json.dumps(doc, indent=1))+ os.replace(tmp, p) # the cockpit never reads half a document+++def run(quick: bool = False, no_spin: bool = False) -> None:+ ctx: dict = {}+ check_stack(ctx)+ check_ports(ctx)+ check_firmware(ctx, quick)+ check_lidar(ctx, spin=not (quick or no_spin))+ check_realsense(ctx)+ check_wrist(ctx)+ check_gamepad(ctx)+ check_system(ctx)+ check_software(ctx)+ check_state_dir(ctx)+++def main(argv: list[str] | None = None) -> int:+ ap = argparse.ArgumentParser(description="MOSS flight check (read-only)")+ ap.add_argument("--json", action="store_true", help="print one JSON document instead of lines")+ ap.add_argument("--quick", action="store_true", help="skip the lidar spin and the firmware listen")+ ap.add_argument("--no-spin", action="store_true", help="skip only the lidar spin")+ ap.add_argument("--save", metavar="FILE", help="also write the JSON document to FILE (atomically)")+ a = ap.parse_args(argv)+ RESULTS.clear()+ _QUIET[0] = a.json+ flags = {"quick": a.quick, "no_spin": a.no_spin or a.quick}+ if a.json:+ # imports (dimos above all) may print: keep stdout for the document only+ with contextlib.redirect_stdout(sys.stderr):+ run(a.quick, a.no_spin)+ else:+ run(a.quick, a.no_spin)+ doc = document(flags)+ if a.save:+ try:+ _save(a.save, doc)+ except OSError as exc:+ print(f"(could not save {a.save}: {exc})", file=sys.stderr)+ if a.json:+ print(json.dumps(doc, indent=1))+ else:+ print(f"\n{doc['summary']}" + (f" ({doc['skipped_count']} skipped)" if doc["skipped_count"] else ""))+ return 1 if doc["ko_count"] else 0+++if __name__ == "__main__":+ raise SystemExit(main())