MOSS: a small, mostly 3D-printed litter-picking rover. Cleaner streets, one maker at a time.
Python
dimOS main compatibility: moved imports with fallbacks, the camera mount on main, the relocalizer as a spectator (MOSS_RELOC_MAP)
metrox5f5af17f9f88
diff --git a/benches/bench_bump_imu_cold.py b/benches/bench_bump_imu_cold.pyindex 93a744e..b7007e1 100644--- a/benches/bench_bump_imu_cold.py+++ b/benches/bench_bump_imu_cold.py@@ -619,78 +619,82 @@ check("MossRealSense is a RealSenseCamera with exactly the parent's streams (imuissubclass(ri.MossRealSense, RealSenseCamera) and not vars(ri.MossRealSense).get("__annotations__")and _streams(ri.MossRealSense) == _streams(RealSenseCamera) and ("imu", Imu, "out") in _streams(RealSenseCamera))check("its stop keeps @rpc", "stop" in ri.MossRealSense.rpcs)-dev, motion = d455()-cam._profile = FakePipelineProfile(dev)-cam._start_imu()-check("_start_imu: the motion sensor of the pipeline's own device is opened with [accel 200 Hz, gyro 200 Hz]",- cam._imu_sensor is motion and [(p.kind, p.rate) for p in motion.opened] == [(rs.stream.accel, 200), (rs.stream.gyro, 200)],- f"{motion.opened}")-check("... and started on the parent's own handler (_on_motion_frame), no second pipeline",- motion.calls == ["open", "start"] and motion.callback == cam._on_motion_frame and cam._imu_pipeline is None)-check("... announced in the log with the rates and the axes",- any("accel 200 Hz, gyro 200 Hz" in ln and "z forward" in ln for ln in spy2.lines), f"{spy2.lines}")---class FakeMotionFrame:- def __init__(self, kind, ms, xyz):- self.kind, self.ms, self.xyz = kind, ms, xyz-- def as_motion_frame(self):- return self-- def get_motion_data(self):- return types.SimpleNamespace(x=self.xyz[0], y=self.xyz[1], z=self.xyz[2])-- def get_timestamp(self):- return self.ms-- def get_profile(self):- return FakeProfile(self.kind, 200)---published = []-cam.imu = types.SimpleNamespace(publish=published.append)-base_ms = T0 * 1000.0-motion.callback(FakeMotionFrame(rs.stream.accel, base_ms, (0.10, -9.81, 0.20)))-motion.callback(FakeMotionFrame(rs.stream.accel, base_ms + 10.0, (0.30, -9.81, 0.40)))-motion.callback(FakeMotionFrame(rs.stream.gyro, base_ms + 5.0, (0.01, 0.02, 0.03)))-imu = published[0] if published else None-check("the parent publishes one Imu per gyro sample, accel interpolated to it: (0.20, -9.81, 0.30) at +5 ms",- imu is not None and len(published) == 1- and all(abs(a - b) < 1e-4 for a, b in zip((imu.linear_acceleration.x, imu.linear_acceleration.y, imu.linear_acceleration.z),- (0.20, -9.81, 0.30)))- and abs(imu.ts - (T0 + 0.005)) < 1e-6, f"{imu!r}")-check("... unrotated (librealsense's axes as they come: y down = -9.81) in camera_accel_optical_frame",- imu is not None and imu.frame_id == "camera_accel_optical_frame"- and (imu.angular_velocity.x, imu.angular_velocity.y, imu.angular_velocity.z) == (0.01, 0.02, 0.03))-cam.stop()-check("stop(): the motion sensor is stopped then closed, and forgotten",- motion.calls == ["open", "start", "stop", "close"] and cam._imu_sensor is None, f"{motion.calls}")--spy2.lines.clear()-bare = ri.MossRealSense(enable_imu=True, imu_hz=200)-MODULES.append(bare)-bare._profile = FakePipelineProfile(FakeDevice(d455()[0].sensors[:2]))-bare._start_imu()-bare._start_imu()-check("a camera without a motion module: ONE warning for two starts, no sensor, no exception",- bare._imu_sensor is None and len([ln for ln in spy2.lines if "no motion module" in ln]) == 1, f"{spy2.lines}")-spy2.lines.clear()-odd = ri.MossRealSense(enable_imu=True, imu_hz=300)-MODULES.append(odd)-odd._profile = FakePipelineProfile(d455()[0])-odd._start_imu()-check("imu_hz=300 (not offered): a warning naming the rates, the camera goes on (no exception)",- odd._imu_sensor is None and any("[200, 400]" in ln and "colour and depth go on" in ln for ln in spy2.lines), f"{spy2.lines}")-spy2.lines.clear()-fdev, fmotion = d455(fail_start=True)-failing = ri.MossRealSense(enable_imu=True, imu_hz=200)-MODULES.append(failing)-failing._profile = FakePipelineProfile(fdev)-failing._start_imu()-check("a start that fails ('failed to set power state'): the sensor is closed again, a warning, the camera goes on",- failing._imu_sensor is None and fmotion.calls == ["open", "start", "close"]- and any("failed to set power state" in ln for ln in spy2.lines), f"{fmotion.calls} {spy2.lines}")+NATIVE_CAMERA = any(c.__name__ == "NativeModule" for c in RealSenseCamera.__mro__) # dimOS main (7a83fe5b1): Rust opens the IMU+if NATIVE_CAMERA:+ print(" -- SKIP _start_imu and the motion handler: this dimOS camera is native (Rust) and opens its motion module itself")+else:+ dev, motion = d455()+ cam._profile = FakePipelineProfile(dev)+ cam._start_imu()+ check("_start_imu: the motion sensor of the pipeline's own device is opened with [accel 200 Hz, gyro 200 Hz]",+ cam._imu_sensor is motion and [(p.kind, p.rate) for p in motion.opened] == [(rs.stream.accel, 200), (rs.stream.gyro, 200)],+ f"{motion.opened}")+ check("... and started on the parent's own handler (_on_motion_frame), no second pipeline",+ motion.calls == ["open", "start"] and motion.callback == cam._on_motion_frame and cam._imu_pipeline is None)+ check("... announced in the log with the rates and the axes",+ any("accel 200 Hz, gyro 200 Hz" in ln and "z forward" in ln for ln in spy2.lines), f"{spy2.lines}")+++ class FakeMotionFrame:+ def __init__(self, kind, ms, xyz):+ self.kind, self.ms, self.xyz = kind, ms, xyz++ def as_motion_frame(self):+ return self++ def get_motion_data(self):+ return types.SimpleNamespace(x=self.xyz[0], y=self.xyz[1], z=self.xyz[2])++ def get_timestamp(self):+ return self.ms++ def get_profile(self):+ return FakeProfile(self.kind, 200)+++ published = []+ cam.imu = types.SimpleNamespace(publish=published.append)+ base_ms = T0 * 1000.0+ motion.callback(FakeMotionFrame(rs.stream.accel, base_ms, (0.10, -9.81, 0.20)))+ motion.callback(FakeMotionFrame(rs.stream.accel, base_ms + 10.0, (0.30, -9.81, 0.40)))+ motion.callback(FakeMotionFrame(rs.stream.gyro, base_ms + 5.0, (0.01, 0.02, 0.03)))+ imu = published[0] if published else None+ check("the parent publishes one Imu per gyro sample, accel interpolated to it: (0.20, -9.81, 0.30) at +5 ms",+ imu is not None and len(published) == 1+ and all(abs(a - b) < 1e-4 for a, b in zip((imu.linear_acceleration.x, imu.linear_acceleration.y, imu.linear_acceleration.z),+ (0.20, -9.81, 0.30)))+ and abs(imu.ts - (T0 + 0.005)) < 1e-6, f"{imu!r}")+ check("... unrotated (librealsense's axes as they come: y down = -9.81) in camera_accel_optical_frame",+ imu is not None and imu.frame_id == "camera_accel_optical_frame"+ and (imu.angular_velocity.x, imu.angular_velocity.y, imu.angular_velocity.z) == (0.01, 0.02, 0.03))+ cam.stop()+ check("stop(): the motion sensor is stopped then closed, and forgotten",+ motion.calls == ["open", "start", "stop", "close"] and cam._imu_sensor is None, f"{motion.calls}")++ spy2.lines.clear()+ bare = ri.MossRealSense(enable_imu=True, imu_hz=200)+ MODULES.append(bare)+ bare._profile = FakePipelineProfile(FakeDevice(d455()[0].sensors[:2]))+ bare._start_imu()+ bare._start_imu()+ check("a camera without a motion module: ONE warning for two starts, no sensor, no exception",+ bare._imu_sensor is None and len([ln for ln in spy2.lines if "no motion module" in ln]) == 1, f"{spy2.lines}")+ spy2.lines.clear()+ odd = ri.MossRealSense(enable_imu=True, imu_hz=300)+ MODULES.append(odd)+ odd._profile = FakePipelineProfile(d455()[0])+ odd._start_imu()+ check("imu_hz=300 (not offered): a warning naming the rates, the camera goes on (no exception)",+ odd._imu_sensor is None and any("[200, 400]" in ln and "colour and depth go on" in ln for ln in spy2.lines), f"{spy2.lines}")+ spy2.lines.clear()+ fdev, fmotion = d455(fail_start=True)+ failing = ri.MossRealSense(enable_imu=True, imu_hz=200)+ MODULES.append(failing)+ failing._profile = FakePipelineProfile(fdev)+ failing._start_imu()+ check("a start that fails ('failed to set power state'): the sensor is closed again, a warning, the camera goes on",+ failing._imu_sensor is None and fmotion.calls == ["open", "start", "close"]+ and any("failed to set power state" in ln for ln in spy2.lines), f"{fmotion.calls} {spy2.lines}")ri.logger = real_ri_loggerdel sys.modules["pyrealsense2"]diff --git a/benches/bench_cost_gradient_cold.py b/benches/bench_cost_gradient_cold.pyindex 7efaeaa..86c8da3 100644--- a/benches/bench_cost_gradient_cold.py+++ b/benches/bench_cost_gradient_cold.py@@ -31,7 +31,7 @@ sys.path.insert(0, str(_FsPath(__file__).resolve().parent.parent))from dimos.msgs.geometry_msgs.Pose import Pose # noqa: E402from dimos.msgs.nav_msgs.OccupancyGrid import CostValues, OccupancyGrid # noqa: E402-from dimos.navigation.replanning_a_star.min_cost_astar import min_cost_astar # noqa: E402+from moss_dimos.recovering_planner import min_cost_astar # noqa: E402 (dimOS's, from wherever this dimOS keeps it)from moss_dimos import robot_profile # noqa: E402from moss_dimos.recovering_planner import ( # noqa: E402PIVOT_PENALTY,diff --git a/benches/bench_costmap2d_cold.py b/benches/bench_costmap2d_cold.pyindex aa3ac19..ef09027 100644--- a/benches/bench_costmap2d_cold.py+++ b/benches/bench_costmap2d_cold.py@@ -163,6 +163,7 @@ os.environ["MOSS_OCCUPIED_AT"] = "2"os.environ["MOSS_NEW_VIEWPOINT_M"] = "0.10"os.environ["MOSS_PUBLISH_EVERY"] = "5"+os.environ.setdefault("DIMOS_TRANSPORT", "lcm") # MOSS runs on LCM (tools/moss-fly.sh sets it); dimOS main defaults to zenoh (4333a8fa6)from dimos_lcm.std_msgs import Bool # noqa: E402from moss_dimos import costmap2d as cmod # noqa: E402diff --git a/benches/bench_moss_blueprints_cold.py b/benches/bench_moss_blueprints_cold.pyindex 424d181..3094f8d 100644--- a/benches/bench_moss_blueprints_cold.py+++ b/benches/bench_moss_blueprints_cold.py@@ -88,6 +88,7 @@ import importlibimport jsonimport mathimport os+from types import SimpleNamespaceimport pickleimport reimport shutil@@ -499,7 +500,7 @@ cam = atom(nav, "DedicatedMossRealSense").kwargscheck("RealSense 640x480 @ 5 fps, colour + depth (aligned), no point cloud, the IMU in the camera's own process (MOSS_IMU on by default)",(cam.get("width"), cam.get("height"), cam.get("fps"), cam.get("enable_depth"),cam.get("enable_pointcloud"), cam.get("enable_imu")) == (640, 480, 5, True, False, True))-t = cam["base_transform"]+t = cam["base_transform"] if "base_transform" in cam else SimpleNamespace(translation=SimpleNamespace(**dict(zip("xyz", atom(nav, "MossCameraMount").kwargs["xyzq"])))) # main: a mount modulecheck("camera mount from the profile: (0.156, 0, 0.075) m",close(t.translation.x, 0.156) and close(t.translation.z, 0.075))check("recorder at the MOSS state dir", atom(nav, "MossRecorder").kwargs.get("db_path") == str(home_db))diff --git a/benches/bench_moss_reloc_cold.py b/benches/bench_moss_reloc_cold.pynew file mode 100644index 0000000..25dcd7c--- /dev/null+++ b/benches/bench_moss_reloc_cold.py@@ -0,0 +1,242 @@+"""Cold bench for the relocalizer spectator in moss-explore (dimOS main only).++Known input -> known output in physical units. Groups:++ A. knob off - MOSS_RELOC_MAP unset: moss-explore is the blueprint it was,+ no relocalizer atom, and importing/building it never imports+ dimos.mapping.relocalization (the pin a7d19f76 lacks it)+ B. knob on - MOSS_RELOC_MAP=<premap>: one LidarWindowRelocalization atom+ with map_file = that path; autoconnect puts LidarOdometry's+ `lidar` Out and the relocalizer's `lidar` In on one stream,+ the relocalizer's `tf` Out on the same `tf` stream as the+ odometry's; its `loaded_map` and `tf` reach the costmap+ only (MossCostMap(premap=False): ignored unless+ MOSS_PREMAP=1), the odometry reads nothing back (open+ loop); dimOS's stream type check accepts the build, with+ MOSS_PREMAP=1 too; dimOS's `--map-file` and+ `--republish-loaded-map` CLI options reach the relocalizer+ C. the handler - the module built for real (map_file = the courseB premap),+ its tf / loaded_map Outs captured; recorded `lidar` clouds of+ the same run windowed exactly as the module's window() does+ (deque of max_frames, one window every reloc_interval s of+ recording time, min_frames, raw sum), fed to the module's own+ _relocalize(); the first accepted fix is published as ONE+ world -> map transform on tf and the premap on loaded_map+ (frame map), then the module stops (relocalize_once). The+ premap is built from this same run, so the truth is the+ identity: the fix must sit within 0.5 m and 10 degrees of it.++B and C need dimOS main (dimos.mapping.relocalization) and open3d: on the+pinned dimOS they are skipped, said in the log. C needs a premap and the+recording it was built from, both under MOSS_RELOC_BENCH_DATA=<dir>:+courseB_explorer2.pc2.lcm and rec/courseB_explorer2.db; without them C is+skipped, said in the log. No hardware, no network.++Run: PYTHONDONTWRITEBYTECODE=1 PYTHONPATH=. <venv>/bin/python benches/bench_moss_reloc_cold.py+"""+from collections import defaultdict, deque+from functools import reduce+import math+import operator+import os+from pathlib import Path+import subprocess+import sys+from types import SimpleNamespace++ROOT = Path(__file__).resolve().parent.parent+sys.path.insert(0, str(ROOT))++_data = os.environ.get("MOSS_RELOC_BENCH_DATA", "").strip()+DATA = Path(_data) if _data else None+PREMAP = (DATA or Path("/maps")) / "courseB_explorer2.pc2.lcm" # dimos map global --export of courseB, 5,294 points+RECORDING = DATA / "rec" / "courseB_explorer2.db" if DATA else None # the same run: lidar (world frame) + odom+HAVE_DATA = PREMAP.is_file() and RECORDING is not None and RECORDING.is_file()+MAX_WINDOWS = 250 # courseB is 504 s: about 250 windows at 2 s++OK = KO = 0+++def check(label, ok, detail=""):+ global OK, KO+ if ok:+ OK += 1+ else:+ KO += 1+ print(f" {'OK' if ok else 'KO'} {label}" + (f" - {detail}" if detail else ""), flush=True)+++def finish():+ print(f"{OK} OK, {KO} KO")+ print("TEST PASSED" if KO == 0 else "TEST FAILED")+ sys.exit(0 if KO == 0 else 1)+++os.environ.pop("MOSS_RELOC_MAP", None)+import moss_dimos.blueprints as mb # noqa: E402+import moss_dimos.memory as mb_memory # noqa: E402+from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 # noqa: E402+from dimos.msgs.tf2_msgs.TFMessage import TFMessage # noqa: E402+++def names(bp):+ return sorted(b.module.__name__ for b in bp.active_blueprints)+++def wiring(bp):+ """{(stream name after remaps, type): {(module class name, direction)}}, as the blueprint bench."""+ out = defaultdict(set)+ for b in bp.active_blueprints:+ for conn in b.streams:+ name = bp.remapping_map.get((b.name, conn.name), conn.name)+ out[(name, conn.type)].add((b.module.__name__, conn.direction))+ return out+++print("A. knob off")+proc = subprocess.run([sys.executable, "-c",+ "import sys, moss_dimos.blueprints as mb; mb.build_moss_explore(); "+ "print([m for m in sys.modules if m.startswith('dimos.mapping.relocalization')])"],+ cwd=ROOT, capture_output=True, text=True, timeout=300,+ env={**{k: v for k, v in os.environ.items() if k != "MOSS_RELOC_MAP"},+ "PYTHONPATH": os.pathsep.join(filter(None, (str(ROOT), os.environ.get("PYTHONPATH")))), "PYTHONDONTWRITEBYTECODE": "1"})+last = (proc.stdout.strip().splitlines() or [""])[-1]+check("building moss-explore without MOSS_RELOC_MAP imports no dimos.mapping.relocalization module",+ proc.returncode == 0 and last == "[]", f"rc {proc.returncode}, {last or proc.stderr[-300:]}")+off = mb.build_moss_explore()+check(" ... and has no relocalizer atom", "LidarWindowRelocalization" not in names(off), f"{names(off)}")++import importlib.util # noqa: E402+if importlib.util.find_spec("dimos.mapping.relocalization.lidar") is None:+ print("B., C. SKIPPED: this dimOS has no dimos.mapping.relocalization.lidar (the pin a7d19f76); MOSS_RELOC_MAP needs dimOS main")+ finish()+print("B. knob on")+os.environ["MOSS_RELOC_MAP"] = str(PREMAP)+on = mb.build_moss_explore()+os.environ.pop("MOSS_RELOC_MAP")+added = sorted(set(names(on)) - set(names(off)))+check("MOSS_RELOC_MAP=<premap>: exactly one atom more, LidarWindowRelocalization",+ added == ["LidarWindowRelocalization"] and len(names(on)) == len(names(off)) + 1, f"{added}")+reloc = next(b for b in on.active_blueprints if b.module.__name__ == "LidarWindowRelocalization")+check(" ... map_file = the premap path, every other knob dimOS's default (world_frame world, map_frame map, MID360)",+ reloc.kwargs == {"map_file": str(PREMAP)}, f"{reloc.kwargs}")+w = wiring(on)+lidar = w[("lidar", PointCloud2)]+check("autoconnect: LidarOdometry's `lidar` Out and the relocalizer's `lidar` In are one stream",+ ("LidarOdometry", "out") in lidar and ("LidarWindowRelocalization", "in") in lidar, f"{sorted(lidar)}")+tf = w[("tf", TFMessage)]+check(" ... the relocalizer's `tf` Out joins the odometry's `tf` stream (world -> map beside world -> base_link)",+ ("LidarWindowRelocalization", "out") in tf and ("LidarOdometry", "out") in tf, f"{sorted(tf)}")+lm = w[("loaded_map", PointCloud2)]+cm_kw = next(b for b in on.active_blueprints if b.module.__name__ == "MossCostMap").kwargs+check(" ... `loaded_map` is published by the relocalizer and read by the costmap only, which ignores it "+ "(MossCostMap(premap=False), MOSS_PREMAP unset)",+ lm == {("LidarWindowRelocalization", "out"), ("MossCostMap", "in")} and cm_kw == {"premap": False},+ f"{sorted(lm)} {cm_kw}")+ins = sorted(c.name for c in reloc.streams if c.direction == "in")+check(" ... the relocalizer reads `lidar` only", ins == ["lidar"], f"{ins}")+readers_tf = sorted(m for (m, d) in tf if d == "in")+rec = next(b for b in on.active_blueprints if b.module.__name__ == "MossRecorder")+check("open loop: the `tf` readers are the costmap (a world -> map fix only, with MOSS_PREMAP=1) and dimOS's "+ "Recorder base under MossRecorder, which records no tf (record_tf False) and anchors `lidar` on odom; "+ "the odometry reads nothing back",+ readers_tf == ["MossCostMap", "MossRecorder"]+ and rec.kwargs.get("record_tf", mb_memory.MossMemoryConfig.model_fields["record_tf"].default) is False+ and not any(c.direction == "in" and c.name in ("tf", "loaded_map")+ for b in on.active_blueprints if b.module.__name__ == "LidarOdometry" for c in b.streams),+ f"{readers_tf}")+from dimos.core.coordination.module_coordinator import _verify_no_name_conflicts # noqa: E402+os.environ.update(MOSS_RELOC_MAP=str(PREMAP), MOSS_PREMAP="1")+both = mb.build_moss_explore()+for v in ("MOSS_RELOC_MAP", "MOSS_PREMAP"):+ os.environ.pop(v)+lm_both = wiring(both)[("loaded_map", PointCloud2)]+check("MOSS_RELOC_MAP and MOSS_PREMAP=1: MossCostMap(premap=True) reads the relocalizer's `loaded_map`, one stream",+ next(b for b in both.active_blueprints if b.module.__name__ == "MossCostMap").kwargs == {"premap": True}+ and lm_both == {("LidarWindowRelocalization", "out"), ("MossCostMap", "in")}, f"{sorted(lm_both)}")+try:+ _verify_no_name_conflicts(on)+ _verify_no_name_conflicts(both)+ check("dimOS's stream type check accepts both builds (MOSS_PREMAP unset and 1)", True)+except ValueError as exc:+ check("dimOS's stream type check accepts both builds (MOSS_PREMAP unset and 1)", False, str(exc)[:300])+try:+ from dimos.core.coordination.blueprint_config.parser import BlueprintConfigParser+ parsed = BlueprintConfigParser(on).parse(["--map-file", "/elsewhere/other.pc2.lcm", "--republish-loaded-map", "10"],+ environ={})+ got = parsed.module_kwargs(reloc.name)+ check("dimOS's CLI: `dimos run moss-dimos.moss-explore --map-file X --republish-loaded-map 10` sets the "+ "relocalizer's map_file and its loaded_map resend period (s)",+ got.get("map_file") == "/elsewhere/other.pc2.lcm" and got.get("republish_loaded_map") == 10.0, f"{got}")+except Exception as e: # noqa: BLE001+ check("dimOS's CLI: `--map-file X --republish-loaded-map 10` reach the relocalizer", False,+ f"{type(e).__name__}: {str(e)[:300]}")++if not HAVE_DATA:+ print("C. SKIPPED: no premap and recording here (MOSS_RELOC_BENCH_DATA=<dir> with courseB_explorer2.pc2.lcm "+ "and rec/courseB_explorer2.db)")+ finish()+print("C. the module's handler on the recording")+import numpy as np # noqa: E402+from reactivex import Subject # noqa: E402+from dimos.mapping.relocalization.lidar.module import LidarWindowRelocalization # noqa: E402+from dimos.mapping.relocalization.module import yaw_deg # noqa: E402+from dimos.memory.store.sqlite import SqliteStore # noqa: E402++m = LidarWindowRelocalization(map_file=str(PREMAP))+tf_out, map_out = [], []+m.tf = SimpleNamespace(publish=tf_out.append) # the two Outs, captured+m.loaded_map = SimpleNamespace(publish=map_out.append)+m.lidar = SimpleNamespace(observable=lambda: Subject()) # the In: windows go to the handler below+m.start()+cfg, rc = m.config, m.config.relocalize+check("the module loads the premap in frame `map` and builds its relocalizer",+ m.premap is not None and m.premap.frame_id == "map" and m._relocalizer is not None,+ f"{len(m.premap) if m.premap is not None else None} points")+print(f" window {rc.min_frames}..{rc.max_frames} scans, one per {cfg.reloc_interval} s, "+ f"min_local_points {cfg.min_local_points}, fitness_threshold {rc.fitness_threshold}")++store = SqliteStore(path=str(RECORDING), must_exist=True)+store.start()+held = deque(maxlen=rc.max_frames)+last_emit = t_first = None+attempts = skipped = 0+for obs in store.stream("lidar", PointCloud2).order_by("ts"):+ t_first = obs.ts if t_first is None else t_first+ held.append(obs.data)+ if last_emit is not None and obs.ts - last_emit < cfg.reloc_interval:+ continue+ last_emit = obs.ts+ if len(held) < rc.min_frames or not m.keep_relocalizing():+ if not m.keep_relocalizing():+ break+ continue+ msg = reduce(operator.add, held)+ if len(msg) < cfg.min_local_points:+ skipped += 1+ continue+ attempts += 1+ m._relocalize(msg) # the module's own handler+ if tf_out or attempts >= MAX_WINDOWS:+ t_fix = obs.ts - t_first+ break+store.stop()+check(f"at least one window accepted ({attempts} attempts, {skipped} skipped under min_local_points)",+ bool(tf_out), f"first fix at {t_fix:.1f} s of recording" if tf_out else "no fix")+if tf_out:+ msg = tf_out[0]+ check(" ... published as a TFMessage with ONE transform world -> map",+ isinstance(msg, TFMessage) and len(msg.transforms) == 1+ and (msg.transforms[0].frame_id, msg.transforms[0].child_frame_id) == ("world", "map"),+ f"{[(t.frame_id, t.child_frame_id) for t in getattr(msg, 'transforms', [])]}")+ fix = msg.transforms[0]+ x, y, z, yaw = fix.translation.x, fix.translation.y, fix.translation.z, yaw_deg(fix)+ dist = math.hypot(x, y)+ print(f" fix: translation ({x:+.3f}, {y:+.3f}, {z:+.3f}) m, yaw {yaw:+.2f} deg")+ check(" ... the premap is this run's own map: the fix is the identity within 0.5 m and 10 deg",+ dist < 0.5 and abs(yaw) < 10.0 and abs(z) < 0.5, f"{dist * 100:.1f} cm, {yaw:+.2f} deg")+ check(" ... the premap goes out once on loaded_map, frame map",+ len(map_out) == 1 and map_out[0].frame_id == "map", f"{len(map_out)}")+ check(" ... and the module stops attempting (relocalize_once)", m.placed and not m.keep_relocalizing())+m.dispose()+finish()diff --git a/benches/bench_robot_profile_cold.py b/benches/bench_robot_profile_cold.pyindex ff8442f..2e76a16 100644--- a/benches/bench_robot_profile_cold.py+++ b/benches/bench_robot_profile_cold.py@@ -395,7 +395,7 @@ if SG is not None:_m is not None and float(_m.group(1)) == SG.NERF_SPEED_DEFAULT,f"launcher {_m.group(1) if _m else None}, guard {SG.NERF_SPEED_DEFAULT}")_spec = importlib.util.find_spec("dimos")- _lp = Path(_spec.origin).parent / "navigation" / "replanning_a_star" / "local_planner.py" if _spec and _spec.origin else None+ _lp = next((p for d in ("go2", ".") if (p := Path(_spec.origin).parent / "navigation" / d / "replanning_a_star" / "local_planner.py").is_file()), None) if _spec and _spec.origin else None # main: navigation/go2/ (e5679069d)if _lp is None or not _lp.is_file():print(" -- SKIP the planner-speed check: no dimOS source here")else:diff --git a/docs/cockpit.md b/docs/cockpit.mdindex fad7c19..4be9145 100644--- a/docs/cockpit.md+++ b/docs/cockpit.md@@ -214,13 +214,52 @@ two lines received neither input. With `MOSS_PREMAP=0`, the default, every(MOSS_PREMAP=0)` once. `MOSS_PREMAP` reads its value like the four switchesabove, when `moss-nav` or `moss-explore` is built. The costmap never saves apremap of its own. The relocalization that produces the two inputs is a separate-module from dimOS main, run apart from this stack, a version step the robot has-not taken. This stack hears it only on LCM, on `/loaded_map` and `/tf`: run the+module from dimOS main, a version step the robot has not taken. Run apart from+this stack, it is heard only on LCM, on `/loaded_map` and `/tf`: run therelocalization with `DIMOS_TRANSPORT=lcm` (dimOS main's default transport iszenoh, which this stack does not hear) and with `republish_loaded_map` above 0,for example 10 s, because by default it sends the premap once per accepted fix(once per run when it stops at its first fix) and LCM does not resend a lost-message.+message. On dimOS main, `MOSS_RELOC_MAP` runs it inside `moss-explore` instead+([Running on dimOS main](#running-on-dimos-main)).++### Running on dimOS main++`pyproject.toml` pins the dimOS commit this package runs against, and the same+code also builds and runs its cold benches on dimOS main, where four things+moved. The replanning A* planner now lives under `dimos.navigation.go2` and the+frontier explorer under `dimos.navigation.experimental`: `recovering_planner.py`+and `explorer2.py` import from the new place, else from the pinned one. The+RealSense is a native module, a Rust binary that opens the IMU itself before+its video stream (what `MossRealSense` does on the pinned dimOS) and takes no+`base_transform`: the blueprints then add `MossCameraMount`, a static+`base_link` -> `camera_link` transform from the robot profile. `MOSS_RERUN=1`+asks for dimOS's websocket command center by name, because main no longer adds+it by default. And main's default transport is zenoh, not LCM: the launchers+export `DIMOS_TRANSPORT=lcm`, so nothing changes under them, but a `dimos run`+typed by hand needs it exported first, or `tools/explore_ctl.py` and the+cockpit, which speak LCM, reach nobody (`bench_explore_ctl_cold` expects LCM+when neither `DIMOS_TRANSPORT` nor `TRANSPORT` is set, and fails on main for+that one case). The camera's binary is not on PyPI. dimOS looks for it in its+own source tree: a pip install has none, so at its first start dimOS clones its+main branch from GitHub and builds there through nix. On the robot, install+dimOS from a git checkout instead, editable, and build the binary there once:+`cargo build --release` in `dimos/hardware/sensors/camera/realsense/rust`,+against the robot's own librealsense2, found through pkg-config (the crate is+`realsense-sys` 2.56, and one dependency comes from git, so the build needs the+network). Without the binary, `MOSS_CAMERA=0` runs the stack, with no depth+obstacles and no IMU contacts. Main also brings dimOS's lidar relocalizer:+`MOSS_RELOC_MAP=<premap.pc2.lcm>` adds `LidarWindowRelocalization` to+`moss-explore` as a spectator. It reads `lidar` and publishes the `world` ->+`map` fix on `tf` and the premap on `loaded_map`; only the costmap reads them,+and only with `MOSS_PREMAP=1`, and nothing goes back to the odometry. Inside+the stack it is on the stack's own transport, but by default it sends the+premap once per run: with `MOSS_PREMAP=1`, add `--republish-loaded-map 10`+to the `dimos run` line. `--map-file X` replaces the premap's path. Unset, the+blueprint is the one without it; on the pinned dimOS, `moss-explore` does not+build with it set. `benches/bench_moss_reloc_cold.py` checks that wiring, and+runs the module on a recording when `MOSS_RELOC_BENCH_DATA` names a directory+holding a premap and the run it was built from.## The robot profile@@ -293,7 +332,8 @@ Other variables:| `MOSS_RAY_MAX_M` | how far from the rover the lidar's rays clear the costmap's lidar layer (and the camera's rays its own), 4 m by default (0 = never). Anything farther than that from every place the rover stood is never forgotten: 6 forgets somebody who stood 4.5 m away, and longer rays cost more CPU per revolution || `MOSS_PUBLISH_EVERY` | one costmap grid every N lidar revolutions, 2 by default (5 grids a second), 1 to 20. Every subscriber decodes every grid and the recorder writes each one: raise it if the load climbs || `MOSS_CONTACT_WALL` | `1` turns a contact into a wall on the costmap: the band 2 to 20 cm beyond the body's front, across its width, when the rover was driving straight forward (3 cm or more along its heading and less than 10 degrees of turn over the last 0.5 s). Off by default: a contact is a reflex, only the lidar and the camera write the map |-| `MOSS_PREMAP` | `1` lets the costmap take a premap: the points dimOS's relocalization publishes on `loaded_map`, placed by its `world` -> `map` fix on `tf`, become lidar walls at the lowest wall score, published with the first grid of the mission, cleared by a lidar ray within `MOSS_RAY_MAX_M` like any wall, written again when the fix moves more than 2 cm or 1 degree. The relocalization must publish on LCM (`DIMOS_TRANSPORT=lcm`). Off by default: every `loaded_map` is ignored, said once in the log. Read when `moss-nav` or `moss-explore` is built ([A premap in the costmap](#a-premap-in-the-costmap)) |+| `MOSS_PREMAP` | `1` lets the costmap take a premap: the points dimOS's relocalization publishes on `loaded_map`, placed by its `world` -> `map` fix on `tf`, become lidar walls at the lowest wall score, published with the first grid of the mission, cleared by a lidar ray within `MOSS_RAY_MAX_M` like any wall, written again when the fix moves more than 2 cm or 1 degree. Run apart, the relocalization must publish on LCM (`DIMOS_TRANSPORT=lcm`); `MOSS_RELOC_MAP` runs it inside `moss-explore`. Off by default: every `loaded_map` is ignored, said once in the log. Read when `moss-nav` or `moss-explore` is built ([A premap in the costmap](#a-premap-in-the-costmap)) |+| `MOSS_RELOC_MAP` | dimOS main only: a premap file (`.pc2.lcm`, absolute path) adds dimOS's lidar relocalizer, `LidarWindowRelocalization`, to `moss-explore`. It reads `lidar` and publishes the `world` -> `map` fix on `tf` and the premap on `loaded_map`, which the costmap uses only with `MOSS_PREMAP=1`; with it, add `--republish-loaded-map 10` to the `dimos run` line (the premap goes out once per run by default). `--map-file X` replaces the path. Unset by default: no relocalizer. Read when `moss-explore` is built ([Running on dimOS main](#running-on-dimos-main)) |## Recordingdiff --git a/moss_dimos/blueprints.py b/moss_dimos/blueprints.pyindex 66731a0..810131f 100644--- a/moss_dimos/blueprints.py+++ b/moss_dimos/blueprints.py@@ -60,8 +60,12 @@ parses them: 0, false, no or off = off, any other value = on, unset = the defaulMOSS_RECORD=0 no recording (default on: MossRecorder, <MOSS_STATE_DIR>/recordings/moss.db)MOSS_RERUN=1 add the stock Rerun/websocket viewers (default off: no 7779/9877/3030 on MOSS)MOSS_PREMAP=1 the costmap takes a premap (default off: every loaded_map is ignored, said once): the- points a relocalization module run apart publishes on loaded_map, placed by its world ->- map fix on tf, become lidar walls (moss_dimos/costmap2d.py, the premap)+ points a relocalization module publishes on loaded_map (run apart, or MOSS_RELOC_MAP),+ placed by its world -> map fix on tf, become lidar walls (moss_dimos/costmap2d.py, the premap)+ MOSS_RELOC_MAP moss-explore only, dimOS main only: a premap file (.pc2.lcm, absolute path) adds dimOS's+ LidarWindowRelocalization to the stack (default unset: no relocalizer). It reads `lidar`+ and publishes the world -> map fix on tf and the premap on loaded_map; nothing in the+ stack acts on them unless MOSS_PREMAP=1Why the coordinator is a subclass defined HERE. An out-of-tree drive trainneeds two things, and the second one is not obvious:@@ -558,6 +562,11 @@ def _common_parts(ports: dict, profile: RobotProfile) -> list:serial = os.environ.get("MOSS_RS_SERIAL", "").strip()if serial:cam_kwargs["serial_number"] = serial+ from dimos.hardware.sensors.camera.realsense.camera import RealSenseCameraConfig+ if "base_transform" not in RealSenseCameraConfig.model_fields: # dimOS main's native camera (7a83fe5b1)+ from moss_dimos.realsense_imu import MossCameraMount+ cam_kwargs.pop("base_transform")+ parts.append(MossCameraMount.blueprint(xyzq=(x, y, z, qx, qy, qz, qw)))# the camera in a worker of its own (_realsense_module_class), with or without the IMUif imu:cam_kwargs.update(enable_imu=True, imu_hz=IMU_HZ)@@ -594,7 +603,10 @@ def _common_parts(ports: dict, profile: RobotProfile) -> list:if robot_profile.env_on("MOSS_RERUN", False):from dimos.core.global_config import global_configfrom dimos.visualization.vis_module import vis_module- parts.append(vis_module(viewer_backend=global_config.viewer))+ try: # dimOS main made the websocket command center opt-in (e7e289830); the pinned dimOS always adds it+ parts.append(vis_module(viewer_backend=global_config.viewer, websocket_vis=True))+ except TypeError:+ parts.append(vis_module(viewer_backend=global_config.viewer))return parts@@ -693,6 +705,12 @@ def build_moss_explore():lethal_clearance_m=lethal, pivot_clearance_m=pivot,**explorer_env()),]+ reloc_map = os.environ.get("MOSS_RELOC_MAP", "").strip() # a premap (.pc2.lcm, absolute path); unset = no relocalizer+ # dimOS main's relocalizer reads `lidar` (world frame) and publishes the world -> map fix on tf and the premap on+ # loaded_map: a spectator, since only the costmap reads those two, and only with MOSS_PREMAP=1+ if reloc_map:+ from dimos.mapping.relocalization.lidar.module import LidarWindowRelocalization # dimOS main only+ parts.append(LidarWindowRelocalization.blueprint(map_file=reloc_map))return _jpeg_camera(autoconnect(*parts)).remappings([(MossControlCoordinator, "twist_command", "cmd_vel"),])diff --git a/moss_dimos/costmap2d.py b/moss_dimos/costmap2d.pyindex cb1c95d..a4a9bd8 100644--- a/moss_dimos/costmap2d.py+++ b/moss_dimos/costmap2d.py@@ -106,7 +106,8 @@ exploration (`explore_done`, `stop_explore_cmd`) stops the explorer, not themap, because a cockpit goal or STOP sends `stop_explore_cmd` mid-run.The premap (`premap=True`, MOSS_PREMAP=1 on the blueprints; off by default).-dimOS's relocalization module, run apart from this stack, places this run's+dimOS's relocalization module (run apart from this stack, or inside it with+MOSS_RELOC_MAP on a dimOS build that carries it) places this run's`world` inside a map saved earlier and publishes two things: the fix, a`world` -> `map` transform on `tf` (sent again every few seconds), and thesaved map's points on `loaded_map`, in `map`. Until BOTH have arrived nothingdiff --git a/moss_dimos/explorer2.py b/moss_dimos/explorer2.pyindex 8314dff..ae2f8f6 100644--- a/moss_dimos/explorer2.py+++ b/moss_dimos/explorer2.py@@ -1185,9 +1185,16 @@ if HAVE_DIMOS: # pragma: no cover - needs the dimOS stackfrom dimos.core.core import rpcfrom dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid- from dimos.navigation.frontier_exploration.wavefront_frontier_goal_selector import (- WavefrontConfig, WavefrontFrontierExplorer,- )+ try: # dimOS main moved the explorer under navigation/experimental (e5679069d)+ from dimos.navigation.experimental.frontier_exploration.wavefront_frontier_goal_selector import (+ WavefrontConfig, WavefrontFrontierExplorer,+ )+ except ModuleNotFoundError as _e: # the pinned dimOS (pyproject.toml) keeps it at navigation/frontier_exploration+ if not (_e.name or "").startswith("dimos.navigation.experimental"):+ raise+ from dimos.navigation.frontier_exploration.wavefront_frontier_goal_selector import (+ WavefrontConfig, WavefrontFrontierExplorer,+ )from dimos.utils.logging_config import setup_loggerfrom dimos.core.stream import Outfrom dimos_lcm.std_msgs import Booldiff --git a/moss_dimos/realsense_imu.py b/moss_dimos/realsense_imu.pyindex 3b083d3..b9e4fae 100644--- a/moss_dimos/realsense_imu.py+++ b/moss_dimos/realsense_imu.py@@ -148,3 +148,25 @@ class MossRealSense(RealSenseCamera):except Exception: # noqa: BLE001 - already stopped or the device is gonepasssuper().stop()+++# dimOS main (7a83fe5b1): the RealSense is a native (Rust) module that publishes only its own+# subtree from camera_link and takes no base_transform; the mount is a static edge of its own.+from dimos.msgs.geometry_msgs.Quaternion import Quaternion # noqa: E402+from dimos.msgs.geometry_msgs.Transform import Transform # noqa: E402+from dimos.msgs.geometry_msgs.Vector3 import Vector3 # noqa: E402+from dimos.protocol.tf.static_tf_publisher import StaticTfPublisher, StaticTfPublisherConfig # noqa: E402+++class MossCameraMountConfig(StaticTfPublisherConfig):+ xyzq: tuple[float, float, float, float, float, float, float] = (0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0)+++class MossCameraMount(StaticTfPublisher):+ """base_link -> camera_link (blueprints.camera_mount_xyzq), for a camera that no longer publishes it."""+ config: MossCameraMountConfig++ def transforms(self) -> list[Transform]:+ x, y, z, qx, qy, qz, qw = self.config.xyzq+ return [Transform(translation=Vector3(x, y, z), rotation=Quaternion(qx, qy, qz, qw),+ frame_id="base_link", child_frame_id="camera_link")]diff --git a/moss_dimos/recovering_planner.py b/moss_dimos/recovering_planner.pyindex ffb11d2..298865c 100644--- a/moss_dimos/recovering_planner.py+++ b/moss_dimos/recovering_planner.py@@ -39,9 +39,16 @@ from dimos.msgs.nav_msgs.OccupancyGrid import CostValues, OccupancyGridfrom dimos.msgs.nav_msgs.Path import Pathfrom dimos.core.stream import Infrom dimos.navigation.base import NavigationState-from dimos.navigation.replanning_a_star.global_planner import GlobalPlanner-from dimos.navigation.replanning_a_star.min_cost_astar import min_cost_astar-from dimos.navigation.replanning_a_star.module import ReplanningAStarPlanner+try: # dimOS main moved the planner under navigation/go2 (e5679069d)+ from dimos.navigation.go2.replanning_a_star.global_planner import GlobalPlanner+ from dimos.navigation.go2.replanning_a_star.min_cost_astar import min_cost_astar+ from dimos.navigation.go2.replanning_a_star.module import ReplanningAStarPlanner+except ModuleNotFoundError as _e: # the pinned dimOS (pyproject.toml) keeps it at navigation/replanning_a_star+ if not (_e.name or "").startswith("dimos.navigation.go2"):+ raise+ from dimos.navigation.replanning_a_star.global_planner import GlobalPlanner+ from dimos.navigation.replanning_a_star.min_cost_astar import min_cost_astar+ from dimos.navigation.replanning_a_star.module import ReplanningAStarPlannerfrom dimos_lcm.std_msgs import Boolfrom dimos.utils.logging_config import setup_logger