Skip to content

armgit / moss

Public Imported from GitHub
0

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

Python

tools/moss_litter_probe.py: measure the litter detector on the floor, stack down (nose camera + the mission's YOLO, one CSV row per detection with the object in base_link, hit rate, median position and spread, error against the tape with --expect; --fake runs the chain without camera or torch); bench 30; docs/litter.md

metrox4ee39c5d122c
diff --git a/benches/bench_moss_litter_probe_cold.py b/benches/bench_moss_litter_probe_cold.py
new file mode 100644
index 0000000..efcc511
--- /dev/null
+++ b/benches/bench_moss_litter_probe_cold.py
@@ -0,0 +1,148 @@
+"""Cold bench: tools/moss_litter_probe.py, the litter detector measured on the floor.
+
+Known input -> known output, in metres: a pixel and a depth give a known point in the
+camera's optical frame; that point lands at a known place in base_link through the
+mechanical frame, the mount's pitch (+ = nose down) and the camera's position; a depth
+patch with a hole in the middle gives the median of what is left; ten frames with eight
+hits give an 80 % hit rate and a median position 2 cm from the tape; and the whole tool,
+run with --fake, writes a CSV whose rows put the fake object where the geometry says
+(x 1.156, y -0.100, z -0.075 with the profile's camera at (0.156, 0, 0.075)). No camera,
+no torch, no dimOS.
+
+Run: PYTHONDONTWRITEBYTECODE=1 PYTHONPATH=. python3 benches/bench_moss_litter_probe_cold.py
+"""
+import csv
+import importlib.util
+import math
+import os
+import subprocess
+import sys
+import tempfile
+from pathlib import Path
+
+ROOT = Path(__file__).resolve().parent.parent
+sys.path.insert(0, str(ROOT))
+
+import numpy as np # noqa: E402
+
+spec = importlib.util.spec_from_file_location("moss_litter_probe", ROOT / "tools" / "moss_litter_probe.py")
+lp = importlib.util.module_from_spec(spec)
+spec.loader.exec_module(lp)
+
+OK = KO = 0
+
+
+def check(cond, label, detail=""):
+ global OK, KO
+ print(f" {'OK ' if cond else 'KO '} {label}{' - ' + str(detail) if detail and not cond else ''}")
+ if cond:
+ OK += 1
+ else:
+ KO += 1
+
+
+def near(a, b, tol=1e-6):
+ return a is not None and abs(a - b) <= tol
+
+
+print("A. geometry")
+INTR = (400.0, 400.0, 320.0, 240.0)
+p = lp.deproject(320, 240, 1.0, INTR)
+check(all(near(a, b) for a, b in zip(p, (0.0, 0.0, 1.0))), "the centre pixel at 1 m -> (0, 0, 1) in the optical frame", p)
+p = lp.deproject(720, 240, 2.0, INTR)
+check(all(near(a, b) for a, b in zip(p, (2.0, 0.0, 2.0))), "400 px right of the centre (one focal length) at 2 m -> X = 2 m", p)
+p = lp.deproject(320, 440, 1.0, INTR)
+check(all(near(a, b) for a, b in zip(p, (0.0, 0.5, 1.0))), "200 px below the centre at 1 m -> Y = 0.5 m (down)", p)
+CAM = (0.156, 0.0, 0.075)
+b = lp.optical_to_base((0.0, 0.0, 1.0), CAM, 0.0)
+check(all(near(a, c) for a, c in zip(b, (1.156, 0.0, 0.075))), "straight ahead at 1 m, level camera -> base (1.156, 0, 0.075)", b)
+b = lp.optical_to_base((0.5, 0.0, 1.0), CAM, 0.0)
+check(all(near(a, c) for a, c in zip(b, (1.156, -0.5, 0.075))), "0.5 m to the RIGHT of the optical axis -> base y = -0.5 (y is left)", b)
+b = lp.optical_to_base((0.0, 0.3, 1.0), CAM, 0.0)
+check(all(near(a, c) for a, c in zip(b, (1.156, 0.0, -0.225))), "0.3 m DOWN in the image -> base z = 0.075 - 0.3", b)
+b = lp.optical_to_base((0.0, 0.0, 1.0), CAM, 90.0)
+check(all(near(a, c) for a, c in zip(b, (0.156, 0.0, -0.925))), "camera pitched 90 deg nose down: the axis point is straight below the camera", b)
+b = lp.optical_to_base((0.0, 0.0, 1.0), CAM, 30.0)
+check(near(b[0], 0.156 + math.cos(math.radians(30))) and near(b[2], 0.075 - math.sin(math.radians(30))),
+ "30 deg nose down: x = 0.156 + cos 30, z = 0.075 - sin 30 (the point is lower than the camera)", b)
+check(lp.parse_xyz("0.026 -0.084 0.075") == (0.026, -0.084, 0.075) and lp.parse_xyz("1,2,3") == (1.0, 2.0, 3.0),
+ "parse_xyz reads 'x y z' and 'x,y,z'")
+try:
+ lp.parse_xyz("1 2")
+ check(False, "parse_xyz refuses two numbers")
+except ValueError:
+ check(True, "parse_xyz refuses two numbers")
+
+print("B. the depth under a box")
+depth = np.full((100, 100), 1000, dtype=np.uint16)
+depth[45:55, 45:55] = 0 # a hole in the middle
+depth[0:10, :] = 3000 # far edges the central third must not see
+check(near(lp.box_depth_m(depth, (20, 20, 80, 80), 0.001), 1.0), "a 60x60 box over 1000 mm with a hole in the middle -> 1.000 m")
+depth2 = np.zeros((100, 100), dtype=np.uint16)
+check(lp.box_depth_m(depth2, (20, 20, 80, 80), 0.001) is None, "no depth under the box -> None")
+check(lp.box_depth_m(depth, (200, 200, 260, 260), 0.001) is None, "a box outside the image -> None")
+depth3 = np.full((100, 100), 2000, dtype=np.uint16)
+depth3[30:70, 30:70] = 800 # the object fills the central third, the edges are floor
+check(near(lp.box_depth_m(depth3, (10, 10, 90, 90), 0.001), 0.8), "edges at 2 m, central third at 0.8 m -> 0.800 m (the object, not the floor)")
+check(lp.box_centre((10, 20, 30, 60)) == (20.0, 40.0), "box centre")
+
+print("C. the summary")
+rows = []
+hits = [1, 2, 4, 5, 6, 8, 9, 10] # frames 3 and 7 saw nothing
+for k, i in enumerate(hits): # x alternates 1.03 / 1.01: median 1.02, spread 0.01
+ rows.append({"frame": i, "t_s": i * 0.2, "latency_ms": 180 + i, "cls": "Metal", "conf": 0.8,
+ "u1": 0, "v1": 0, "u2": 10, "v2": 10, "depth_m": 1.0,
+ "x_m": 1.02 + (0.01 if k % 2 == 0 else -0.01), "y_m": 0.00, "z_m": -0.07})
+rows.append({"frame": 5, "t_s": 1.0, "latency_ms": 185, "cls": "Plastic", "conf": 0.55,
+ "u1": 0, "v1": 0, "u2": 10, "v2": 10, "depth_m": None, "x_m": None, "y_m": None, "z_m": None})
+s = lp.summarise(rows, 10, expect=(1.0, 0.0))
+check(s["frames"] == 10 and s["hit_frames"] == 8 and near(s["hit_rate"], 0.8), "10 frames, 8 with a located detection -> hit rate 80 %", s)
+m = s["classes"]["Metal"]
+check(m["n"] == 8 and near(m["x"], 1.02, 1e-9) and near(m["y"], 0.0) and near(m["sx"], 0.01, 1e-9),
+ "Metal: 8 boxes, median x 1.02, spread 0.01", m)
+check(s["classes"]["Plastic"]["x"] is None and s["classes"]["Plastic"]["no_depth"] == 1, "a box without depth counts, with no position")
+check(near(s["error"]["dx"], 0.02, 1e-9) and near(s["error"]["dy"], 0.0) and near(s["error"]["dist"], 0.02, 1e-9),
+ "against the tape at (1.00, 0.00): the most seen class is 2 cm off", s["error"])
+check(s["latency_ms"] is not None and 180 < s["latency_ms"] < 191, "median latency of the rows")
+lines = lp.summary_lines(s, (1.0, 0.0))
+check(lines[0].startswith("frames 10, with a detection 8 (hit rate 80 %)") and any("Metal: 8 boxes" in l for l in lines)
+ and any("2.0 cm off" in l for l in lines), "the printed summary says the hit rate, the class and the error", lines)
+e = lp.summarise([], 5, expect=(1.0, 0.0))
+check(e["hit_rate"] == 0.0 and e["error"] is None and lp.summary_lines(e, (1.0, 0.0))[-1].endswith("nothing to compare"),
+ "no detection at all: hit rate 0, nothing to compare")
+check(lp.summarise([], 0)["hit_rate"] == 0.0, "zero frames do not divide by zero")
+
+print("D. the whole tool on the fake camera")
+with tempfile.TemporaryDirectory(prefix="moss_litter_probe_") as td:
+ out = Path(td) / "probe.csv"
+ env = {**os.environ, "PYTHONDONTWRITEBYTECODE": "1", "MOSS_STATE_DIR": td}
+ for k in ("MOSS_CAMERA_XYZ_M", "MOSS_CAMERA_PITCH_DEG"):
+ env.pop(k, None)
+ r = subprocess.run([sys.executable, str(ROOT / "tools" / "moss_litter_probe.py"), "--fake", "--frames", "6",
+ "--seconds", "0", "--out", str(out), "--expect", "1.156", "-0.100"],
+ capture_output=True, text=True, timeout=120, env=env)
+ check(r.returncode == 0, "exit 0", r.stderr[-400:])
+ rows = list(csv.DictReader(out.open())) if out.is_file() else []
+ check(len(rows) == 6 and sorted({int(x["frame"]) for x in rows}) == [1, 2, 3, 4, 5, 6], "6 frames -> 6 rows, one Metal each", len(rows))
+ if rows:
+ x, y, z, d = (float(rows[0][k]) for k in ("x_m", "y_m", "z_m", "depth_m"))
+ check(near(d, 1.0, 1e-3) and near(x, 1.156, 2e-3) and near(y, -0.100, 2e-3) and near(z, -0.075, 2e-3),
+ "the fake object (40 px right, 60 px down, 1 m) lands at base (1.156, -0.100, -0.075): depth survives the hole",
+ (x, y, z, d))
+ check(rows[0]["cls"] == "Metal" and near(float(rows[0]["conf"]), 0.83, 1e-6), "class and confidence from the detector")
+ check("hit rate 100 %" in r.stdout and "Metal: 6 boxes" in r.stdout and "0.0 cm off" in r.stdout and "csv " in r.stdout,
+ "the summary: hit rate 100 %, Metal 6 boxes, 0.0 cm off the expected point, the csv path", r.stdout[-500:])
+ r2 = subprocess.run([sys.executable, str(ROOT / "tools" / "moss_litter_probe.py"), "--fake", "--frames", "2", "--seconds", "0",
+ "--out", str(Path(td) / "p2.csv"), "--camera-xyz", "0.026 -0.084 0.075", "--pitch-deg", "0"],
+ capture_output=True, text=True, timeout=120, env=env)
+ rows2 = list(csv.DictReader((Path(td) / "p2.csv").open())) if (Path(td) / "p2.csv").is_file() else []
+ check(r2.returncode == 0 and rows2 and near(float(rows2[0]["x_m"]), 1.026, 2e-3) and near(float(rows2[0]["y_m"]), -0.184, 2e-3),
+ "--camera-xyz moves the answer with the camera: (0.026, -0.084, 0.075) -> x 1.026, y -0.184",
+ rows2[:1] if rows2 else r2.stderr[-300:])
+ r3 = subprocess.run([sys.executable, str(ROOT / "tools" / "moss_litter_probe.py"), "--fake", "--frames", "0", "--seconds", "0"],
+ capture_output=True, text=True, timeout=60, env=env)
+ check(r3.returncode == 2 and "nothing to do" in r3.stderr, "no frames and no seconds -> exit 2 with a reason")
+
+print(f"{OK} OK, {KO} KO")
+print("TEST PASSED" if KO == 0 else "TEST FAILED")
+sys.exit(1 if KO else 0)
diff --git a/docs/litter.md b/docs/litter.md
index cf1eab7..217835b 100644
--- a/docs/litter.md
+++ b/docs/litter.md
@@ -49,6 +49,29 @@ uv pip install --python .venv/bin/python --index-url https://download.pytorch.or
uv pip install --python .venv/bin/python ultralytics lap # lap: the tracker ultralytics would otherwise fetch on first use
```
+### Measure the detector on the floor before trusting it
+
+`tools/moss_litter_probe.py` runs the nose camera and the detector alone, stack
+down, and writes one CSV line per detection with the object's position in
+`base_link` (x forward, y left, z up, the lidar's floor projection: the frame
+of `MOSS_PICK_XY_M`), then a summary: the share of frames that saw the litter,
+the median position and its spread per class, the detector's time per frame,
+and, with `--expect X Y`, how far the median is from where the tape measure
+put the object.
+
+```sh
+tools/moss-fly.sh stop # the camera must be free
+tools/moss_litter_probe.py --seconds 10 --expect 1.00 0.00 # a can 1.00 m ahead of the lidar axis, on it
+tools/moss_litter_probe.py --fake --frames 20 # no camera, no torch: the chain on a synthetic frame
+```
+
+Three or four pieces of litter at measured places, ten seconds each, give the
+two numbers this chain needs before any training: the hit rate on our floor
+and the position error at the approach distance. The geometry is the one the
+stack uses: `MOSS_CAMERA_XYZ_M` and `MOSS_CAMERA_PITCH_DEG` from the robot
+profile (`--camera-xyz`, `--pitch-deg` override them for a test). The CSV
+lands in `<MOSS_STATE_DIR>/litter_probe/`.
+
## The mission
`moss_dimos/mission.py`. The decision (`MissionLogic`) is pure and the
diff --git a/tools/moss_litter_probe.py b/tools/moss_litter_probe.py
new file mode 100755
index 0000000..8c2cc49
--- /dev/null
+++ b/tools/moss_litter_probe.py
@@ -0,0 +1,402 @@
+#!/usr/bin/env python3
+"""Measure the litter detector on the real floor: does it see the litter, and where?
+
+The mission trusts two things it has never been measured on in this room:
+that the YOLO weights see our litter on our floor, and that the box lifted
+through the depth image lands where the object is. This tool runs the nose
+camera and the detector alone, without the stack, and writes one CSV line per
+detection with the object's position in base_link (x forward, y left, z up,
+the lidar's floor projection), then a summary: how many frames saw the
+litter, where it was seen, how far from where the tape measure says it is.
+
+Run it on the rover with the stack DOWN (it opens the RealSense itself; two
+readers of one camera do not work). Put the litter on the floor, measure its
+position from the lidar axis with a tape, and:
+
+ tools/moss_litter_probe.py --seconds 10 # the nose camera, the weights, one CSV
+ tools/moss_litter_probe.py --seconds 10 --expect 1.00 0.00 # the can is 1.00 m ahead on the axis
+ tools/moss_litter_probe.py --fake --frames 20 # no camera, no torch: a synthetic frame, a fake detector
+
+Options: --weights (MOSS_LITTER_WEIGHTS, ~/.local/state/moss/models/litter.pt),
+--classes "Metal,Plastic" (MOSS_LITTER_CLASSES: the ones the mission picks up),
+--device cpu|cuda (MOSS_LITTER_DEVICE), --conf 0.5 --iou 0.6 (the detector's own
+defaults in the mission), --width 640 --height 480 --fps 15 (the camera),
+--camera-xyz "x y z" and --pitch-deg (the robot profile's MOSS_CAMERA_XYZ_M and
+MOSS_CAMERA_PITCH_DEG otherwise), --out <csv> (default
+<MOSS_STATE_DIR>/litter_probe/litter_probe_<stamp>.csv). It writes the CSV and
+nothing else: no motor, no bus.
+
+The geometry: the colour pixel and the aligned depth give a point in the
+camera's optical frame (x right, y down, z forward); the mechanical camera
+frame is x forward, y left, z up (x = Z, y = -X, z = -Y); the mount is a pure
+pitch about +y, positive = nose down (blueprints.camera_mount_xyzq), then the
+camera's position in base_link. The depth of a box is the median of the
+non-zero depths in its central third (edges and holes left out).
+"""
+from __future__ import annotations
+
+import argparse
+import csv
+import math
+import os
+import statistics
+import sys
+import time
+from pathlib import Path
+
+ROOT = Path(__file__).resolve().parent.parent
+sys.path.insert(0, str(ROOT))
+
+from moss_dimos.robot_profile import moss_profile, moss_state_dir # noqa: E402
+
+try: # the mission's own names and defaults; dimOS is not needed here, so fall back without it
+ from moss_dimos.litter_detect import LITTER_CLASSES_DEFAULT, litter_classes, weights_path
+except Exception: # noqa: BLE001 (ImportError, or dimos' own import errors)
+ LITTER_CLASSES_DEFAULT = "Glass,Metal,Paper,Plastic,Waste"
+
+ def weights_path(env=None):
+ env = os.environ if env is None else env
+ return Path(env.get("MOSS_LITTER_WEIGHTS", "").strip() or "~/.local/state/moss/models/litter.pt").expanduser()
+
+ def litter_classes(env=None):
+ env = os.environ if env is None else env
+ raw = env.get("MOSS_LITTER_CLASSES", "").strip() or LITTER_CLASSES_DEFAULT
+ return {c.strip() for c in raw.split(",") if c.strip()}
+
+CONF_DEFAULT = 0.5 # the detector's own thresholds in litter_detect
+IOU_DEFAULT = 0.6
+CSV_FIELDS = ("frame", "t_s", "latency_ms", "cls", "conf", "u1", "v1", "u2", "v2", "depth_m", "x_m", "y_m", "z_m")
+
+# the fake camera: one object at a known place, so the whole chain has a known answer
+FAKE_INTR = (400.0, 400.0, 320.0, 240.0) # fx, fy, cx, cy
+FAKE_PIXEL = (360, 300) # the object's centre: 40 px right, 60 px down of the centre
+FAKE_DEPTH_M = 1.0
+FAKE_CLASS = "Metal"
+FAKE_CONF = 0.83
+FAKE_BOX_HALF = 20 # px
+
+
+# ---------------------------------------------------------------- geometry (pure)
+
+def deproject(u: float, v: float, depth_m: float, intr) -> tuple[float, float, float]:
+ """Pixel (u, v) at depth_m -> the point in the camera's optical frame (X right, Y down, Z forward)."""
+ fx, fy, cx, cy = intr
+ return ((u - cx) / fx * depth_m, (v - cy) / fy * depth_m, depth_m)
+
+
+def optical_to_base(p, camera_xyz, pitch_deg: float = 0.0) -> tuple[float, float, float]:
+ """Optical frame -> base_link: mechanical camera frame (x = Z, y = -X, z = -Y), the mount's pitch
+ about +y (positive = nose down, the convention of blueprints.camera_mount_xyzq), then the camera's
+ position in base_link."""
+ xc, yc, zc = p[2], -p[0], -p[1]
+ th = math.radians(pitch_deg)
+ xr = math.cos(th) * xc + math.sin(th) * zc
+ zr = -math.sin(th) * xc + math.cos(th) * zc
+ return (camera_xyz[0] + xr, camera_xyz[1] + yc, camera_xyz[2] + zr)
+
+
+def box_centre(box) -> tuple[float, float]:
+ u1, v1, u2, v2 = box
+ return ((u1 + u2) / 2.0, (v1 + v2) / 2.0)
+
+
+def box_depth_m(depth, box, depth_scale: float) -> float | None:
+ """The median of the non-zero depths in the box's central third, in metres; None when the
+ patch has no depth (a hole, or the box out of the image)."""
+ import numpy as np
+
+ h, w = depth.shape[:2]
+ u1, v1, u2, v2 = box
+ du, dv = (u2 - u1) / 3.0, (v2 - v1) / 3.0
+ a, b = int(max(0, math.floor(u1 + du))), int(min(w, math.ceil(u2 - du)))
+ c, d = int(max(0, math.floor(v1 + dv))), int(min(h, math.ceil(v2 - dv)))
+ if b <= a or d <= c:
+ return None
+ patch = np.asarray(depth[c:d, a:b], dtype=np.float64)
+ vals = patch[patch > 0]
+ if vals.size == 0:
+ return None
+ return float(np.median(vals)) * float(depth_scale)
+
+
+def parse_xyz(raw: str) -> tuple[float, float, float]:
+ vals = [float(t) for t in raw.replace(",", " ").split()]
+ if len(vals) != 3:
+ raise ValueError(f"three numbers expected, got {raw!r}")
+ return (vals[0], vals[1], vals[2])
+
+
+# ---------------------------------------------------------------- the summary (pure)
+
+def _median(xs):
+ return statistics.median(xs) if xs else None
+
+
+def _spread(xs):
+ """Half the 16-84 percentile span: one sigma for a normal spread, robust to a stray box."""
+ if len(xs) < 2:
+ return 0.0
+ s = sorted(xs)
+ lo = s[int(round(0.16 * (len(s) - 1)))]
+ hi = s[int(round(0.84 * (len(s) - 1)))]
+ return (hi - lo) / 2.0
+
+
+def summarise(rows, frames: int, expect=None) -> dict:
+ """rows: dicts with the CSV fields (one per detection); frames: frames looked at.
+ -> {frames, hit_frames, hit_rate, latency_ms, classes: {name: {n, conf, x, y, z, sx, sy}}, error}"""
+ hit = {r["frame"] for r in rows if r.get("x_m") is not None}
+ by = {}
+ for r in rows:
+ by.setdefault(r["cls"], []).append(r)
+ classes = {}
+ for name, rs in by.items():
+ xs = [r["x_m"] for r in rs if r.get("x_m") is not None]
+ ys = [r["y_m"] for r in rs if r.get("y_m") is not None]
+ zs = [r["z_m"] for r in rs if r.get("z_m") is not None]
+ classes[name] = {"n": len(rs), "no_depth": sum(1 for r in rs if r.get("x_m") is None),
+ "conf": _median([r["conf"] for r in rs]),
+ "x": _median(xs), "y": _median(ys), "z": _median(zs), "sx": _spread(xs), "sy": _spread(ys)}
+ lat = _median([r["latency_ms"] for r in rows]) if rows else None
+ out = {"frames": frames, "hit_frames": len(hit), "hit_rate": (len(hit) / frames) if frames else 0.0,
+ "latency_ms": lat, "classes": classes, "error": None}
+ if expect is not None and classes:
+ # the error of the most seen class against the tape measure
+ best = max(classes.values(), key=lambda c: c["n"])
+ if best["x"] is not None and best["y"] is not None:
+ dx, dy = best["x"] - expect[0], best["y"] - expect[1]
+ out["error"] = {"dx": dx, "dy": dy, "dist": math.hypot(dx, dy)}
+ return out
+
+
+def summary_lines(s: dict, expect=None) -> list[str]:
+ lines = [f"frames {s['frames']}, with a detection {s['hit_frames']} (hit rate {100 * s['hit_rate']:.0f} %)"
+ + (f", detector {s['latency_ms']:.0f} ms a frame (median)" if s["latency_ms"] is not None else "")]
+ for name, c in sorted(s["classes"].items(), key=lambda kv: -kv[1]["n"]):
+ if c["x"] is None:
+ lines.append(f" {name}: {c['n']} boxes, no depth under any of them")
+ continue
+ lines.append(f" {name}: {c['n']} boxes, conf {c['conf']:.2f}, at x {c['x']:+.3f} y {c['y']:+.3f} z {c['z']:+.3f} m"
+ f" (spread {c['sx']:.3f} / {c['sy']:.3f})" + (f", {c['no_depth']} without depth" if c["no_depth"] else ""))
+ if expect is not None:
+ e = s["error"]
+ if e is None:
+ lines.append(f" expected x {expect[0]:+.3f} y {expect[1]:+.3f}: nothing to compare")
+ else:
+ lines.append(f" against the tape (x {expect[0]:+.3f} y {expect[1]:+.3f}): dx {e['dx']:+.3f} dy {e['dy']:+.3f}"
+ f" m, {e['dist'] * 100:.1f} cm off")
+ return lines
+
+
+# ---------------------------------------------------------------- cameras and detectors
+
+class FakeCamera:
+ """One synthetic 640x480 frame: a grey floor, a box of another grey at FAKE_PIXEL, depth FAKE_DEPTH_M
+ there and a farther floor elsewhere."""
+
+ def __init__(self):
+ self.intr = FAKE_INTR
+ self.depth_scale = 0.001
+
+ def frames(self, n: int):
+ import numpy as np
+
+ for _ in range(n):
+ color = np.full((480, 640, 3), 120, dtype=np.uint8)
+ depth = np.full((480, 640), 2500, dtype=np.uint16)
+ u, v = FAKE_PIXEL
+ h = FAKE_BOX_HALF
+ color[v - h:v + h, u - h:u + h] = 40
+ depth[v - h:v + h, u - h:u + h] = int(round(FAKE_DEPTH_M / self.depth_scale))
+ depth[v - 2:v + 2, u - 2:u + 2] = 0 # a hole in the middle: the median must survive it
+ yield color, depth
+
+ def close(self):
+ pass
+
+
+class FakeDetector:
+ names = {0: FAKE_CLASS}
+
+ def detect(self, color):
+ u, v = FAKE_PIXEL
+ h = FAKE_BOX_HALF
+ return [(FAKE_CLASS, FAKE_CONF, (u - h, v - h, u + h, v + h))]
+
+
+class RealSenseCamera:
+ """Colour and depth aligned to colour, from the nose camera, via pyrealsense2."""
+
+ def __init__(self, width: int, height: int, fps: int):
+ import pyrealsense2 as rs
+
+ self._rs = rs
+ self.pipe = rs.pipeline()
+ cfg = rs.config()
+ cfg.enable_stream(rs.stream.color, width, height, rs.format.bgr8, fps)
+ cfg.enable_stream(rs.stream.depth, width, height, rs.format.z16, fps)
+ prof = self.pipe.start(cfg)
+ self.depth_scale = prof.get_device().first_depth_sensor().get_depth_scale()
+ i = prof.get_stream(rs.stream.color).as_video_stream_profile().get_intrinsics()
+ self.intr = (i.fx, i.fy, i.ppx, i.ppy)
+ self.align = rs.align(rs.stream.color)
+
+ def frames(self, n: int):
+ import numpy as np
+
+ for _ in range(n):
+ fs = self.align.process(self.pipe.wait_for_frames(5000))
+ c, d = fs.get_color_frame(), fs.get_depth_frame()
+ if not c or not d:
+ continue
+ yield np.asanyarray(c.get_data()), np.asanyarray(d.get_data())
+
+ def close(self):
+ try:
+ self.pipe.stop()
+ except Exception: # noqa: BLE001
+ pass
+
+
+class YoloDetector:
+ """The mission's detector, standalone: ultralytics YOLO on the weights, the mission's thresholds."""
+
+ def __init__(self, weights: Path, device: str | None, conf: float, iou: float):
+ from ultralytics import YOLO
+
+ self.model = YOLO(str(weights))
+ self.names = self.model.names
+ self.device, self.conf, self.iou = device, conf, iou
+
+ def detect(self, color):
+ kw = {"conf": self.conf, "iou": self.iou, "verbose": False}
+ if self.device:
+ kw["device"] = self.device
+ res = self.model.predict(color, **kw)
+ out = []
+ for r in res:
+ b = r.boxes
+ if b is None or len(b) == 0:
+ continue
+ xyxy = b.xyxy.cpu().numpy()
+ confs = b.conf.cpu().numpy()
+ clss = b.cls.cpu().numpy()
+ for i in range(len(confs)):
+ name = self.names.get(int(clss[i]), str(int(clss[i]))) if isinstance(self.names, dict) else str(int(clss[i]))
+ out.append((str(name), float(confs[i]), tuple(float(x) for x in xyxy[i])))
+ return out
+
+
+# ---------------------------------------------------------------- the run
+
+def default_out(env=None) -> Path:
+ return moss_state_dir(env) / "litter_probe" / ("litter_probe_" + time.strftime("%Y%m%d_%H%M%S") + ".csv")
+
+
+def run(camera, detector, wanted: set[str], camera_xyz, pitch_deg: float, out: Path, frames: int,
+ seconds: float, expect=None, say=print) -> dict:
+ """Frames through the detector, one CSV row per wanted detection, the summary printed and returned."""
+ wanted_l = {w.lower() for w in wanted}
+ out.parent.mkdir(parents=True, exist_ok=True)
+ rows, n_frames, t0 = [], 0, time.monotonic()
+ with out.open("w", newline="") as fh:
+ w = csv.DictWriter(fh, fieldnames=CSV_FIELDS)
+ w.writeheader()
+ for color, depth in camera.frames(frames):
+ t = time.monotonic() - t0
+ if seconds and t > seconds:
+ break
+ n_frames += 1
+ ta = time.monotonic()
+ dets = detector.detect(color)
+ lat = (time.monotonic() - ta) * 1000.0
+ seen = []
+ for name, conf, box in dets:
+ if wanted_l and name.lower() not in wanted_l:
+ continue
+ d = box_depth_m(depth, box, camera.depth_scale)
+ x = y = z = None
+ if d is not None:
+ u, v = box_centre(box)
+ x, y, z = optical_to_base(deproject(u, v, d, camera.intr), camera_xyz, pitch_deg)
+ row = {"frame": n_frames, "t_s": round(t, 3), "latency_ms": round(lat, 1), "cls": name,
+ "conf": round(conf, 3), "u1": round(box[0], 1), "v1": round(box[1], 1),
+ "u2": round(box[2], 1), "v2": round(box[3], 1),
+ "depth_m": None if d is None else round(d, 3),
+ "x_m": None if x is None else round(x, 3), "y_m": None if y is None else round(y, 3),
+ "z_m": None if z is None else round(z, 3)}
+ rows.append(row)
+ w.writerow(row)
+ seen.append(f"{name} {conf:.2f}" + (f" at x {x:+.2f} y {y:+.2f}" if x is not None else " (no depth)"))
+ say(f"frame {n_frames:3d} t {t:5.1f} s {lat:5.0f} ms " + ("; ".join(seen) if seen else "nothing"))
+ s = summarise(rows, n_frames, expect)
+ say("")
+ for line in summary_lines(s, expect):
+ say(line)
+ say(f"csv {out}")
+ return s
+
+
+def main(argv=None) -> int:
+ ap = argparse.ArgumentParser(description="Measure the litter detector on the floor: hits and positions.")
+ ap.add_argument("--fake", action="store_true", help="no camera, no torch: a synthetic frame and a fake detector")
+ ap.add_argument("--frames", type=int, default=0, help="frames to look at (default: until --seconds)")
+ ap.add_argument("--seconds", type=float, default=10.0, help="stop after this long (default 10 s; 0 = frames only)")
+ ap.add_argument("--expect", type=float, nargs=2, metavar=("X", "Y"),
+ help="where the tape says the litter is, in base_link metres (x forward, y left)")
+ ap.add_argument("--weights", default=None, help=f"YOLO weights (default {weights_path()})")
+ ap.add_argument("--classes", default=None, help="class names the mission picks up (default MOSS_LITTER_CLASSES)")
+ ap.add_argument("--device", default=None, help="cpu or cuda (default MOSS_LITTER_DEVICE, else the detector's choice)")
+ ap.add_argument("--conf", type=float, default=CONF_DEFAULT)
+ ap.add_argument("--iou", type=float, default=IOU_DEFAULT)
+ ap.add_argument("--width", type=int, default=640)
+ ap.add_argument("--height", type=int, default=480)
+ ap.add_argument("--fps", type=int, default=15)
+ ap.add_argument("--camera-xyz", default=None, help='camera in base_link, "x y z" m (default: the robot profile)')
+ ap.add_argument("--pitch-deg", type=float, default=None, help="camera pitch, + = nose down (default: the profile)")
+ ap.add_argument("--out", default=None, help="CSV path")
+ a = ap.parse_args(argv)
+
+ profile = moss_profile()
+ camera_xyz = parse_xyz(a.camera_xyz) if a.camera_xyz else profile.camera_xyz_base_m
+ pitch = profile.camera_pitch_deg if a.pitch_deg is None else a.pitch_deg
+ wanted = {c.strip() for c in a.classes.split(",") if c.strip()} if a.classes else litter_classes()
+ out = Path(a.out).expanduser() if a.out else default_out()
+ frames = a.frames if a.frames > 0 else (10 ** 9 if a.seconds else 0)
+ if frames == 0:
+ print("nothing to do: give --frames or --seconds", file=sys.stderr)
+ return 2
+
+ if a.fake:
+ camera, detector = FakeCamera(), FakeDetector()
+ print(f"fake camera and detector: one {FAKE_CLASS} at pixel {FAKE_PIXEL}, depth {FAKE_DEPTH_M} m")
+ else:
+ weights = Path(a.weights).expanduser() if a.weights else weights_path()
+ if not weights.is_file():
+ print(f"no weights at {weights} (MOSS_LITTER_WEIGHTS; docs/litter.md says where to get them)", file=sys.stderr)
+ return 2
+ try:
+ detector = YoloDetector(weights, a.device or os.environ.get("MOSS_LITTER_DEVICE", "").strip() or None,
+ a.conf, a.iou)
+ except ImportError as exc:
+ print(f"ultralytics not importable: {exc} (docs/litter.md: torch + ultralytics in the robot's venv)", file=sys.stderr)
+ return 2
+ try:
+ camera = RealSenseCamera(a.width, a.height, a.fps)
+ except ImportError as exc:
+ print(f"pyrealsense2 not importable: {exc} (Software.md)", file=sys.stderr)
+ return 2
+ except Exception as exc: # noqa: BLE001
+ print(f"the RealSense did not open: {exc} (is the stack up? tools/moss-fly.sh stop first)", file=sys.stderr)
+ return 2
+ print(f"nose camera {a.width}x{a.height} at {a.fps} fps, weights {weights.name}, classes {sorted(wanted)}")
+ print(f"camera in base_link {camera_xyz}, pitch {pitch:.1f} deg (+ = nose down)")
+ try:
+ run(camera, detector, wanted, camera_xyz, pitch, out, frames, a.seconds, a.expect)
+ finally:
+ camera.close()
+ return 0
+
+
+if __name__ == "__main__":
+ sys.exit(main())