diff --git a/.gitignore b/.gitignore index 9b49e8e..eb19fa6 100644 --- a/.gitignore +++ b/.gitignore @@ -15,3 +15,6 @@ scratchpad* # Sim benchmark outputs (timestamped metrics/reports) benchmark_results/ + +# Bag-replay scoring outputs (timestamped metrics/reports/maps) +bag_replay_results/ diff --git a/mote_simulation/tools/bag_replay/README.md b/mote_simulation/tools/bag_replay/README.md new file mode 100644 index 0000000..084c399 --- /dev/null +++ b/mote_simulation/tools/bag_replay/README.md @@ -0,0 +1,114 @@ +# Bag-replay scoring + +Offline harness that replays a **real recorded mapping bag** through SLAM (or +kinematic_icp) under two or more parameter sets and scores the results with +**truth-free** proxies, then writes a side-by-side comparison report (metrics +table + rendered maps). + +This is the reality check that complements the sim benchmark +(`mote_simulation/tools/benchmark/`, `pixi run bench`). The sim proves +parameters against *simulated* sensors and can report absolute error because +Gazebo publishes the robot's true pose. A real bag carries no ground truth, so +this harness scores *self-consistency* and *map appearance* instead — see +[Limitations](#limitations). The two share one metrics module: the truth-free +functions used here (`map_quality`, `loop_drift`, `path_length`) live in the +benchmark's `metrics.py`, reused not forked. + +## Usage + +```bash +pixi run bag-replay -- \ + --bag ~/.mote/bags/mapping/ \ + --params mote_bringup/config/slam_toolbox_params.yaml \ + mote_simulation/tools/bag_replay/examples/sparse_no_loop.yaml +``` + +- `--bag` — a recorded **mapping** stream bag directory (`/tf`, `/tf_static`, + `/scan_filtered`). These are recorded by default on every mapping session + (`mapping_launch.py` → `record_launch.py streams:=mapping`) and stamped into + the map revision by `save-map`; `pixi run site info` shows which bag built a + map. Bags live under the robot's `~/.mote/bags/mapping/` — copy one over to + score it (no code is synced *to* the Pi). +- `--params a.yaml b.yaml …` — one **complete** `slam_toolbox` params file per + set (slam mode). Default: the committed `slam_toolbox_params.yaml` as a single + baseline. Compare it against `examples/sparse_no_loop.yaml` (a variant with + aggressive scan decimation + loop closure off) for a ready two-set example. +- `--mode slam|icp` — `slam` (default) scores the map + map-frame trajectory; + `icp` scores kinematic_icp's odometry self-consistency (no map). ICP params + are inline in the launch, so in icp mode `--params` are just repeat labels. +- `--rate` — replay speed × realtime (default 1.0). 2–3× is safe on a + workstation; too fast and SLAM can drop scans in wall time. +- `--max-scans N` — stop after N scans (debug/quick smoke). + +Output lands in `bag_replay_results//`: `report.md` (open it — the map +images are relative links), `run.json` (full metrics + provenance), and per set +a `stack.log`, `replay.log`, `series.json` (re-scorable trajectory), `map.npz` +(occupancy grid) and `map.png`. + +## How it works + +`replay.py` (orchestrator) runs each parameter set in its **own freshly-launched +stack under a random `ROS_DOMAIN_ID`** — so a stray node can never reach a live +robot, and sequential runs can't leak into each other (the sim sweep learned +that DDS-isolation lesson the hard way). Per set it: + +1. launches `replay_stack_launch.py` (env packages only — `slam_toolbox` / + `kinematic_icp` / `nav2_lifecycle_manager`, no workspace build needed), +2. runs `replayer.py`, which streams the bag's sensor + TF messages back onto + the graph in **sim-time order** (it drives `/clock` from the bag stamps, so + the stack runs on bag time regardless of wall speed), records the estimator's + output trajectory, and grabs the finished `/map`, +3. scores with the shared `metrics` module and renders the map (`render.py`), +4. tears the stack down and moves on. `report.py` then builds the comparison. + +**TF ownership** is the one subtlety. A mapping bag's `/tf` already contains the +edges the *original* run produced — `map→odom` (slam_toolbox) and +`odom→base_footprint` (kinematic_icp). Replaying those verbatim would fight the +fresh node publishing the same edge, so the edge the node-under-test owns is +stripped from the replayed `/tf`; everything else passes through. In slam mode +the recorded `odom→base` is kept and *fed* to slam as its odometry prior, +exactly as on the robot — so a slam-parameter comparison replays against +identical odometry. + +## Metrics (all truth-free) + +- **loop drift** (`loop_drift`) — start↔end distance of the estimated + trajectory and its ratio to path length. Small on a bag where the robot + physically returned to its start ⇒ little accumulated drift. +- **map crispness / coverage** (`map_quality`, slam mode) — from the finished + occupancy grid: `mean_wall_thickness_m` (iterated erosion; blur/double-walls + read thicker — lower is crisper), `speckle_frac` (isolated occupied cells — + scan-match noise, lower is cleaner), `unknown/free/occ_frac`, and + `explored_area_m2`. + +## Limitations + +These are **proxies, not error measures** — be honest about what they can and +cannot prove versus the sim's ground truth: + +- **No absolute accuracy.** Without a surveyed reference the bag cannot tell you + the map is metrically *correct*, only whether it is self-consistent and looks + clean. For metric-accuracy claims (ATE), use the sim benchmark. +- **Loop drift needs a loop.** It cannot distinguish a legitimate open A→B + traverse (honestly large end distance) from a drifting loop. Know the bag's + shape before reading it. +- **Crispness ≠ correctness.** A confidently *wrong* map — e.g. a mis-closed + loop drawn with sharp walls — can score well on wall thickness and speckle. The + crispness proxies catch blur, noise, and incompleteness, not global error. +- **Not bit-exact.** The recorded sensor stream makes the *input* deterministic, + but SLAM's solver is not bit-identical run to run; treat small deltas as noise + and lean on the map images for anything marginal. + +## Tests + +`test_bag_replay.py` and the benchmark's `test_metrics.py` cover the ROS-free +pieces (metrics, scoring, rendering, report, domain-ID isolation) with no ROS +graph, so the test path can never touch a live robot. Run them with: + +```bash +python mote_simulation/tools/bag_replay/test_bag_replay.py +python mote_simulation/tools/benchmark/test_metrics.py +``` + +The ROS replay itself needs `slam_toolbox` and a real bag and is exercised by +`pixi run bag-replay`, not by the unit tests. diff --git a/mote_simulation/tools/bag_replay/examples/sparse_no_loop.yaml b/mote_simulation/tools/bag_replay/examples/sparse_no_loop.yaml new file mode 100644 index 0000000..057d885 --- /dev/null +++ b/mote_simulation/tools/bag_replay/examples/sparse_no_loop.yaml @@ -0,0 +1,73 @@ +slam_toolbox: + ros__parameters: + + # Frames + odom_frame: odom + map_frame: map + base_frame: base_link + scan_topic: /scan_filtered + mode: mapping + + # Solver + solver_plugin: solver_plugins::CeresSolver + ceres_linear_solver: SPARSE_NORMAL_CHOLESKY + ceres_preconditioner: SCHUR_JACOBI + ceres_trust_strategy: LEVENBERG_MARQUARDT + ceres_dogleg_type: TRADITIONAL_DOGLEG + ceres_loss_function: None + + # Map + resolution: 0.05 + map_update_interval: 5.0 + + # Laser — RPLIDAR C1: 12 m range, 10 Hz + max_laser_range: 16.0 + minimum_time_interval: 0.5 + + # TF + transform_publish_period: 0.02 + transform_timeout: 0.2 + tf_buffer_duration: 30.0 + + # Scan matching — aggressive decimation: integrate far fewer scans. + use_scan_matching: true + use_scan_barycenter: true + minimum_travel_distance: 0.8 + minimum_travel_heading: 0.8 + scan_buffer_size: 10 + scan_buffer_maximum_scan_distance: 10.0 + link_match_minimum_response_fine: 0.1 + link_scan_maximum_distance: 1.5 + + # Loop closure — disabled, so accumulated drift is never corrected. + do_loop_closing: false + loop_search_maximum_distance: 3.0 + loop_match_minimum_chain_size: 10 + loop_match_maximum_variance_coarse: 3.0 + loop_match_minimum_response_coarse: 0.35 + loop_match_minimum_response_fine: 0.45 + + # Correlation + correlation_search_space_dimension: 0.5 + correlation_search_space_resolution: 0.01 + correlation_search_space_smear_deviation: 0.1 + + # Loop search space + loop_search_space_dimension: 8.0 + loop_search_space_resolution: 0.05 + loop_search_space_smear_deviation: 0.03 + + # Penalties + distance_variance_penalty: 0.5 + angle_variance_penalty: 1.0 + fine_search_angle_offset: 0.00349 + coarse_search_angle_offset: 0.349 + coarse_angle_resolution: 0.0349 + minimum_angle_penalty: 0.9 + minimum_distance_penalty: 0.5 + use_response_expansion: false + + debug_logging: false + throttle_scans: 1 + stack_size_to_use: 40000000 + enable_interactive_mode: true diff --git a/mote_simulation/tools/bag_replay/render.py b/mote_simulation/tools/bag_replay/render.py new file mode 100644 index 0000000..3f150ea --- /dev/null +++ b/mote_simulation/tools/bag_replay/render.py @@ -0,0 +1,55 @@ +"""Render a replayed occupancy grid to a PNG, optionally with the trajectory. + +Uses cv2 (already a project dependency — map_cleanup and sites.py save maps with +it), matching map_saver's convention: occupied black, free white, unknown grey, +north up (grid row 0 is the bottom, so the image is flipped vertically). The +estimator trajectory, if given, is drawn in the map frame over the grid. +""" + +from __future__ import annotations + +from pathlib import Path + +import cv2 +import numpy as np + +OCC_THRESH = 65 +FREE_THRESH = 25 + + +def grid_to_bgr(grid: np.ndarray) -> np.ndarray: + """int8 occupancy grid (0..100, -1 unknown) -> flipped BGR image.""" + img = np.full(grid.shape, 205, dtype=np.uint8) # unknown grey (map_saver value) + img[(grid >= 0) & (grid <= FREE_THRESH)] = 254 # free + img[grid >= OCC_THRESH] = 0 # occupied + img = np.flipud(img) # ROS grid origin is bottom-left; image north is up + return cv2.cvtColor(img, cv2.COLOR_GRAY2BGR) + + +def render_map(npz_path, out_png, traj=None) -> None: + data = np.load(npz_path) + grid = data["grid"] + res = float(data["resolution"]) + ox, oy = (float(v) for v in data["origin"]) + h = grid.shape[0] + img = grid_to_bgr(grid) + + if traj: + pts = [] + for row in traj: + _, x, y = row[0], row[1], row[2] + px = int(round((x - ox) / res)) + py = int(round((y - oy) / res)) + py = h - 1 - py # match the vertical flip + pts.append([px, py]) + if len(pts) >= 2: + cv2.polylines(img, [np.array(pts, dtype=np.int32)], False, (0, 128, 255), 1) + cv2.circle(img, tuple(pts[0]), 3, (0, 200, 0), -1) # start green + cv2.circle(img, tuple(pts[-1]), 3, (0, 0, 255), -1) # end red + + # Upscale small maps so features are legible in the report. + scale = max(1, int(round(900 / max(img.shape[:2])))) + if scale > 1: + img = cv2.resize(img, None, fx=scale, fy=scale, interpolation=cv2.INTER_NEAREST) + Path(out_png).parent.mkdir(parents=True, exist_ok=True) + cv2.imwrite(str(out_png), img) diff --git a/mote_simulation/tools/bag_replay/replay.py b/mote_simulation/tools/bag_replay/replay.py new file mode 100755 index 0000000..f872957 --- /dev/null +++ b/mote_simulation/tools/bag_replay/replay.py @@ -0,0 +1,315 @@ +#!/usr/bin/env python3 +"""Bag-replay scoring harness: validate parameter choices against real recordings. + +The reality check that complements the sim benchmark. The sim proves parameters +against *simulated* sensors with Gazebo ground truth; this replays a *real* +recorded mapping bag through SLAM (or kinematic_icp) under two or more parameter +sets and scores the results with truth-free proxies — no ground truth needed — +then writes a side-by-side comparison report with a metrics table and rendered +map images. + + pixi run bag-replay -- --bag ~/.mote/bags/mapping/ \\ + --params a.yaml b.yaml + +Each parameter set is replayed in its own freshly-launched stack under a random +``ROS_DOMAIN_ID`` (so a stray node can never reach a live robot, and sequential +runs can't leak into each other — the sim sweep learned that lesson). The +per-replay ROS work lives in ``replayer.py`` (fresh rclpy context per set); the +metric maths is the sim benchmark's ROS-free ``metrics`` module, reused not +forked; rendering and the report are local siblings. + +The stack is launched from ``replay_stack_launch.py`` (env packages only), so the +harness runs against a bag without the workspace being built. +""" + +from __future__ import annotations + +import argparse +import json +import os +import random +import signal +import subprocess +import sys +import time +from datetime import datetime, timezone +from pathlib import Path + +HERE = Path(__file__).resolve().parent +REPO = HERE.parents[2] +BRINGUP = REPO / "mote_bringup" +DEFAULT_SLAM_PARAMS = BRINGUP / "config" / "slam_toolbox_params.yaml" +STACK_LAUNCH = HERE / "replay_stack_launch.py" + +sys.path.insert(0, str(REPO / "mote_simulation" / "tools" / "benchmark")) +import metrics # noqa: E402 + +sys.path.insert(0, str(HERE)) +import render # noqa: E402 +import report # noqa: E402 + +# Readiness needle in the launch log, per mode. slam autostarts via +# nav2_lifecycle_manager, which announces activation; icp has no lifecycle. +READY_NEEDLE = {"slam": "Managed nodes are active", "icp": None} +KILL_NAMES = ( + "async_slam_toolbox_node", + "kinematic_icp_online_node", + "lifecycle_manager", +) + + +def log(msg): + print(f"[bag-replay] {msg}", flush=True) + + +def popen_group(cmd, log_path, env): + f = open(log_path, "wb") + p = subprocess.Popen( + cmd, stdout=f, stderr=subprocess.STDOUT, start_new_session=True, env=env + ) + return p, f + + +def kill_group(p): + if p and p.poll() is None: + try: + os.killpg(os.getpgid(p.pid), signal.SIGTERM) + except (ProcessLookupError, PermissionError): + pass + + +def wait_for_ready(log_path, needle, proc, timeout_s, min_wait=5.0): + """Block until ``needle`` appears (or a fixed settle if ``needle`` is None), + failing fast if the launch process dies. Returns (ok, reason).""" + deadline = time.monotonic() + timeout_s + started = time.monotonic() + while time.monotonic() < deadline: + if proc.poll() is not None: + return False, f"stack exited early (code {proc.returncode})" + text = ( + Path(log_path).read_text(errors="ignore") if Path(log_path).exists() else "" + ) + if needle is None: + if time.monotonic() - started >= min_wait: + return True, "settled" + elif needle in text: + return True, "ready" + time.sleep(1) + return False, f"timed out waiting for '{needle}'" + + +def teardown(procs, files, env): + for p in procs: + kill_group(p) + time.sleep(2) + for name in KILL_NAMES: + subprocess.run(["pkill", "-9", "-f", name], stderr=subprocess.DEVNULL, env=env) + subprocess.run( + ["ros2", "daemon", "stop"], + stderr=subprocess.DEVNULL, + stdout=subprocess.DEVNULL, + env=env, + ) + for f in files: + try: + f.close() + except OSError: + pass + + +def git_commit(): + r = subprocess.run( + ["git", "rev-parse", "--short", "HEAD"], + cwd=REPO, + capture_output=True, + text=True, + ) + return r.stdout.strip() or "unknown" + + +def isolated_env(): + """A child env pinned to a random high ROS_DOMAIN_ID so a stray node can + never touch a live robot and sequential replays can't leak into each other.""" + env = dict(os.environ) + env["ROS_DOMAIN_ID"] = str(random.randint(1, 232)) + return env + + +def score(series_path, map_npz): + """Compute truth-free metrics for one replay from its recorded outputs.""" + series = json.loads(Path(series_path).read_text()) + traj = series.get("traj", []) + out = { + "n_scans": series.get("n_scans", 0), + "traj_samples": len(traj), + "loop": metrics.loop_drift(traj), + } + if map_npz and Path(map_npz).exists(): + import numpy as np + + data = np.load(map_npz) + out["map"] = metrics.map_quality(data["grid"], float(data["resolution"])) + return out, traj + + +def run_one(bag, params_file, name, mode, run_dir, args): + """Launch the stack for one parameter set, replay the bag, score, render.""" + set_dir = run_dir / name + set_dir.mkdir(parents=True, exist_ok=True) + env = isolated_env() + stack_log = set_dir / "stack.log" + procs, files = [], [] + + subprocess.run( + ["ros2", "daemon", "stop"], + stderr=subprocess.DEVNULL, + stdout=subprocess.DEVNULL, + env=env, + ) + time.sleep(1) + try: + log(f"[{name}] launching {mode} stack (domain {env['ROS_DOMAIN_ID']})") + cmd = [ + "ros2", + "launch", + str(STACK_LAUNCH), + f"mode:={mode}", + "use_sim_time:=true", + ] + if mode == "slam": + cmd.append(f"slam_params_file:={params_file}") + stack_p, stack_f = popen_group(cmd, stack_log, env) + procs.append(stack_p) + files.append(stack_f) + + ok, reason = wait_for_ready( + stack_log, READY_NEEDLE[mode], stack_p, args.boot_timeout + ) + if not ok: + log(f"[{name}] FAIL: {reason}") + return None + log(f"[{name}] stack {reason}; replaying bag") + + rp = subprocess.run( + [ + sys.executable, + str(HERE / "replayer.py"), + "--bag", + str(bag), + "--mode", + mode, + "--out-dir", + str(set_dir), + "--rate", + str(args.rate), + "--settle", + str(args.settle), + "--max-scans", + str(args.max_scans), + ], + env=env, + timeout=args.replay_timeout, + ) + if rp.returncode != 0: + log(f"[{name}] FAIL: replayer exited {rp.returncode}") + return None + except subprocess.TimeoutExpired: + log(f"[{name}] FAIL: replay exceeded wall-clock timeout") + return None + finally: + teardown(procs, files, env) + time.sleep(2) + + map_npz = set_dir / "map.npz" + m, traj = score(set_dir / "series.json", map_npz) + map_png = None + if map_npz.exists(): + rel = f"{name}/map.png" + render.render_map(map_npz, run_dir / rel, traj=traj) + map_png = rel + return { + "name": name, + "params_file": str(params_file) if mode == "slam" else None, + "metrics": m, + "map_png": map_png, + "series_json": str((set_dir / "series.json").relative_to(run_dir)), + } + + +def main(): + ap = argparse.ArgumentParser(description=__doc__) + ap.add_argument("--bag", required=True, help="path to a recorded mapping bag dir") + ap.add_argument( + "--params", + nargs="*", + default=[], + help="slam_toolbox param YAMLs, one per set (slam mode). " + "Default: the committed slam_toolbox_params.yaml as a single baseline.", + ) + ap.add_argument("--mode", choices=("slam", "icp"), default="slam") + ap.add_argument("--rate", type=float, default=1.0, help="replay speed x realtime") + ap.add_argument("--settle", type=float, default=8.0) + ap.add_argument("--max-scans", type=int, default=0, help="0 = whole bag") + ap.add_argument("--out", default=str(REPO / "bag_replay_results")) + ap.add_argument("--boot-timeout", type=float, default=120.0) + ap.add_argument("--replay-timeout", type=float, default=3600.0) + args = ap.parse_args() + + bag = Path(args.bag).expanduser() + if not bag.exists(): + sys.exit(f"bag not found: {bag}") + + if args.mode == "slam": + param_files = [Path(p).expanduser() for p in args.params] or [ + DEFAULT_SLAM_PARAMS + ] + for p in param_files: + if not p.exists(): + sys.exit(f"param file not found: {p}") + sets = [(p.stem, p) for p in param_files] + else: + # icp params are inline in the launch; a set is just a repeat label. + labels = args.params or ["icp"] + sets = [(str(lbl), None) for lbl in labels] + # Disambiguate duplicate names (e.g. two files both named slam_toolbox_params). + seen = {} + uniq = [] + for name, pf in sets: + seen[name] = seen.get(name, 0) + 1 + uniq.append((f"{name}_{seen[name]}" if seen[name] > 1 else name, pf)) + sets = uniq + + ts = datetime.now(timezone.utc).strftime("%Y%m%dT%H%M%SZ") + run_dir = Path(args.out) / ts + run_dir.mkdir(parents=True, exist_ok=True) + log(f"results -> {run_dir}") + log(f"bag: {bag} mode: {args.mode} sets: {[n for n, _ in sets]}") + + provenance = { + "timestamp": ts, + "git_commit": git_commit(), + "bag": str(bag), + "mode": args.mode, + "rate": args.rate, + } + + results = [] + for name, pf in sets: + log(f"=== parameter set: {name} ===") + r = run_one(bag, pf, name, args.mode, run_dir, args) + if r is not None: + results.append(r) + + if not results: + log("no replays completed — check per-set stack.log / replay.log") + return 1 + + run = report.build_run(provenance, results) + (run_dir / "run.json").write_text(json.dumps(run, indent=2)) + (run_dir / "report.md").write_text(report.build_markdown(run)) + log(f"wrote {run_dir / 'report.md'} and run.json ({len(results)} set(s))") + return 0 + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/mote_simulation/tools/bag_replay/replay_stack_launch.py b/mote_simulation/tools/bag_replay/replay_stack_launch.py new file mode 100644 index 0000000..7c39c63 --- /dev/null +++ b/mote_simulation/tools/bag_replay/replay_stack_launch.py @@ -0,0 +1,88 @@ +"""Launch the SLAM/ICP stack under test for offline bag replay. + +Deliberately self-contained — it references only environment packages +(slam_toolbox, nav2_lifecycle_manager, kinematic_icp), never the mote_bringup +share, so it runs against a bag without the workspace being built and can be +launched by absolute path: + + ros2 launch mode:=slam slam_params_file:= + +It mirrors the robot's real slam_launch.py / localization_launch.py node setup, +but takes ``slam_params_file`` as an argument (the real launch hardcodes it) so a +parameter set can be swapped per replay. ``use_sim_time`` defaults true because +the replayer drives ``/clock`` from bag timestamps. The bag already carries the +``base_footprint->odom_wheel`` wheel-odom edge, so ICP needs no odom relay here. +""" + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, GroupAction +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration, PythonExpression +from launch_ros.actions import Node, SetParameter + + +def generate_launch_description(): + use_sim_time = LaunchConfiguration("use_sim_time") + mode = LaunchConfiguration("mode") + slam_params_file = LaunchConfiguration("slam_params_file") + + is_slam = IfCondition(PythonExpression(["'", mode, "' == 'slam'"])) + is_icp = IfCondition(PythonExpression(["'", mode, "' == 'icp'"])) + + slam = GroupAction( + condition=is_slam, + actions=[ + Node( + package="slam_toolbox", + executable="async_slam_toolbox_node", + name="slam_toolbox", + parameters=[slam_params_file], + output="screen", + ), + Node( + package="nav2_lifecycle_manager", + executable="lifecycle_manager", + name="lifecycle_manager_slam", + parameters=[ + { + "autostart": True, + "node_names": ["slam_toolbox"], + "bond_timeout": 0.0, + } + ], + output="screen", + ), + ], + ) + + icp = Node( + condition=is_icp, + package="kinematic_icp", + executable="kinematic_icp_online_node", + name="online_node", + namespace="kinematic_icp", + output="screen", + parameters=[ + { + "lidar_topic": "/scan_filtered", + "use_2d_lidar": True, + "lidar_odom_frame": "odom", + "wheel_odom_frame": "odom_wheel", + "base_frame": "base_footprint", + "publish_odom_tf": True, + "invert_odom_tf": False, + "tf_timeout": 0.05, + } + ], + ) + + return LaunchDescription( + [ + DeclareLaunchArgument("use_sim_time", default_value="true"), + DeclareLaunchArgument("mode", default_value="slam"), + DeclareLaunchArgument("slam_params_file", default_value=""), + SetParameter(name="use_sim_time", value=use_sim_time), + slam, + icp, + ] + ) diff --git a/mote_simulation/tools/bag_replay/replayer.py b/mote_simulation/tools/bag_replay/replayer.py new file mode 100755 index 0000000..8252db6 --- /dev/null +++ b/mote_simulation/tools/bag_replay/replayer.py @@ -0,0 +1,293 @@ +#!/usr/bin/env python3 +"""Replay one recorded mapping bag into a live SLAM/ICP node and capture outputs. + +Assumes the stack under test is already running in the same (isolated) ROS graph +— ``replay.py`` owns launching ``async_slam_toolbox_node`` (slam mode) or +``kinematic_icp_online_node`` (icp mode) and tearing it down. This process is the +ROS client for a single parameter set: it streams the bag's ``/scan_filtered`` + +``/tf`` + ``/tf_static`` back onto the graph in sim-time order (driving ``/clock`` +itself, so the node runs on bag time regardless of wall speed), records the +estimator's output trajectory, and grabs the finished ``/map``. + +The one subtlety is TF ownership. A mapping bag's ``/tf`` already contains the +edges the *original* run produced — ``map->odom`` from slam_toolbox and +``odom->base_footprint`` from kinematic_icp. Replaying those verbatim would fight +the fresh node publishing the same edge, so the edge the node-under-test owns is +stripped from the replayed ``/tf`` (see ``STRIP``); everything else is passed +through. In slam mode the recorded ``odom->base`` is kept and *fed* to slam as +its odometry prior, exactly as on the robot. + +Outputs (in ``--out-dir``): ``series.json`` (raw re-scorable trajectory) and, in +slam mode, ``map.npz`` (occupancy grid + resolution/origin for rendering and +map-quality metrics). Run one replay per process for a clean rclpy context. +""" + +from __future__ import annotations + +import argparse +import json +import sys +import time +from pathlib import Path + +import numpy as np +import rclpy +import tf2_ros +from nav_msgs.msg import OccupancyGrid +from rclpy.node import Node +from rclpy.serialization import deserialize_message +from rclpy.qos import QoSDurabilityPolicy, QoSProfile, QoSReliabilityPolicy +from rosgraph_msgs.msg import Clock +from sensor_msgs.msg import LaserScan +from tf2_msgs.msg import TFMessage + +import rosbag2_py + +# The TF edge the node under test publishes itself — stripped from the replayed +# /tf so the recorded copy can't fight the fresh one. (parent, child) pairs. +STRIP = { + "slam": {("map", "odom")}, + "icp": {("map", "odom"), ("odom", "base_footprint")}, +} + +# Which output edge to sample as the estimator's trajectory, per mode. +OUTPUT_EDGE = { + "slam": ("map", "base_link"), + "icp": ("odom", "base_footprint"), +} + +BAG_TOPICS = { + "/tf": TFMessage, + "/tf_static": TFMessage, + "/scan_filtered": LaserScan, +} + + +def yaw_of(q): + import math + + return math.atan2(2 * (q.w * q.z + q.x * q.y), 1 - 2 * (q.y * q.y + q.z * q.z)) + + +class Replayer(Node): + def __init__(self, mode, sample_dt): + super().__init__("bag_replayer") + self.set_parameters([rclpy.parameter.Parameter("use_sim_time", value=True)]) + self.mode = mode + self.strip = STRIP[mode] + self.out_parent, self.out_child = OUTPUT_EDGE[mode] + self.sample_dt = sample_dt + + self.clock_pub = self.create_publisher(Clock, "/clock", 10) + self.tf_pub = self.create_publisher(TFMessage, "/tf", 100) + static_qos = QoSProfile( + depth=1, + reliability=QoSReliabilityPolicy.RELIABLE, + durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, + ) + self.static_pub = self.create_publisher(TFMessage, "/tf_static", static_qos) + self.scan_pub = self.create_publisher(LaserScan, "/scan_filtered", 10) + + map_qos = QoSProfile( + depth=1, + reliability=QoSReliabilityPolicy.RELIABLE, + durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, + ) + self.create_subscription(OccupancyGrid, "/map", self.on_map, map_qos) + self.latest_map = None + + self.tf_buffer = tf2_ros.Buffer() + self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self) + + self.traj = [] # [sim_t, x, y, yaw] + self._static_frames = {} # (parent, child) -> TransformStamped + self._next_sample = None + + def on_map(self, msg): + self.latest_map = msg + + def publish_clock(self, t_ns): + c = Clock() + c.clock.sec = t_ns // 1_000_000_000 + c.clock.nanosec = t_ns % 1_000_000_000 + self.clock_pub.publish(c) + + def handle_tf_static(self, msg): + # Accumulate every static edge ever seen and republish the union latched, + # so a subscriber that joins late still gets the full static tree. + changed = False + for tr in msg.transforms: + key = (tr.header.frame_id, tr.child_frame_id) + if key not in self._static_frames: + self._static_frames[key] = tr + changed = True + if changed: + out = TFMessage() + out.transforms = list(self._static_frames.values()) + self.static_pub.publish(out) + + def handle_tf(self, msg): + kept = [ + tr + for tr in msg.transforms + if (tr.header.frame_id, tr.child_frame_id) not in self.strip + ] + if kept: + out = TFMessage() + out.transforms = kept + self.tf_pub.publish(out) + + def sample_output(self, sim_t): + try: + tf = self.tf_buffer.lookup_transform( + self.out_parent, self.out_child, rclpy.time.Time() + ) + except tf2_ros.TransformException: + return + tr = tf.transform.translation + self.traj.append([sim_t, tr.x, tr.y, yaw_of(tf.transform.rotation)]) + + +def read_bag(bag): + r = rosbag2_py.SequentialReader() + r.open( + rosbag2_py.StorageOptions(uri=str(bag), storage_id="mcap"), + rosbag2_py.ConverterOptions("", ""), + ) + while r.has_next(): + topic, data, t = r.read_next() + if topic in BAG_TOPICS: + yield topic, deserialize_message(data, BAG_TOPICS[topic]), t + + +def main(): + ap = argparse.ArgumentParser() + ap.add_argument("--bag", required=True) + ap.add_argument("--mode", choices=("slam", "icp"), default="slam") + ap.add_argument("--out-dir", required=True) + ap.add_argument("--rate", type=float, default=1.0, help="replay speed x realtime") + ap.add_argument( + "--sample-dt", type=float, default=0.1, help="traj sample period (s)" + ) + ap.add_argument( + "--settle", type=float, default=8.0, help="extra sim s after last scan" + ) + ap.add_argument( + "--max-scans", type=int, default=0, help="0 = whole bag (debug cap)" + ) + args = ap.parse_args() + + out = Path(args.out_dir) + out.mkdir(parents=True, exist_ok=True) + + rclpy.init() + node = Replayer(args.mode, args.sample_dt) + + # Give the node-under-test a moment to come up on the graph and discover us. + for _ in range(20): + rclpy.spin_once(node, timeout_sec=0.1) + + wall0 = None + t0_ns = None + last_ns = None + last_sample = -1e18 # sim-time of the last trajectory sample (gap-robust) + n_scans = 0 + stop = False + + for topic, msg, t_ns in read_bag(args.bag): + if t0_ns is None: + t0_ns = t_ns + wall0 = time.monotonic() + sim_t = (t_ns - t0_ns) / 1e9 + last_ns = t_ns + + # Pace to the requested fraction of realtime, spinning so /map and TF + # callbacks fire while we wait. + target = wall0 + sim_t / args.rate + while True: + dt = target - time.monotonic() + if dt <= 0: + break + rclpy.spin_once(node, timeout_sec=min(dt, 0.02)) + + node.publish_clock(t_ns) + if topic == "/tf_static": + node.handle_tf_static(msg) + elif topic == "/tf": + node.handle_tf(msg) + elif topic == "/scan_filtered": + node.scan_pub.publish(msg) + n_scans += 1 + rclpy.spin_once(node, timeout_sec=0.0) + + if sim_t - last_sample >= args.sample_dt: + node.sample_output(sim_t) + last_sample = sim_t + + if args.max_scans and n_scans >= args.max_scans: + stop = True + break + + if t0_ns is None: + print("FAIL: bag had no replayable topics", file=sys.stderr) + (out / "error.txt").write_text("bag had no /scan_filtered /tf /tf_static\n") + node.destroy_node() + rclpy.shutdown() + return 1 + + # Keep advancing sim time (no new scans) so slam fires a final map update and + # the last poses flush through TF. Sim time is advanced strictly in step with + # wall time * rate — spin_once returns instantly while slam publishes, so the + # loop must be paced by the wall clock, not by iteration count, or sim time + # would run away. + settle_start = time.monotonic() + settle_wall = args.settle / args.rate + while True: + elapsed = time.monotonic() - settle_start + if elapsed >= settle_wall: + break + cur_ns = last_ns + int(elapsed * args.rate * 1e9) + node.publish_clock(cur_ns) + rclpy.spin_once(node, timeout_sec=0.02) + sim_t = (cur_ns - t0_ns) / 1e9 + if sim_t - last_sample >= args.sample_dt: + node.sample_output(sim_t) + last_sample = sim_t + time.sleep(0.01) + + result = { + "mode": args.mode, + "bag": str(args.bag), + "truncated": bool(stop), + "n_scans": n_scans, + "traj": node.traj, + } + if node.latest_map is not None: + m = node.latest_map + grid = np.array(m.data, dtype=np.int16).reshape(m.info.height, m.info.width) + np.savez_compressed( + out / "map.npz", + grid=grid, + resolution=np.float64(m.info.resolution), + origin=np.array( + [m.info.origin.position.x, m.info.origin.position.y], dtype=np.float64 + ), + ) + result["map"] = { + "width": int(m.info.width), + "height": int(m.info.height), + "resolution": float(m.info.resolution), + } + (out / "series.json").write_text(json.dumps(result)) + print( + f"DONE {args.mode}: {n_scans} scans, {len(node.traj)} traj samples, " + f"map={'yes' if node.latest_map is not None else 'NONE'}", + flush=True, + ) + node.destroy_node() + rclpy.shutdown() + return 0 + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/mote_simulation/tools/bag_replay/report.py b/mote_simulation/tools/bag_replay/report.py new file mode 100644 index 0000000..754eee1 --- /dev/null +++ b/mote_simulation/tools/bag_replay/report.py @@ -0,0 +1,125 @@ +"""Build the comparative bag-replay report: markdown metrics table + map images. + +Truth-free by construction — a real bag carries no ground truth, so the report +scores self-consistency (loop drift) and map crispness (:mod:`metrics` +``map_quality``) side by side across parameter sets, and embeds each run's +rendered map so the numbers can be eyeballed. ROS-free: consumes the ``run`` +dict that ``replay.py`` assembles. +""" + +from __future__ import annotations + + +def _fmt(v, nd=3): + if v is None or (isinstance(v, float) and v != v): + return "—" + if isinstance(v, float): + return f"{v:.{nd}f}" + return str(v) + + +def _get(d, dotted): + cur = d + for k in dotted.split("."): + if not isinstance(cur, dict) or k not in cur: + return None + cur = cur[k] + return cur + + +# (label, dotted metric key, decimals, "lower"/"higher" is better) +ROWS = [ + ("scans replayed", "n_scans", 0, ""), + ("traj samples", "traj_samples", 0, ""), + ("path length (m)", "loop.path_length_m", 2, ""), + ("loop drift (m)", "loop.start_end_dist_m", 3, "lower"), + ("drift ratio", "loop.drift_ratio", 4, "lower"), + ("explored area (m²)", "map.explored_area_m2", 1, "higher"), + ("unknown frac", "map.unknown_frac", 3, ""), + ("occupied frac", "map.occ_frac", 4, ""), + ("wall thickness (m)", "map.mean_wall_thickness_m", 3, "lower"), + ("speckle frac", "map.speckle_frac", 4, "lower"), +] + + +def build_markdown(run) -> str: + p = run["provenance"] + results = run["results"] + lines = [ + "# Bag-replay scoring report", + "", + f"- **generated (UTC):** {p['timestamp']}", + f"- **git commit:** `{p['git_commit']}`", + f"- **bag:** `{p['bag']}`", + f"- **mode:** {p['mode']} · **replay rate:** {p['rate']}× realtime", + f"- **parameter sets:** {len(results)}", + "", + "## Metrics", + "", + "Truth-free proxies — see Limitations. Arrows mark the better direction.", + "", + ] + header = "| metric | " + " | ".join(r["name"] for r in results) + " |" + sep = "| --- | " + " | ".join("---" for _ in results) + " |" + lines += [header, sep] + for label, key, nd, better in ROWS: + arrow = {"lower": " ↓", "higher": " ↑"}.get(better, "") + vals = [_get(r["metrics"], key) for r in results] + cells = [_mark(v, vals, better, nd) for v in vals] + lines.append(f"| {label}{arrow} | " + " | ".join(cells) + " |") + lines.append("") + + lines += ["## Maps", ""] + for r in results: + lines += [f"### {r['name']}", ""] + if r.get("params_file"): + lines.append(f"- params: `{r['params_file']}`") + if r.get("map_png"): + lines.append("") + lines.append(f"![map for {r['name']}]({r['map_png']})") + else: + lines.append("- _no map produced_") + lines.append("") + + lines += _limitations() + return "\n".join(lines) + + +def _mark(v, vals, better, nd): + """Bold the best value in a row when a direction is defined.""" + s = _fmt(v, nd) + if not better or v is None: + return s + nums = [x for x in vals if isinstance(x, (int, float))] + if len(nums) < 2: + return s + best = min(nums) if better == "lower" else max(nums) + return f"**{s}**" if v == best else s + + +def _limitations() -> list: + return [ + "## Limitations", + "", + "These metrics are **truth-free proxies**, not error measures. A real bag" + " carries no surveyed ground truth, so unlike the sim benchmark (which has" + " Gazebo's true pose and reports ATE), this harness can only score" + " *self-consistency* and *map appearance*:", + "", + "- **Loop drift** is only meaningful when the robot physically returned to" + " its start — it cannot distinguish a legitimate open A→B traverse from a" + " drifting loop. Know the bag's shape before reading it.", + "- **Map crispness** (wall thickness, speckle, unknown fraction) catches" + " blur, noise, and incompleteness. It does **not** catch a confidently" + " *wrong* map: a mis-closed loop drawn with sharp walls scores well here.", + "- No absolute scale/position check is possible without a reference map or" + " survey. For metric-accuracy claims, use the sim benchmark's ATE.", + "- Replaying the same recorded sensor stream makes the comparison" + " deterministic in its *input*, but SLAM's solver is not bit-exact" + " run-to-run; treat small deltas as noise.", + "", + ] + + +def build_run(provenance, results) -> dict: + return {"provenance": provenance, "results": results} diff --git a/mote_simulation/tools/bag_replay/test_bag_replay.py b/mote_simulation/tools/bag_replay/test_bag_replay.py new file mode 100755 index 0000000..985f3fe --- /dev/null +++ b/mote_simulation/tools/bag_replay/test_bag_replay.py @@ -0,0 +1,151 @@ +#!/usr/bin/env python3 +"""Tests for the bag-replay harness's ROS-free pieces. + +Deliberately node-less: nothing here calls rclpy, launches a stack, or opens a +bag, so the test path can never reach a live robot (the harness's live path +additionally pins a random ROS_DOMAIN_ID — asserted below). Runs standalone +(`python test_bag_replay.py`) or under pytest. The ROS replay itself needs +slam_toolbox and a real bag and is exercised by `pixi run bag-replay`, not here. +""" + +import json +import sys +import tempfile +from pathlib import Path + +import numpy as np + +sys.path.insert(0, str(Path(__file__).resolve().parent)) +import render +import replay +import report + + +def test_isolated_env_pins_random_domain(): + # The live path must never run on the default domain 0 (a robot's domain). + ids = set() + for _ in range(50): + env = replay.isolated_env() + d = int(env["ROS_DOMAIN_ID"]) + assert 1 <= d <= 232 + ids.add(d) + assert len(ids) > 1 # actually randomised, not a constant + + +def _fixture_run_dir(tmp): + """A fake completed replay: series.json + map.npz for one set.""" + d = Path(tmp) / "baseline" + d.mkdir(parents=True) + traj = [ + [i * 0.1, np.cos(2 * np.pi * i / 120), np.sin(2 * np.pi * i / 120), 0.0] + for i in range(120) + ] + (d / "series.json").write_text( + json.dumps({"mode": "slam", "n_scans": 300, "traj": traj}) + ) + grid = np.full((80, 80), -1, dtype=np.int16) + grid[20:60, 20:60] = 0 + grid[40, 20:60] = 100 + np.savez_compressed( + d / "map.npz", + grid=grid, + resolution=np.float64(0.05), + origin=np.array([-2.0, -2.0]), + ) + return d + + +def test_score_from_recorded_outputs(): + with tempfile.TemporaryDirectory() as tmp: + d = _fixture_run_dir(tmp) + m, traj = replay.score(d / "series.json", d / "map.npz") + assert m["n_scans"] == 300 + assert m["traj_samples"] == 120 + assert m["loop"]["start_end_dist_m"] < 0.2 # near-closed circle + assert "mean_wall_thickness_m" in m["map"] + assert m["map"]["occ_frac"] > 0.0 + assert len(traj) == 120 + + +def test_render_writes_png(): + with tempfile.TemporaryDirectory() as tmp: + d = _fixture_run_dir(tmp) + traj = json.loads((d / "series.json").read_text())["traj"] + out = Path(tmp) / "map.png" + render.render_map(d / "map.npz", out, traj=traj) + assert out.exists() and out.stat().st_size > 0 + + +def test_report_compares_sets(): + results = [ + { + "name": "baseline", + "params_file": "a.yaml", + "map_png": "baseline/map.png", + "metrics": { + "n_scans": 300, + "traj_samples": 120, + "loop": { + "start_end_dist_m": 0.05, + "path_length_m": 6.2, + "drift_ratio": 0.008, + }, + "map": { + "explored_area_m2": 12.0, + "unknown_frac": 0.8, + "occ_frac": 0.01, + "mean_wall_thickness_m": 0.05, + "speckle_frac": 0.01, + }, + }, + }, + { + "name": "loose", + "params_file": "b.yaml", + "map_png": "loose/map.png", + "metrics": { + "n_scans": 300, + "traj_samples": 120, + "loop": { + "start_end_dist_m": 0.40, + "path_length_m": 6.4, + "drift_ratio": 0.06, + }, + "map": { + "explored_area_m2": 11.0, + "unknown_frac": 0.82, + "occ_frac": 0.02, + "mean_wall_thickness_m": 0.14, + "speckle_frac": 0.05, + }, + }, + }, + ] + run = report.build_run( + { + "timestamp": "t", + "git_commit": "abc", + "bag": "/b", + "mode": "slam", + "rate": 1.0, + }, + results, + ) + md = report.build_markdown(run) + assert "| baseline | loose |" in md + assert "wall thickness (m) ↓" in md + assert "![map for baseline](baseline/map.png)" in md + assert "**0.050**" in md # baseline wins wall thickness (lower is better) + assert "Limitations" in md + + +def main(): + fns = [v for k, v in sorted(globals().items()) if k.startswith("test_")] + for fn in fns: + fn() + print(f"ok {fn.__name__}") + print(f"\n{len(fns)} bag-replay tests passed") + + +if __name__ == "__main__": + main() diff --git a/mote_simulation/tools/benchmark/metrics.py b/mote_simulation/tools/benchmark/metrics.py index 48bdded..6dcbdfc 100644 --- a/mote_simulation/tools/benchmark/metrics.py +++ b/mote_simulation/tools/benchmark/metrics.py @@ -113,6 +113,135 @@ def path_length(traj) -> float: return float(np.sum(np.linalg.norm(np.diff(a[:, 1:3], axis=0), axis=1))) +# --------------------------------------------------------------------------- +# Truth-free metrics (no ground truth needed) +# +# The metrics above compare an estimate against a known-true series — available +# in the sim, where Gazebo publishes the robot's true pose. On a real recorded +# bag there is no truth, so these functions instead score *self-consistency*: +# how tight a loop closes, and how crisp the resulting map is. They are proxies, +# not error measures — see loop_drift/map_quality docstrings for what each can +# and cannot prove. Same numpy-only, ROS-free contract as everything else here. +# --------------------------------------------------------------------------- + + +def loop_drift(traj) -> dict: + """Odometry self-consistency from a single estimated trajectory. + + ``traj`` is ``list[[t, x, y, yaw]]``. Reports the straight-line distance + between the first and last pose and its ratio to the path travelled. On a + bag where the robot physically returns to its start (a closed loop), a small + ``start_end_dist_m`` means the estimator drifted little over the whole run; + ``drift_ratio`` normalises that by path length so runs of different sizes + compare. This is only meaningful when the trajectory is actually a loop — + it cannot tell an open A→B traverse (legitimately large end distance) from a + drifting loop, so the caller must know the bag's shape. Truth-free: it never + sees a true pose, only the estimate's own consistency. + """ + a = _arr(traj) + if a.shape[0] < 2: + return {"n": int(a.shape[0]), "note": "insufficient trajectory samples"} + start, end = a[0, 1:3], a[-1, 1:3] + dist = float(np.linalg.norm(end - start)) + plen = path_length(traj) + return { + "n": int(a.shape[0]), + "start_end_dist_m": dist, + "path_length_m": plen, + "drift_ratio": float(dist / plen) if plen > 1e-6 else 0.0, + "duration_s": float(a[-1, 0] - a[0, 0]), + } + + +def _erode4(mask: np.ndarray) -> np.ndarray: + """One 4-connectivity binary erosion, numpy-only (border cells erode away). + A True cell survives only if its N/S/E/W neighbours are all True.""" + out = np.zeros_like(mask) + out[1:-1, 1:-1] = ( + mask[1:-1, 1:-1] + & mask[:-2, 1:-1] + & mask[2:, 1:-1] + & mask[1:-1, :-2] + & mask[1:-1, 2:] + ) + return out + + +def _occ_neighbor_count(mask: np.ndarray) -> np.ndarray: + """Number of 4-connected True neighbours per cell (0..4), numpy-only.""" + n = np.zeros(mask.shape, dtype=np.int8) + n[1:, :] += mask[:-1, :] + n[:-1, :] += mask[1:, :] + n[:, 1:] += mask[:, :-1] + n[:, :-1] += mask[:, 1:] + return n + + +def map_quality( + grid, resolution: float, occ_thresh=65, free_thresh=25, max_iter=30 +) -> dict: + """Crispness/coverage proxies for a finished occupancy grid. + + ``grid`` is a 2-D array in ROS ``OccupancyGrid`` convention: 0 (free) .. 100 + (occupied), -1 unknown. ``resolution`` is metres per cell. No ground truth is + used — these are structural proxies for "did the map come out clean": + + * ``unknown_frac`` / ``free_frac`` / ``occ_frac`` — cell mix. A more complete + map has more decided (non-unknown) cells. + * ``explored_area_m2`` — decided cells x cell area. + * ``mean_wall_thickness_m`` — mean thickness of occupied structure, from + iterated 4-connectivity erosion (each occupied cell's erosion depth is its + distance to the nearest free/unknown cell; thickness ~= 2*depth+1). A + double-walled or smeared map from a bad scan-match reads thicker than a + crisp single-cell wall. Lower is crisper. + * ``speckle_frac`` — fraction of occupied cells with no occupied neighbour, + i.e. isolated specks. Poor odometry/scan-matching sprays these; lower is + cleaner. + + Truth-free caveat: a crisp map is not necessarily a *correct* one — a + confidently wrong map (e.g. a mis-closed loop drawn sharply) can still score + well here. These proxies catch blur, incompleteness, and noise, not global + metric error, which needs a surveyed reference the bag does not carry. + """ + g = np.asarray(grid) + total = int(g.size) + if total == 0: + return {"n_cells": 0, "note": "empty grid"} + unknown = g < 0 + occ = g >= occ_thresh + free = (g >= 0) & (g <= free_thresh) + n_occ = int(occ.sum()) + + thickness_m = 0.0 + if n_occ: + depth_sum = 0 + m = occ.copy() + for _ in range(max_iter): + m = _erode4(m) + s = int(m.sum()) + if s == 0: + break + depth_sum += s + mean_depth = depth_sum / n_occ + thickness_m = float((2.0 * mean_depth + 1.0) * resolution) + + speckle_frac = 0.0 + if n_occ: + isolated = occ & (_occ_neighbor_count(occ) == 0) + speckle_frac = float(int(isolated.sum()) / n_occ) + + decided = total - int(unknown.sum()) + return { + "n_cells": total, + "unknown_frac": float(int(unknown.sum()) / total), + "free_frac": float(int(free.sum()) / total), + "occ_frac": float(n_occ / total), + "explored_area_m2": float(decided * resolution * resolution), + "mean_wall_thickness_m": thickness_m, + "speckle_frac": speckle_frac, + } + + def goal_stats(goals) -> dict: """Success rate and time-to-goal over the scripted goal sequence.""" goals = list(goals or []) diff --git a/mote_simulation/tools/benchmark/test_metrics.py b/mote_simulation/tools/benchmark/test_metrics.py index f15c143..3bfe9bd 100755 --- a/mote_simulation/tools/benchmark/test_metrics.py +++ b/mote_simulation/tools/benchmark/test_metrics.py @@ -75,6 +75,52 @@ def test_smoothness_counts_reversals(): assert s["linear_jerk_rms"] > 0.0 +def test_loop_drift_closes_and_ratio(): + # A closed square loop: start == end -> ~zero drift, small ratio. + pts = [(0, 0), (2, 0), (2, 2), (0, 2), (0, 0)] + traj = [] + for i in range(len(pts) - 1): + (x0, y0), (x1, y1) = pts[i], pts[i + 1] + for s in np.linspace(0, 1, 20, endpoint=False): + traj.append([i + s, x0 + s * (x1 - x0), y0 + s * (y1 - y0), 0.0]) + traj.append([len(pts), 0.0, 0.0, 0.0]) + d = metrics.loop_drift(traj) + assert d["start_end_dist_m"] < 1e-6 + assert abs(d["path_length_m"] - 8.0) < 0.1 + assert d["drift_ratio"] < 1e-6 + + +def test_loop_drift_open_traverse(): + traj = [[i * 0.1, i * 0.1, 0.0, 0.0] for i in range(50)] + d = metrics.loop_drift(traj) + assert d["start_end_dist_m"] > 4.0 # A->B, honestly large + assert 0.9 < d["drift_ratio"] <= 1.0 # straight line: dist ~= path length + + +def test_map_quality_crisp_vs_blurred(): + # Crisp: a single-cell-thick wall on an otherwise-free field. + crisp = np.zeros((60, 60), dtype=int) + crisp[30, 5:55] = 100 + # Blurred/smeared: a thick 5-cell wall (bad scan-match proxy). + blurred = np.zeros((60, 60), dtype=int) + blurred[28:33, 5:55] = 100 + qc = metrics.map_quality(crisp, 0.05) + qb = metrics.map_quality(blurred, 0.05) + assert qc["mean_wall_thickness_m"] < qb["mean_wall_thickness_m"] + assert abs(qc["mean_wall_thickness_m"] - 0.05) < 1e-6 # 1 cell => ~1*res + assert qc["unknown_frac"] == 0.0 + + +def test_map_quality_speckle_and_unknown(): + grid = np.full((40, 40), -1, dtype=int) # all unknown + grid[10:30, 10:30] = 0 # a free box + grid[0, 0] = 100 # one isolated occupied speck inside unknown + q = metrics.map_quality(grid, 0.05) + assert q["speckle_frac"] == 1.0 # the lone occupied cell is a speck + assert 0.0 < q["unknown_frac"] < 1.0 + assert q["explored_area_m2"] > 0.0 + + def test_summarize_and_aggregate(): truth, est = _synthetic_run() series = { diff --git a/pixi.toml b/pixi.toml index 31f645e..91ad532 100644 --- a/pixi.toml +++ b/pixi.toml @@ -29,6 +29,13 @@ clean-map = "PYTHONPATH=mote_bringup python -m mote_bringup.map_cleanup.cli" save-zone = "ros2 run mote_tasks save_zone" site = "ros2 run mote_bringup site" +# Replay a recorded real-robot mapping bag through SLAM (or kinematic_icp) under +# N parameter sets and score them with truth-free proxies -> comparison report +# (metrics table + rendered maps). The reality check that complements `bench` +# (which needs Gazebo ground truth). Needs no sim/gz. Example: +# pixi run bag-replay -- --bag ~/.mote/bags/mapping/ --params a.yaml b.yaml +bag-replay = "python mote_simulation/tools/bag_replay/replay.py" + # Quick launch commands launch = "ros2 launch mote_bringup mote_launch.py" mapping = "ros2 launch mote_bringup mapping_launch.py"