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.pynew file mode 100644index 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.mdindex 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.oruv 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 thediff --git a/tools/moss_litter_probe.py b/tools/moss_litter_probe.pynew file mode 100755index 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())