MOSS: a small, mostly 3D-printed litter-picking rover. Cleaner streets, one maker at a time.
Python
costmap: a relocalized premap as an input (loaded_map + the world->map tf), behind MOSS_PREMAP
metroxd703bdb79c4c
diff --git a/benches/bench_costmap2d_cold.py b/benches/bench_costmap2d_cold.pyindex 5b9b64c..aa3ac19 100644--- a/benches/bench_costmap2d_cold.py+++ b/benches/bench_costmap2d_cold.py@@ -96,6 +96,42 @@ Known input -> known output, in metres, cells and seconds:carves 0.75 to 2.50 m)26. the carve band follows the camera height: 0.013-0.060 m at 0.075 m,the original 0.10-0.45 m at 0.56 m+ 27. the premap needs both inputs: a loaded_map alone, or a `world` -> `map`+ fix alone, writes nothing and says it arrived (the fix once); a+ `map` -> `world` transform is not a fix; both (in either order, or+ before the first pose) write 20 walls from 100 points with the fix+ (x 1 m, yaw 90 deg): map (mx, -1.025) lands at world (2.025, mx), on the+ lidar layer at OCCUPIED_AT; the floor layer (z 0.025) and points above+ 1.30 m are left out; a cell the lidar has seen free stays free; the+ mission gate does not hold the write and nothing is published before+ it; a different premap under the same fix replaces the old one; a+ 5 300-point premap is written in one go+ 28. a lidar ray through a premap wall carves it like any wall: a premap cell+ and a live wall at the same score both turn free on the same revolution+ 29. a fix 5 cm away clears the premap (each cell loses what the premap+ added: a live hit since stays, a ray's miss since stays, an unseen cell+ goes back to unknown) and writes it 5 cm over; a fix 1 cm away, the same+ fix again or the same premap again change nothing; a fix creeping by+ 1.5 cm steps is measured against the written one (re-written at 2.5 cm);+ 1.5 deg of turn alone re-writes; +179.6 to -179.6 deg (0.8 deg apart)+ does not, -178.4 (2 deg) does; a live wall from two viewpoints under the+ premap is still a wall after the re-write; a cell at HIT_CAP stays at+ HIT_CAP through the write and the clear; Clear map takes the premap out+ and the same fix does not bring it back+ 30. premap off (the default, MOSS_PREMAP=0): loaded_map and the fix write+ nothing, publish nothing, and "costmap: premap ignored (MOSS_PREMAP=0)"+ is said once for three loaded_maps+ 31. the camera layer and the seen mask are untouched by a premap write, a+ re-write and a clear+ 32. nothing is published before the mission, premap or not; the first+ grid of the mission carries the premap walls as 100, the floor the+ lidar saw as 0 and the floor beside the walls as -1; once a grid has+ gone out a re-write publishes at once, the old cells -1 again; after+ Clear map a re-write waits for the map's next grid+ 33. a premap wall a lidar ray carved stays carved through a re-write, even+ over a one-viewpoint hit from before the premap (1 + 1 - 1 = 1 would+ be a wall again): free, not flagged; a lidar hit from a new viewpoint+ lifts the mark and the next write claims the cell againNeeds dimOS (the messages are real).@@ -669,12 +705,14 @@ def test_the_mission_signals_cross_the_process_boundary() -> None:from dimos.msgs.geometry_msgs.PoseStamped import PoseStampedfrom dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGridfrom dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2+ from dimos.msgs.tf2_msgs.TFMessage import TFMessagefrom dimos.protocol.pubsub.impl.lcmpubsub import Topic as LCMTopiccm = {(s.name, s.type): s.direction for s in BlueprintAtom.create(MossCostMap, {}).streams}want = {("lidar", PointCloud2): "in", ("camera_floor", PointCloud2): "in", ("odom", PoseStamped): "in", ("explore_cmd", Bool): "in",("clear_map_cmd", Bool): "in", ("bump", Bool): "in",("explore_done", Bool): "in", ("stop_explore_cmd", Bool): "in",+ ("loaded_map", PointCloud2): "in", ("tf", TFMessage): "in",("global_costmap", OccupancyGrid): "out"}assert cm == want, f"stream table: extra {set(cm) - set(want)}, missing {set(want) - set(cm)}"assert MossCostMap.name == "mosscostmap", MossCostMap.name@@ -702,7 +740,7 @@ def test_the_mission_signals_cross_the_process_boundary() -> None:for v, val in saved.items():if val is not None:os.environ[v] = val- print(f" costmap: 5 Ins + global_costmap, one Bool class; explorer2 agrees; "+ print(f" costmap: {len(want) - 1} Ins + global_costmap, one Bool class; explorer2 agrees; "f"stop lands on {want_topic} (explore_ctl.py publishes there)")@@ -1481,6 +1519,407 @@ def test_the_doctrine_defaults() -> None:f"{' '.join(knobs[:4])} (this bench pins 2 / 0.10 / 5)")++# --- the premap: a relocalization's saved map, placed by its fix, as lidar walls ---+# The scene in `map`: a wall of 20 cells along x at y = -1.025, each cell at three heights+# inside the band (60 points), the floor layer of a 5 cm voxel premap (z 0.025) at y = -0.525+# and points above the band (z 1.325 and 1.5) at y = -1.525: 100 points, 20 wall cells.+# The fix (x 1 m, yaw 90 deg) puts map (mx, my) at world (1 - my, mx): the wall at x = 2.025.+PREMAP_XS = 0.025 + 0.05 * np.arange(20)+PREMAP_WALL = np.array([[mx, -1.025, z] for mx in PREMAP_XS for z in (0.075, 0.375, 1.275)])+PREMAP_FLOOR = np.array([[mx, -0.525, 0.025] for mx in PREMAP_XS])+PREMAP_HIGH = np.array([[mx, -1.525, 1.325 if k % 2 else 1.5] for k, mx in enumerate(PREMAP_XS)])+PREMAP_SCENE = np.vstack([PREMAP_WALL, PREMAP_FLOOR, PREMAP_HIGH])+FIX = (1.0, 0.0, 90.0)+++def _premap_cloud(pts: np.ndarray):+ """A premap as it arrives off the bus: float32, frame `map`, through an LCM round trip."""+ from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2+ pc = PointCloud2.from_numpy(np.asarray(pts, dtype=np.float32), frame_id="map", timestamp=time.time())+ return PointCloud2.lcm_decode(pc.lcm_encode())+++def _tf(x: float, y: float, yaw_deg: float, parent: str = "world", child: str = "map"):+ """One transform on the tf stream, as dimOS's relocalization sends its fix, through an LCM round trip."""+ from dimos.msgs.geometry_msgs.Quaternion import Quaternion+ from dimos.msgs.geometry_msgs.Transform import Transform+ from dimos.msgs.geometry_msgs.Vector3 import Vector3+ from dimos.msgs.tf2_msgs.TFMessage import TFMessage+ h = math.radians(yaw_deg) / 2+ msg = TFMessage(Transform(translation=Vector3(x, y, 0.0), rotation=Quaternion(0.0, 0.0, math.sin(h), math.cos(h)),+ frame_id=parent, child_frame_id=child))+ return TFMessage.lcm_decode(msg.lcm_encode())+++def _premap_module(on: bool = True, pose=(0.0, 0.0)) -> MossCostMap:+ m = MossCostMap(premap=on)+ m.global_costmap = Sink()+ MODULES.append(m)+ if pose is not None:+ asyncio.run(m.handle_odom(_odom(*pose)))+ return m+++def _cells(g: ScoredGrid, xy) -> set:+ gx, gy = g.cell(np.asarray(xy)[:, 0], np.asarray(xy)[:, 1])+ return set(zip(gy.tolist(), gx.tolist()))+++def _nonzero(layer: np.ndarray) -> set:+ return set(map(tuple, np.argwhere(layer != 0).tolist()))+++def _wall_world(dx: float = 0.0) -> np.ndarray:+ """The scene's wall in `world` under FIX, moved dx along world x."""+ return np.column_stack([np.full(20, 2.025 + dx), PREMAP_XS])+++def _logged(fn):+ """Run fn with costmap2d's logger replaced; returns the lines it said."""+ spy, real = LogSpy(), cmod.logger+ cmod.logger = spy+ try:+ fn()+ finally:+ cmod.logger = real+ return spy.lines+++def test_the_premap_needs_its_map_and_its_fix() -> None:+ """Nothing changes until BOTH a loaded_map and a `world` -> `map` fix+ have arrived; then the premap's points inside the band become walls on+ the lidar layer, at OCCUPIED_AT, at the cells the fix places them on.+ The mission gate stays shut through all of it: the premap is not a+ reading from where the rover stands."""+ from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2+ from dimos.msgs.tf2_msgs.TFMessage import TFMessage+ from dimos.protocol.pubsub.impl.lcmpubsub import Topic as LCMTopic++ # the premap alone: kept and said, nothing written; the odometry's own tf and an inverse+ # map -> world transform change nothing+ m = _premap_module()+ lines = _logged(lambda: (asyncio.run(m.handle_loaded_map(_premap_cloud(PREMAP_SCENE))),+ asyncio.run(m.handle_tf(_tf(0.3, 0.1, 10.0, child="base_link"))),+ asyncio.run(m.handle_tf(_tf(*FIX, parent="map", child="world")))))+ assert not _nonzero(m._grid.lidar) and not m._grid.premap.any() and not m.global_costmap.msgs, \+ "a loaded_map without a fix writes nothing (world -> base_link and map -> world are not fixes)"+ assert lines == ["costmap: premap received: 100 points, 60 in the 0.05-1.3 m band; "+ "written when a world -> map fix arrives on tf"], lines+ assert m._premap_fix is None+ # the fix alone: said once, nothing written+ t = _premap_module()+ lines = _logged(lambda: (asyncio.run(t.handle_tf(_tf(*FIX))), asyncio.run(t.handle_tf(_tf(*FIX)))))+ assert not _nonzero(t._grid.lidar) and not t._grid.premap.any() and not t.global_costmap.msgs, \+ "a fix without a loaded_map writes nothing"+ assert lines == ["costmap: premap fix received on tf: (+1.00, +0.00, +90.0 deg); "+ "written when a loaded_map arrives"], lines+ # both: 20 walls at the placed cells, nothing else on the lidar layer+ lines = _logged(lambda: asyncio.run(m.handle_tf(_tf(*FIX))))+ g = m._grid+ want = _cells(g, _wall_world())+ assert len(want) == 20 and _nonzero(g.lidar) == want, sorted(_nonzero(g.lidar) ^ want)[:5]+ assert set(map(tuple, np.argwhere(g.premap).tolist())) == want, "every wall cell is flagged premap"+ gy, gx = zip(*sorted(want))+ assert set(g.lidar[list(gy), list(gx)].tolist()) == {OCCUPIED_AT}, "written at OCCUPIED_AT, the lowest wall score"+ assert all(g.value_at(2.025, y) == 100 for y in PREMAP_XS), "the wall at world x = 2.025"+ assert not (_cells(g, PREMAP_WALL[:, :2]) & _nonzero(g.lidar)), "the map-frame positions themselves hold nothing"+ for name, pts in (("the floor layer (z 0.025)", PREMAP_FLOOR), ("above the band (z 1.325, 1.5)", PREMAP_HIGH)):+ placed = np.column_stack([1.0 - pts[:, 1], pts[:, 0]])+ assert not (_cells(g, placed) & _nonzero(g.lidar)), f"{name} is left out"+ assert m._mission_frozen, "the mission gate is still shut: the premap does not need it"+ assert not m.global_costmap.msgs, "nothing goes out before the mission (test 32)"+ head = "costmap: premap written: 20 walls from 100 points, tf (+1.00, +0.00, +90.0 deg)"+ assert len(lines) == 1 and lines[0].startswith(head), lines+ # the fix first, then the premap: the same 20 cells+ asyncio.run(t.handle_loaded_map(_premap_cloud(PREMAP_SCENE)))+ assert np.array_equal(t._grid.lidar, g.lidar) and np.array_equal(t._grid.premap, g.premap), "order does not matter"+ # both before the first pose: written when the grid is born+ early = _premap_module(pose=None)+ asyncio.run(early.handle_loaded_map(_premap_cloud(PREMAP_SCENE)))+ asyncio.run(early.handle_tf(_tf(*FIX)))+ assert early._grid is None+ asyncio.run(early.handle_odom(_odom(0.0, 0.0)))+ assert np.array_equal(early._grid.lidar, g.lidar), "a premap and a fix that came before the first pose"+ # a different premap under the same fix replaces the old one: the wall 5 cm further along -y in+ # `map` is 5 cm further along +x in `world`, and the old cells are empty again+ other = PREMAP_SCENE.copy()+ other[:60, 1] -= 0.05+ lines = _logged(lambda: asyncio.run(t.handle_loaded_map(_premap_cloud(other))))+ moved = _cells(t._grid, _wall_world(0.05))+ assert set(map(tuple, np.argwhere(t._grid.premap).tolist())) == moved and _nonzero(t._grid.lidar) == moved, \+ "a different premap replaces the old one"+ assert len(lines) == 3 and lines[0] == "costmap: premap received: 100 points, 60 in the 0.05-1.3 m band" \+ and lines[1] == "costmap: premap cleared: 20 cells" and lines[2].startswith("costmap: premap written: 20 walls"), lines+ # a cell the lidar has seen free stays free+ f = _premap_module()+ fy, fx = sorted(want)[3]+ f._grid.lidar[fy, fx] = -2+ f._grid.seen[fy, fx] = True+ lines = _logged(lambda: (asyncio.run(f.handle_loaded_map(_premap_cloud(PREMAP_SCENE))),+ asyncio.run(f.handle_tf(_tf(*FIX)))))+ assert f._grid.lidar[fy, fx] == -2 and not f._grid.premap[fy, fx] and int(f._grid.premap.sum()) == 19, \+ "the live map wins over the prior: a cell seen free is not overwritten"+ assert lines and lines[-1].startswith("costmap: premap written: 19 walls from 100 points") \+ and "1 cells the lidar has seen free left free" in lines[-1], lines+ # where they land on the bus: the canonical topics dimOS's relocalization publishes on+ assert str(LCMTopic("/loaded_map", PointCloud2)) == "/loaded_map#sensor_msgs.PointCloud2"+ assert str(LCMTopic("/tf", TFMessage)) == "/tf#tf2_msgs.TFMessage"+ # the cost of a room-sized premap (5 300 points over 15 x 8 m), written and published once+ rng = np.random.default_rng(1)+ big = np.column_stack([rng.uniform(-7.5, 7.5, 5300), rng.uniform(-1.5, 6.5, 5300), rng.uniform(0.07, 1.27, 5300)])+ b = _premap_module()+ asyncio.run(b.handle_loaded_map(_premap_cloud(big)))+ t0 = time.perf_counter()+ asyncio.run(b.handle_tf(_tf(0.3, -0.2, 30.0)))+ ms = (time.perf_counter() - t0) * 1000+ n_big = int(b._grid.premap.sum())+ assert n_big > 4000 and not b.global_costmap.msgs and ms < 100, (n_big, len(b.global_costmap.msgs), ms)+ print(f" a premap alone, a fix alone, a map -> world tf: nothing written, each input said; both: 20 walls at "+ f"world x = 2.025 (score {OCCUPIED_AT}), floor layer and > 1.30 m left out, any order, before the first pose "+ f"too, gate still shut, nothing published; a cell seen free stays free (19 walls); a different premap "+ f"replaces the old one; /loaded_map and /tf; 5 300 points -> {n_big} walls written in {ms:.1f} ms")+++def test_a_lidar_ray_carves_a_premap_wall_like_any_wall() -> None:+ """A premap wall is a lidar wall at OCCUPIED_AT: the room may have+ changed since the premap was made, and one lidar ray through the cell+ takes it out, exactly as it takes out a live wall at the same score."""+ m = _premap_module()+ asyncio.run(m.handle_explore_cmd(Bool(data=True)))+ asyncio.run(m.handle_loaded_map(_premap_cloud(PREMAP_SCENE)))+ asyncio.run(m.handle_tf(_tf(*FIX)))+ g = m._grid+ p, q = (2.025, 0.025), (2.025, -0.025) # a premap cell, and a live wall right below it+ g._hit(g.lidar, np.array([q[0]]), np.array([q[1]]), (0.0, -1.0))+ g._hit(g.lidar, np.array([q[0]]), np.array([q[1]]), (0.0, -0.8))+ (py, px), = _cells(g, [p])+ (qy, qx), = _cells(g, [q])+ assert g.premap[py, px] and not g.premap[qy, qx]+ assert g.lidar[py, px] == g.lidar[qy, qx] == OCCUPIED_AT and g.value_at(*p) == g.value_at(*q) == 100+ behind = _lidar_cloud(np.array([[3.0, 0.025], [3.0, -0.025]])) # returns behind both: each ray crosses one of them+ trace = []+ for _ in range(2 * RAY_EVERY):+ asyncio.run(m.handle_lidar(_cloud(behind)))+ trace.append((g.value_at(*p), g.value_at(*q)))+ assert trace[0] == (100, 100), trace+ assert trace[RAY_EVERY - 1] == (0, 0), f"both freed by the first carving revolution: {trace}"+ assert g.lidar[py, px] == g.lidar[qy, qx] == OCCUPIED_AT - 2 and g.seen[py, px], (g.lidar[py, px], g.lidar[qy, qx])+ assert g.value_at(2.025, 0.525) == 100, "a premap cell no ray crossed is still a wall"+ print(f" premap cell and live wall at score {OCCUPIED_AT}: {trace} over {2 * RAY_EVERY} revolutions "+ f"(rays every {RAY_EVERY}): both free on the same one, the uncrossed premap cells stay walls")+++def test_a_moved_fix_clears_and_rewrites_the_premap() -> None:+ """The premap is written again only when a fix differs from the one it+ was written with by more than 2 cm or 1 deg; its old cells lose exactly+ what the premap added first, so a corrected fix leaves no ghost wall."""+ m = _premap_module()+ g = m._grid+ # before the premap: a live wall from two viewpoints (OCCUPIED_AT, the premap adds nothing to it)+ # and a cell at HIT_CAP, both under premap points+ wy, cy_ = PREMAP_XS[2], PREMAP_XS[3]+ g._hit(g.lidar, np.array([2.025]), np.array([wy]), (0.0, 0.0))+ g._hit(g.lidar, np.array([2.025]), np.array([wy]), (0.0, 0.3))+ (wgy, wgx), = _cells(g, [(2.025, wy)])+ (cgy, cgx), = _cells(g, [(2.025, cy_)])+ g.lidar[cgy, cgx] = HIT_CAP+ g.seen[cgy, cgx] = True+ asyncio.run(m.handle_loaded_map(_premap_cloud(PREMAP_SCENE)))+ asyncio.run(m.handle_tf(_tf(*FIX)))+ old = _cells(g, _wall_world())+ assert g.lidar[wgy, wgx] == OCCUPIED_AT and g.lidar[cgy, cgx] == HIT_CAP and g.premap[wgy, wgx] and g.premap[cgy, cgx], \+ (g.lidar[wgy, wgx], g.lidar[cgy, cgx])+ (ay, ax), (by_, bx) = sorted(old)[0], sorted(old)[1]+ g._hit(g.lidar, np.array([2.025]), np.array([g.oy + (ay + 0.5) * g.res]), (0.0, 0.0)) # a live hit since+ g._miss(g.lidar, np.array([bx]), np.array([by_])) # a ray through since+ assert g.lidar[ay, ax] == OCCUPIED_AT + 1 and g.lidar[by_, bx] == OCCUPIED_AT - 1+ unseen = [c for c in sorted(old) if not g.seen[c]]+ lines = _logged(lambda: asyncio.run(m.handle_tf(_tf(FIX[0] + 0.05, FIX[1], FIX[2]))))+ new = _cells(g, _wall_world(0.05))+ assert len(lines) == 3 and lines[0] == "costmap: premap re-written: tf moved by (5.0 cm, 0.0 deg)" \+ and lines[1] == "costmap: premap cleared: 20 cells" \+ and lines[2].startswith("costmap: premap written: 20 walls from 100 points, tf (+1.05, +0.00, +90.0 deg)"), lines+ assert set(map(tuple, np.argwhere(g.premap).tolist())) == new and not (new & old), "the wall moved 5 cm, one cell"+ assert g.lidar[ay, ax] == 1 and g.lidar[by_, bx] == -1, \+ f"what the sensors did stays: the live hit {g.lidar[ay, ax]} (1), the ray's miss {g.lidar[by_, bx]} (-1)"+ assert all(g.lidar[c] == 0 and g.value_at(g.ox + (c[1] + 0.5) * g.res, g.oy + (c[0] + 0.5) * g.res) == -1+ for c in unseen), "a cell no sensor saw is unknown again: no ghost wall, no ghost floor"+ assert len(unseen) == 16+ assert g.lidar[wgy, wgx] == OCCUPIED_AT and g.value_at(2.025, wy) == 100, \+ f"the live wall seen from two viewpoints is still a wall: {g.lidar[wgy, wgx]}"+ assert g.lidar[cgy, cgx] == HIT_CAP, f"HIT_CAP through the write and the clear: {g.lidar[cgy, cgx]}"+ snap = g.lidar.copy()+ quiet = _logged(lambda: (asyncio.run(m.handle_tf(_tf(FIX[0] + 0.06, FIX[1], FIX[2]))), # 1 cm from the written fix+ asyncio.run(m.handle_tf(_tf(FIX[0] + 0.05, FIX[1], FIX[2]))), # the same fix again+ asyncio.run(m.handle_tf(_tf(FIX[0] + 0.065, FIX[1], FIX[2] + 0.5))), # 1.5 cm, 0.5 deg+ asyncio.run(m.handle_loaded_map(_premap_cloud(PREMAP_SCENE))))) # the same premap again+ assert not quiet and np.array_equal(g.lidar, snap) and set(map(tuple, np.argwhere(g.premap).tolist())) == new, quiet+ creep = _logged(lambda: asyncio.run(m.handle_tf(_tf(FIX[0] + 0.075, FIX[1], FIX[2])))) # 1 cm from the last one received+ assert creep and creep[0] == "costmap: premap re-written: tf moved by (2.5 cm, 0.0 deg)", \+ f"measured against the WRITTEN fix: {creep}"+ turn = _logged(lambda: asyncio.run(m.handle_tf(_tf(FIX[0] + 0.075, FIX[1], FIX[2] + 1.5))))+ assert turn and turn[0] == "costmap: premap re-written: tf moved by (0.0 cm, 1.5 deg)", turn+ # Clear map: the premap goes with the rest, and the same fix does not bring it back+ cleared = _logged(lambda: asyncio.run(m.handle_clear_map_cmd(Bool(data=True))))+ assert any(ln.startswith("costmap: premap cleared with the map") for ln in cleared), cleared+ assert not m._grid.premap.any() and not _nonzero(m._grid.lidar)+ again = _logged(lambda: asyncio.run(m.handle_tf(_tf(FIX[0] + 0.075, FIX[1], FIX[2] + 1.5))))+ assert not again and not m._grid.premap.any(), again+ back = _logged(lambda: asyncio.run(m.handle_tf(_tf(FIX[0] + 0.105, FIX[1], FIX[2] + 1.5))))+ assert back[0] == "costmap: premap re-written: tf moved by (3.0 cm, 0.0 deg)" and int(m._grid.premap.sum()) == 20, back+ # across +-180 deg: 0.8 deg apart is no move, 2 deg is+ w = _premap_module()+ asyncio.run(w.handle_loaded_map(_premap_cloud(PREMAP_SCENE)))+ asyncio.run(w.handle_tf(_tf(0.0, 0.0, 179.6)))+ wrap = _logged(lambda: asyncio.run(w.handle_tf(_tf(0.0, 0.0, -179.6))))+ turned = _logged(lambda: asyncio.run(w.handle_tf(_tf(0.0, 0.0, -178.4))))+ assert not wrap and turned and turned[0] == "costmap: premap re-written: tf moved by (0.0 cm, 2.0 deg)", (wrap, turned)+ print(" 5 cm: re-written, 20 cells cleared (live hit 3 -> 1, ray's miss 1 -> -1, a two-viewpoint wall still "+ "100, HIT_CAP kept, 16 unseen cells unknown again), 20 written one cell over; 1 cm, the same fix, "+ "1.5 cm + 0.5 deg, the same premap: nothing; creeping to 2.5 cm from the written fix: re-written; 1.5 deg "+ "alone: re-written; +179.6 -> -179.6 deg: nothing, -178.4: re-written; Clear map: gone until a fix moves 3 cm")+++def test_the_premap_off_ignores_and_says_so_once() -> None:+ """premap=False (MOSS_PREMAP=0, the blueprints' default and the+ module's): the In exists, every loaded_map is ignored, and the log says+ so once."""+ m = _premap_module(on=False)+ assert MossCostMap().premap_on is False, "the module's own default is off"+ lines = _logged(lambda: [asyncio.run(f) for f in (m.handle_loaded_map(_premap_cloud(PREMAP_SCENE)),+ m.handle_tf(_tf(*FIX)),+ m.handle_loaded_map(_premap_cloud(PREMAP_SCENE)),+ m.handle_tf(_tf(FIX[0] + 0.5, 0.0, 0.0)),+ m.handle_loaded_map(_premap_cloud(PREMAP_SCENE[:10])))])+ assert lines == ["costmap: premap ignored (MOSS_PREMAP=0)"], lines+ assert not _nonzero(m._grid.lidar) and not m._grid.premap.any() and not m.global_costmap.msgs+ assert m._premap_xy is None and m._premap_fix is None+ print(" premap off: 3 loaded_maps and 2 fixes -> nothing written, nothing published, one line: "+ "'costmap: premap ignored (MOSS_PREMAP=0)'")+++def test_the_premap_never_touches_the_camera_layer() -> None:+ """The premap writes the lidar layer only, and leaves `seen` alone: a+ write, a re-write and a clear leave the camera layer and the seen mask+ byte for byte as they were."""+ m = _premap_module()+ asyncio.run(m.handle_explore_cmd(Bool(data=True)))+ g = m._grid+ shoe = np.array([[2.025, 0.125]]) # a camera wall on a premap cell, and one beside the wall+ _armed(g, np.vstack([shoe, [[1.525, 0.325]]]), t=100.0, viewpoints=((0.0, 0.0), (0.0, 0.15)))+ g.body_clear((0.0, 0.0, 0.0))+ cam0, seen0 = g.camera.copy(), g.seen.copy()+ assert _cells(g, shoe) <= _nonzero(cam0) and int((cam0 > 0).sum()) == 2+ asyncio.run(m.handle_loaded_map(_premap_cloud(PREMAP_SCENE)))+ asyncio.run(m.handle_tf(_tf(*FIX)))+ after_write = (np.array_equal(g.camera, cam0), np.array_equal(g.seen, seen0))+ asyncio.run(m.handle_tf(_tf(FIX[0] + 0.05, FIX[1], FIX[2])))+ after_rewrite = (np.array_equal(g.camera, cam0), np.array_equal(g.seen, seen0))+ n = g.clear_premap()+ after_clear = (np.array_equal(g.camera, cam0), np.array_equal(g.seen, seen0))+ assert after_write == after_rewrite == after_clear == (True, True) and n == 20, (after_write, after_rewrite, after_clear)+ print(" a camera wall on a premap cell and one beside: camera layer and seen mask identical after the write, "+ "the re-write and the clear (20 cells)")+++def test_the_published_grid_carries_the_premap() -> None:+ """Nothing goes out before the mission, premap or not: a grid of premap+ walls with no free floor reads to explorer2 as a walled-in start (a+ back-off, then "no reachable frontier left"). The premap goes out with+ the map's first grid of the mission: walls 100, the floor the lidar saw+ 0, the floor beside the walls -1 (no sensor saw it). Once a grid has gone+ out, a re-write publishes at once, the old cells -1 again; after Clear+ map, a re-write waits for the map's next grid."""+ m = _premap_module()+ asyncio.run(m.handle_loaded_map(_premap_cloud(PREMAP_SCENE)))+ asyncio.run(m.handle_tf(_tf(*FIX)))+ for _ in range(3):+ asyncio.run(m.handle_odom(_odom(0.0, 0.0)))+ assert int(m._grid.premap.sum()) == 20 and not m.global_costmap.msgs, "written, not published, before the mission"+ asyncio.run(m.handle_explore_cmd(Bool(data=True)))+ # the mission is open and the body's footprint is seen free, but the map has put no grid out:+ # a re-write still waits (a grid of walls and one footprint of floor is a walled-in start too)+ asyncio.run(m.handle_odom(_odom(0.0, 0.0)))+ asyncio.run(m.handle_tf(_tf(FIX[0] - 0.05, FIX[1], FIX[2])))+ assert m._grid.seen.any() and int(m._grid.premap.sum()) == 20 and not m.global_costmap.msgs, \+ "no grid before the map's own first one, even with the footprint seen"+ asyncio.run(m.handle_tf(_tf(*FIX)))+ # lidar returns behind the rover (x = -1 m): their rays never cross the premap wall at x = 2.025+ behind = _lidar_cloud(np.array([[-1.0, y] for y in np.arange(-0.5, 0.55, 0.1)]))+ for k in range(PUBLISH_EVERY):+ assert not m.global_costmap.msgs, f"nothing before the map's first grid (revolution {k})"+ asyncio.run(m.handle_lidar(_cloud(behind)))+ out = m.global_costmap.msgs+ assert len(out) == 1, len(out)++ def at(og, x, y):+ grid = np.asarray(og.grid)+ ox, oy = og.origin.position.x, og.origin.position.y+ return int(grid[int(math.floor((y - oy) / RESOLUTION_M)), int(math.floor((x - ox) / RESOLUTION_M))])++ og = out[0]+ grid = np.asarray(og.grid)+ assert og.frame_id == "world" and abs(og.resolution - RESOLUTION_M) < 1e-9+ assert [at(og, 2.025, y) for y in PREMAP_XS] == [100] * 20, "the premap walls are 100"+ assert at(og, 1.975, 0.475) == -1 and at(og, 2.075, 0.475) == -1, "the floor on both sides of the wall is unknown"+ assert at(og, -0.475, 0.025) == 0, "the floor the lidar's rays crossed is free"+ n_wall, n_free = int((grid == 100).sum()), int((grid == 0).sum())+ assert n_wall == 20 and n_free > 0, (n_wall, n_free)+ asyncio.run(m.handle_tf(_tf(FIX[0] + 0.05, FIX[1], FIX[2])))+ assert len(m.global_costmap.msgs) == 2, "once a grid has gone out, a re-write publishes at once"+ og2 = m.global_costmap.msgs[-1]+ assert [at(og2, 2.075, y) for y in PREMAP_XS] == [100] * 20 and [at(og2, 2.025, y) for y in PREMAP_XS] == [-1] * 20+ # Clear map: a re-write waits for the map's next grid, which carries it+ asyncio.run(m.handle_clear_map_cmd(Bool(data=True)))+ asyncio.run(m.handle_tf(_tf(FIX[0] + 0.10, FIX[1], FIX[2])))+ assert int(m._grid.premap.sum()) == 20 and len(m.global_costmap.msgs) == 2, "after Clear map: written, not published"+ for _ in range(PUBLISH_EVERY):+ asyncio.run(m.handle_lidar(_cloud(behind)))+ og3 = m.global_costmap.msgs[-1]+ assert len(m.global_costmap.msgs) == 3 and [at(og3, 2.125, y) for y in PREMAP_XS] == [100] * 20+ print(f" before the mission: written, 0 grids; mission open, footprint seen, a re-write: 0 grids; first grid "+ f"of the mission (revolution {PUBLISH_EVERY}): 20 cells at "+ f"100 along x = 2.025, {n_free} free cells, -1 beside the wall; a 5 cm fix: published at once, 100 along "+ "x = 2.075, -1 where the wall was; after Clear map a re-write goes out with the next grid")+++def test_a_carved_premap_wall_stays_carved_through_a_rewrite() -> None:+ """A premap wall a lidar ray took below OCCUPIED_AT was seen free: a+ re-write never paints it back. The case that needs the mark: the cell+ held a one-viewpoint hit (1) before the premap landed, the premap made it+ a wall (2), a ray carved it (1), and the clear takes the premap's share+ back (0): written again, it would be a wall the lidar removed. A lidar+ hit from a new viewpoint lifts the mark, and the next write claims the+ cell again."""+ m = _premap_module()+ asyncio.run(m.handle_explore_cmd(Bool(data=True)))+ g = m._grid+ p = (2.01, 0.01)+ (cy, cx), = _cells(g, [p])+ g._hit(g.lidar, np.array([p[0]]), np.array([p[1]]), (0.0, 0.0)) # one sighting from one place: 1+ asyncio.run(m.handle_loaded_map(_premap_cloud(np.array([[p[0], p[1], 0.4]]))))+ asyncio.run(m.handle_tf(_tf(0.0, 0.0, 0.0)))+ assert g.lidar[cy, cx] == OCCUPIED_AT and g.premap[cy, cx] and g.value_at(*p) == 100+ behind = _lidar_cloud(np.array([[3.0, 0.01]])) # a return behind it: the ray crosses the cell+ for _ in range(RAY_EVERY):+ asyncio.run(m.handle_lidar(_cloud(behind)))+ assert g.lidar[cy, cx] == OCCUPIED_AT - 1 and g.premap_carved[cy, cx] and g.value_at(*p) == 0, g.lidar[cy, cx]+ assert _cells(g, [(p[0], p[1] + 0.025)]) == {(cy, cx)}, "the moved fix keeps the point in its cell"+ lines = _logged(lambda: asyncio.run(m.handle_tf(_tf(0.0, 0.025, 0.0))))+ assert lines[0] == "costmap: premap re-written: tf moved by (2.5 cm, 0.0 deg)" \+ and lines[-1].startswith("costmap: premap written: 0 walls from 1 points") \+ and "1 cells the lidar has seen free left free" in lines[-1], lines+ assert g.value_at(*p) == 0 and not g.premap[cy, cx] and g.premap_carved[cy, cx], (g.lidar[cy, cx], g.value_at(*p))+ g._hit(g.lidar, np.array([p[0]]), np.array([p[1]]), (0.0, 0.5)) # seen again, from a new viewpoint+ assert not g.premap_carved[cy, cx]+ asyncio.run(m.handle_tf(_tf(0.0, 0.0, 0.0)))+ assert g.premap[cy, cx] and g.value_at(*p) == 100, (g.lidar[cy, cx], g.value_at(*p))+ print(f" one hit (1) + premap -> {OCCUPIED_AT} -> a ray -> {OCCUPIED_AT - 1}, carved -> a 2.5 cm fix in the same "+ "cell: 0 walls, 1 left free, the cell free; a hit from a new viewpoint lifts the mark: the next write "+ "makes it a wall again")++TESTS = (test_the_doctrine_defaults, test_leg_needs_two_viewpoints_and_ray_is_free, test_ramp_hit_from_one_spot_never_becomes_a_wall,test_chair_moved_is_forgotten_by_lidar_rays, test_reinforcement_is_capped_and_unlearning_bounded,test_parked_rover_never_erases_an_established_wall, test_body_clear_is_the_moss_footprint,@@ -1494,7 +1933,11 @@ TESTS = (test_the_doctrine_defaults, test_leg_needs_two_viewpoints_and_ray_is_frtest_the_carve_stays_in_the_cameras_view, test_body_clear_frees_the_camera_layer,test_thin_leg_survives_the_floor_sampled_beside_it, test_moving_object_never_writes_the_map,test_the_cameras_rays_carve_the_camera_layer_only, test_camera_rays_start_at_the_camera_not_the_base,- test_the_carve_band_follows_the_camera_height)+ test_the_carve_band_follows_the_camera_height,+ test_the_premap_needs_its_map_and_its_fix, test_a_lidar_ray_carves_a_premap_wall_like_any_wall,+ test_a_moved_fix_clears_and_rewrites_the_premap, test_the_premap_off_ignores_and_says_so_once,+ test_the_premap_never_touches_the_camera_layer, test_the_published_grid_carries_the_premap,+ test_a_carved_premap_wall_stays_carved_through_a_rewrite)if __name__ == "__main__":OK = KO = 0diff --git a/benches/bench_moss_blueprints_cold.py b/benches/bench_moss_blueprints_cold.pyindex dd3c4ca..424d181 100644--- a/benches/bench_moss_blueprints_cold.py+++ b/benches/bench_moss_blueprints_cold.py@@ -64,6 +64,13 @@ Known input -> known output in physical units. Groups:on top builds dimOS's camera with enable_imu True and theprior on, its imu wired to the odometry, with a warning;FORCE alone changes nothing; no camera, no prior+ M. MOSS_PREMAP - unset / 0 / off: MossCostMap(premap=False) in moss-nav and+ moss-explore, the same blueprints as without the switch,+ no premap line; 1: premap=True, every other kwarg+ unchanged, one info line; the costmap's tf In shares the+ odometry's tf (one TFMessage class), loaded_map has no+ producer in the stack (the relocalization runs apart), and+ dimOS's stream type check accepts the buildI. the worker - what a dimOS forkserver worker does: unpickle thecoordinator class in a clean interpreter -> the module isimported, 'moss' is registered, the registry builds a@@ -785,6 +792,64 @@ check("MOSS_CAMERA=0 alone: no prior, and no MOSS_GYRO line (nothing was asked)"check("every MOSS_GYRO* variable is gone from the environment after the builds",not [k for k in os.environ if k.startswith("MOSS_GYRO") or k in ("MOSS_IMU", "MOSS_CAMERA")])+# ── M. MOSS_PREMAP: the costmap's premap ──────────────────────────────────+print("M. MOSS_PREMAP: the costmap takes a relocalization's premap, or ignores it")+from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 # noqa: E402+from dimos.msgs.tf2_msgs.TFMessage import TFMessage # noqa: E402+++def premap_kw(bp):+ return find(bp, "MossCostMap").kwargs+++def premap_lines(lines):+ return [(lvl, text) for lvl, text in lines if "MOSS_PREMAP" in text or "premap" in text]+++base_nav, base_log = build_logged(mb.build_moss_nav)+base_exp = mb.build_moss_explore()+check("MOSS_PREMAP unset: MossCostMap(premap=False) in moss-nav and moss-explore, no premap line at build",+ premap_kw(base_nav) == {"premap": False} and premap_kw(base_exp) == {"premap": False} and not premap_lines(base_log),+ f"{premap_kw(base_nav)} {premap_kw(base_exp)} {premap_lines(base_log)}")+for spelled_off in ("0", "off"):+ off_nav_p, off_log = build_logged(mb.build_moss_nav, MOSS_PREMAP=spelled_off)+ os.environ["MOSS_PREMAP"] = spelled_off+ off_exp_p = mb.build_moss_explore()+ os.environ.pop("MOSS_PREMAP")+ check(f"MOSS_PREMAP={spelled_off}: premap=False, the same blueprints as unset, kwarg for kwarg, no premap line",+ premap_kw(off_nav_p) == {"premap": False} and blueprint_text(off_nav_p) == blueprint_text(base_nav)+ and blueprint_text(off_exp_p) == blueprint_text(base_exp) and not premap_lines(off_log),+ f"{premap_kw(off_nav_p)} {premap_lines(off_log)}")+on_nav, on_log = build_logged(mb.build_moss_nav, MOSS_PREMAP="1")+os.environ["MOSS_PREMAP"] = "1"+on_exp = mb.build_moss_explore()+os.environ.pop("MOSS_PREMAP")+check("MOSS_PREMAP=1: MossCostMap(premap=True) in moss-nav and moss-explore",+ premap_kw(on_nav) == {"premap": True} and premap_kw(on_exp) == {"premap": True},+ f"{premap_kw(on_nav)} {premap_kw(on_exp)}")+others = lambda bp: [t for t in blueprint_text(bp) if t[0] != "MossCostMap"] # noqa: E731+check("... every other module kwarg for kwarg as without the switch",+ others(on_nav) == others(base_nav) and others(on_exp) == others(base_exp))+check("... one info line at build: 'MOSS blueprint: costmap premap ON (MOSS_PREMAP=1) ...'",+ [lvl for lvl, _ in premap_lines(on_log)] == ["info"]+ and premap_lines(on_log)[0][1].startswith("MOSS blueprint: costmap premap ON (MOSS_PREMAP=1)"),+ f"{premap_lines(on_log)}")+pw = wiring(on_exp)+tf_side = pw.get(("tf", TFMessage), set())+check("tf: the costmap reads the stack's one tf stream (the odometry writes it), one TFMessage class",+ {("LidarOdometry", "out"), ("MossCostMap", "in")} <= tf_side+ and [k for k in pw if k[0] == "tf"] == [("tf", TFMessage)], f"{sorted(tf_side)} {[k for k in pw if k[0] == 'tf']}")+check("loaded_map: read by the costmap, published by nothing in the stack (the relocalization runs apart)",+ pw.get(("loaded_map", PointCloud2), set()) == {("MossCostMap", "in")}+ and [k for k in pw if k[0] == "loaded_map"] == [("loaded_map", PointCloud2)], f"{pw.get(('loaded_map', PointCloud2))}")+try:+ _verify_no_name_conflicts(on_exp)+ _verify_no_name_conflicts(on_nav)+ check("dimOS's stream type check accepts both MOSS_PREMAP=1 builds", True)+except ValueError as exc:+ check("dimOS's stream type check accepts both MOSS_PREMAP=1 builds", False, str(exc)[:300])+check("MOSS_PREMAP is gone from the environment after the builds", "MOSS_PREMAP" not in os.environ)+# ── L. every camera in a worker of its own ───────────────────────────────print("L. own workers: the RealSense with and without the IMU, the wrist camera")import inspect # noqa: E402diff --git a/docs/cockpit.md b/docs/cockpit.mdindex cd5a7b6..fad7c19 100644--- a/docs/cockpit.md+++ b/docs/cockpit.md@@ -177,6 +177,51 @@ The switches `MOSS_CAMERA`, `MOSS_WRIST`, `MOSS_RECORD` and `MOSS_RERUN` read`0`, `false`, `no` and `off` (any case) as off, anything else as on, and unsetor empty as their default. The flight check reads `MOSS_CAMERA` the same way.+### A premap in the costmap++`MOSS_PREMAP=1` lets the costmap start from a map saved on an earlier run. It+takes two inputs, both published by dimOS's relocalization module: `loaded_map`,+the saved map's points in its own `map` frame, and the fix, a `world` -> `map`+transform on `tf` that places this run inside that map (sent again every 10 s).+Nothing changes until both have arrived: the costmap runs as it does without a+premap, and nothing waits for one. Then the premap's points between 5 cm and+1.30 m above the floor are placed by the fix's x, y and yaw and written once+into the lidar layer as walls, at the lowest wall score (`MOSS_OCCUPIED_AT`); a+cell the lidar has already seen free stays free, and the camera layer is never+touched. Nothing is published before the mission, premap or not: the walls go+out with the first grid of the mission, because a grid of walls with no free+floor reads to the explorer as a rover walled in at its start. From there a+premap wall is a lidar wall like any other: one lidar ray through its cell+clears it, since the room may have changed since the map was saved (a table top+or a shoe in the premap is cleared by a ray passing under or over it, and the+camera marks it again when it sees it), and a cleared premap wall is never+written back until the lidar hits that cell again from a new place. Rays clear+up to `MOSS_RAY_MAX_M` from where the rover stood: farther than that, a premap+wall stays. The floor between the walls stays unknown, so the explorer still+has to look at the room, but a frontier never forms against a premap wall, so a+wall that is no longer there (a door closed when the map was made) holds back+the frontier beside it until the rover comes within `MOSS_RAY_MAX_M` of it. A+fix more than 2 cm or 1 degree away from the one the premap was written with+clears the premap's cells and writes them again; the same fix sent again does+nothing. Clear map takes the premap out with the rest, until a fix moves or a+different premap arrives. The log says `costmap: premap received: M points, K+in the band` for each new premap, `costmap: premap fix received on tf: (x, y,+yaw)` once when the fix comes first, then `costmap: premap written: N walls from+M points, tf (x, y, yaw)`, `costmap: premap re-written: tf moved by (d cm, d+deg)` and `costmap: premap cleared`; a run on `MOSS_PREMAP=1` without the first+two lines received neither input. With `MOSS_PREMAP=0`, the default, every+`loaded_map` is ignored and the log says `costmap: premap ignored+(MOSS_PREMAP=0)` once. `MOSS_PREMAP` reads its value like the four switches+above, when `moss-nav` or `moss-explore` is built. The costmap never saves a+premap 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+relocalization with `DIMOS_TRANSPORT=lcm` (dimOS main's default transport is+zenoh, 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.+## The robot profile`moss_dimos/robot_profile.py` carries MOSS's geometry: lidar height and masks,@@ -248,6 +293,7 @@ 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)) |## Recordingdiff --git a/moss_dimos/blueprints.py b/moss_dimos/blueprints.pyindex 2c713e7..66731a0 100644--- a/moss_dimos/blueprints.py+++ b/moss_dimos/blueprints.py@@ -59,6 +59,9 @@ parses them: 0, false, no or off = off, any other value = on, unset = the defaulMOSS_RS_SERIAL pick the RealSense by serial numberMOSS_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)Why the coordinator is a subclass defined HERE. An out-of-tree drive trainneeds two things, and the second one is not obvious:@@ -518,6 +521,11 @@ def _common_parts(ports: dict, profile: RobotProfile) -> list:# MOSS_DESKEW=1 is an experiment too: kiss-icp's deskew (each point moved by its share of the motion# prior) dies on the rover at its first thread, and the odometry loop with itdeskew = robot_profile.env_on("MOSS_DESKEW", False)+ # MOSS_PREMAP=1: a relocalization's premap becomes lidar walls in the costmap (off: ignored, said once)+ premap = robot_profile.env_on("MOSS_PREMAP", False)+ if premap:+ _logger.info("MOSS blueprint: costmap premap ON (MOSS_PREMAP=1): loaded_map, placed by the world -> map "+ "fix on tf, becomes lidar walls")parts = [_coordinator_blueprint(_moss_base(address=ports["base"])),_gamepad_part(),@@ -526,7 +534,7 @@ def _common_parts(ports: dict, profile: RobotProfile) -> list:# on the floor, turns 25 deg between two scans, more than the constant-velocity prior follows);# constant velocity otherwise, the default until the gyro's sign is measured (above)LidarOdometry.blueprint(use_gyro_prior=gyro, deskew=deskew),- MossCostMap.blueprint(),+ MossCostMap.blueprint(premap=premap),]# MOSS_IMU=1: the IMU is opened inside the camera's own process (MossRealSense) and BumpImu turns# its decelerations into contacts on `bump` (in front) and `bump_rear` (behind); the IMU lives ondiff --git a/moss_dimos/costmap2d.py b/moss_dimos/costmap2d.pyindex 647740c..cb1c95d 100644--- a/moss_dimos/costmap2d.py+++ b/moss_dimos/costmap2d.py@@ -105,6 +105,84 @@ motion). It then stays open for the rest of the run: the end of anexploration (`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+`world` inside a map saved earlier and publishes two things: the fix, a+`world` -> `map` transform on `tf` (sent again every few seconds), and the+saved map's points on `loaded_map`, in `map`. Until BOTH have arrived nothing+changes: the map runs as it does without a premap, and nothing waits for one;+the first of the two to arrive says so in the log, so a run where an input+never comes shows it. Then the premap's points inside PREMAP_Z_BAND_M are+placed in `world` by the fix's x, y and yaw and written once into the LIDAR+layer at OCCUPIED_AT, the lowest wall score, flagged premap; a cell the+lidar has seen free stays free (a score below 0, or a premap wall a lidar+ray took below OCCUPIED_AT, `premap_carved`), and the camera layer is never+touched. From there a premap wall is a lidar wall like any other: one lidar+ray through its cell takes it below OCCUPIED_AT (the room may have changed+since the premap was made), a lidar hit from a new viewpoint confirms it,+the body clears it. Rays carve up to RAY_MAX_M only: a premap wall farther+than that from every place the rover stood stays until the rover comes+closer. A table top or a shoe in the premap sits above or below the scan+plane, so a lidar ray passing under or over it carves it too; the camera+marks it again on its own layer when it sees it. The premap is written again+only when a fix differs from the one it was written with by more than+PREMAP_MOVE_M or PREMAP_TURN_DEG (the same fix sent again changes nothing);+the old premap cells are cleared first, each losing exactly what the premap+added (what the sensors added since stays), so a corrected fix leaves no+ghost wall, and a premap wall a lidar ray carved is never written back: it+stays carved until a lidar hit from a new viewpoint lands on it, or Clear+map. The write does not wait for the mission gate (the premap is not a+reading taken from where the rover stands), but the publish does: nothing+goes out before the mission, as without a premap, and the premap goes out+with the first grid of the mission. A grid of premap walls with no free+floor reads to the explorer as a rover walled in at its start: a back-off,+then an exploration over before it began. Once a grid has gone out (since+the mission opened, or since Clear map), a write or a re-write publishes at+once. Clear map takes the premap out with everything else; it comes back+with a fix that moved, or a different premap.++The rover this stack comes from kept a saved grid, and it went wrong three+ways, each avoided here. The grid was loaded into both layers at full+strength, so furniture moved since could not be unlearned by the lidar and+walled the rover in: here the premap is the lidar's to unlearn, at the+weakest wall score. The run's own map was saved back as the next run's grid,+so a run walled in by ghosts handed its prison on: this module never writes+a premap, so what a run maps never becomes a prior. The map froze while the+rover looked for itself, and the drive with it: here nothing freezes or+waits for the premap.++The band is read on the premap's own heights (its floor is z = 0, as in+`world`), and the fix's z, roll and pitch are dropped: read after a full 3D+placement, a 1 deg pitch error would lift a floor point 7 m away by 12 cm,+into the band, as a wall. The band's floor (5 cm) leaves out the bottom layer+of a 5 cm voxel premap, where a premap made by a 3D lidar holds the floor+itself, and a floor written as walls is a prison. Its top (1.30 m) is the top+of the band lidar_odometry turns depth into obstacles (a table top is a+block).++The explorer sees a premap cell as a wall: 100 in the grid, although no+sensor of this run looked at it (it is not marked seen). The floor between+the walls stays unknown, since a point cloud holds where things are, never+where nothing is, so the rover still has to look at the room to explore it.+dimOS's frontier is an unknown cell with a free neighbour and no occupied+one: an unknown cell against a premap wall is never a frontier, and the+planner routes around premap walls from the first grid of the mission. That+is the choice made here; left unknown instead, a premap cell would only be a+hint the explorer drives into. The price: a premap wall that is no longer+there (a door closed when the map was made) also hides the frontier beside+it, and beyond RAY_MAX_M of every place the rover stood no ray carves it, so+it holds that frontier back until the rover comes closer. A premap cell+carved by a lidar ray turns free (the ray saw it); one cleared by a+corrected fix turns unknown again unless a sensor saw it.++Logs: "costmap: premap received: M points, K in the band" (each new premap;+"written when a world -> map fix arrives on tf" while no fix is here),+"costmap: premap fix received on tf: (x, y, yaw); written when a loaded_map+arrives" (once, when the fix comes first), "costmap: premap written: N walls+from M points, tf (x, y, yaw)", "costmap: premap re-written: tf moved by (d+cm, d deg)", "costmap: premap cleared", and with the premap off, "costmap:+premap ignored (MOSS_PREMAP=0)", once, at the first loaded_map.+Numbers (HIT_CAP, FREE_FLOOR, OCCUPIED_AT, RAY_MAX_M) are a starting point tobe tested. HIT_CAP, FREE_FLOOR, OCCUPIED_AT, NEW_VIEWPOINT_M, PUBLISH_EVERY,RAY_MAX_M and the contact wall are read once, at import, from MOSS_HIT_CAP,@@ -130,6 +208,7 @@ from dimos.msgs.geometry_msgs.Pose import Posefrom dimos.msgs.geometry_msgs.PoseStamped import PoseStampedfrom dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGridfrom dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2+from dimos.msgs.tf2_msgs.TFMessage import TFMessagefrom dimos.utils.logging_config import setup_loggerfrom dimos_lcm.std_msgs import Bool@@ -194,6 +273,11 @@ CONTACT_TURN_DEG = 10.0 # ...and turning less than this (3 cm and 10 deg: tMISSION_START_M = 0.05 # the map opens when the rover has moved this far from its first pose (or on explore_cmd).# Measuring the displacement needs no clock: the wait accumulates nothing however# long it lasts, and the first useful revolution leaves with the rover.+PREMAP_Z_BAND_M = (0.05, 1.30) # premap points strictly between these heights (its own z) become walls: above the+ # bottom layer of a 5 cm voxel premap (a 3D lidar's floor), up to the top of the band+ # lidar_odometry turns depth into obstacles (module docstring, the premap)+PREMAP_MOVE_M = 0.02 # a fix this far (or less) from the one the premap was written with changes nothing...+PREMAP_TURN_DEG = 1.0 # ...nor one turned this much or less; beyond either, the premap is cleared and written againdef carve_z_band(camera_z_m: float | None = None) -> tuple[float, float]:@@ -222,6 +306,23 @@ def camera_xy(pose_xy: tuple[float, float], yaw: float) -> tuple[float, float]:return (pose_xy[0] + c * cx - s * cy, pose_xy[1] + s * cx + c * cy)+def planar_fix(transform: Any) -> tuple[float, float, float]:+ """(x, y, yaw) of a `world` -> `map` transform: where the premap's origin+ sits in `world` and how it is turned. A point (mx, my) of the premap lands+ at (x + cos(yaw) mx - sin(yaw) my, y + sin(yaw) mx + cos(yaw) my), the+ convention dimOS's own ray tracer places a loaded map by. The transform's+ z, roll and pitch are dropped (module docstring, the premap)."""+ q = transform.rotation+ yaw = math.atan2(2 * (q.w * q.z + q.x * q.y), 1 - 2 * (q.y * q.y + q.z * q.z))+ return float(transform.translation.x), float(transform.translation.y), yaw+++def fix_moved(a: tuple[float, float, float], b: tuple[float, float, float]) -> tuple[float, float]:+ """How far fix `b` is from fix `a`: (metres in the plane, degrees of yaw, wrapped)."""+ turn = (b[2] - a[2] + math.pi) % (2 * math.pi) - math.pi+ return math.hypot(b[0] - a[0], b[1] - a[1]), abs(math.degrees(turn))++class ScoredGrid:"""Two int8 score layers over a fixed world grid, plus where each cell was last hit from."""@@ -240,6 +341,12 @@ class ScoredGrid:# monotonic clock means nothing in another process.self._last_low_hit = np.full((self.n, self.n), -np.inf, dtype=np.float64)self._cam_prev = np.zeros((self.n, self.n), dtype=bool) # camera obstacle cells of the PREVIOUS depth frame (moving-object gate)+ self.premap = np.zeros((self.n, self.n), dtype=bool) # cells the premap made (or found) a lidar wall+ self._premap_added = np.zeros((self.n, self.n), dtype=np.int8) # ...and what it added to their lidar score+ # premap walls a lidar ray took below OCCUPIED_AT: seen free, so a+ # re-write never paints them back; a lidar hit from a new viewpoint+ # lifts the mark (the sensor sees something there again)+ self.premap_carved = np.zeros((self.n, self.n), dtype=bool)# ---- coordinates -------------------------------------------------------def cell(self, x: np.ndarray, y: np.ndarray) -> tuple[np.ndarray, np.ndarray]:@@ -274,6 +381,8 @@ class ScoredGrid:layer[gy, gx] = newself._last_hit_xy[gy[moved], gx[moved]] = from_xyself.seen[gy, gx] = True+ if layer is self.lidar:+ self.premap_carved[gy[moved], gx[moved]] = Falsereturn int(moved.sum())def _miss(self, layer: np.ndarray, gx: np.ndarray, gy: np.ndarray) -> None:@@ -283,6 +392,8 @@ class ScoredGrid:new = np.maximum(cur - 1, FREE_FLOOR).astype(np.int8)layer[gy, gx] = newself.seen[gy, gx] = True+ if layer is self.lidar:+ self.premap_carved[gy, gx] |= self.premap[gy, gx] & (new < OCCUPIED_AT)_revs: int = 0@@ -434,6 +545,46 @@ class ScoredGrid:self.seen[gy, gx] = Truereturn int(len(gx))+ def write_premap(self, xy: np.ndarray) -> tuple[int, int]:+ """The premap's walls, already placed in `world` (x, y): each cell+ becomes a lidar wall at OCCUPIED_AT (a cell already above keeps its+ score), flagged premap, and remembers what the premap added to it.+ A cell the lidar has seen free (score below 0, or a premap wall a+ lidar ray carved, `premap_carved`) stays as it is: the live map wins+ over the prior. Not marked seen, and the camera layer is never touched+ (module docstring, the premap). A premap already in the grid is+ cleared first. Returns (cells flagged, cells left free)."""+ self.clear_premap()+ gx, gy = self.cell(xy[:, 0], xy[:, 1])+ if len(gx) == 0:+ return 0, 0+ flat = np.unique(gy * self.n + gx)+ gx, gy = flat % self.n, flat // self.n+ cur = self.lidar[gy, gx].astype(np.int16)+ free = (cur < 0) | self.premap_carved[gy, gx]+ gx, gy, cur = gx[~free], gy[~free], cur[~free]+ added = np.maximum(OCCUPIED_AT - cur, 0)+ self.lidar[gy, gx] = (cur + added).astype(np.int8)+ self._premap_added[gy, gx] = added.astype(np.int8)+ self.premap[gy, gx] = True+ return int(len(gx)), int(free.sum())++ def clear_premap(self) -> int:+ """Take the premap out: every flagged cell loses exactly what the+ premap added to its lidar score, so what the sensors did since stays+ (a live hit from a new viewpoint, a ray's miss, the body) and a+ corrected fix leaves no ghost wall. A cell no sensor saw goes back to+ unknown. The carved marks stay: what a lidar ray saw free is still+ free for the next write. Returns the cells cleared."""+ gy, gx = np.nonzero(self.premap)+ if len(gx) == 0:+ return 0+ back = self.lidar[gy, gx].astype(np.int16) - self._premap_added[gy, gx]+ self.lidar[gy, gx] = np.clip(back, FREE_FLOOR, HIT_CAP).astype(np.int8)+ self._premap_added[gy, gx] = 0+ self.premap[gy, gx] = False+ return int(len(gx))+def _ray_cells(self, from_xy: tuple[float, float], hits_xy: np.ndarray):"""Cells crossed by the rays from `from_xy` to each hit (capped atRAY_MAX_M), stopping one cell short of the hit; one sample per cell."""@@ -492,10 +643,12 @@ class ScoredGrid:return int(self.occupancy()[gy[0], gx[0]]) if len(gx) else -1def cropped(self, margin_cells: int = 20) -> tuple[np.ndarray, float, float] | None:- """The occupancy grid cropped to the observed area (+ margin): (grid, origin_x, origin_y)."""- if not self.seen.any():+ """The occupancy grid cropped to the observed area and the premap's+ walls (+ margin): (grid, origin_x, origin_y)."""+ known = self.seen | self.premap+ if not known.any():return None- ys, xs = np.nonzero(self.seen)+ ys, xs = np.nonzero(known)y0, y1 = max(0, ys.min() - margin_cells), min(self.n, ys.max() + margin_cells + 1)x0, x1 = max(0, xs.min() - margin_cells), min(self.n, xs.max() + margin_cells + 1)return self.occupancy()[y0:y1, x0:x1], self.ox + x0 * self.res, self.oy + y0 * self.res@@ -525,7 +678,9 @@ class MossCostMap(Module):z = 0), `odom` (base pose in world), `explore_cmd` (the order that opensthe map), `explore_done` and `stop_explore_cmd` (logged; the map keepswriting), `clear_map_cmd` (a fresh grid), `bump` (a front contact: a wall- ahead only with MOSS_CONTACT_WALL=1, module docstring).+ ahead only with MOSS_CONTACT_WALL=1, module docstring), `loaded_map` and+ `tf` (a relocalization's premap and its `world` -> `map` fix: lidar walls+ with `premap=True`, ignored otherwise; module docstring, the premap).Out: `global_costmap` (the stream name the planner and the explorerlisten to)."""@@ -537,11 +692,25 @@ class MossCostMap(Module):explore_cmd: In[Bool] # the order to explore: opens the map before the first motionclear_map_cmd: In[Bool] # the cockpit's Clear map (explore_ctl.py clear): a fresh grid, the mission gate as it isbump: In[Bool] # a front contact: logged; with MOSS_CONTACT_WALL=1 and the rover driving straight forward, the band ahead becomes a wall+ loaded_map: In[PointCloud2] # a relocalization's premap, in `map`: lidar walls once a fix places it (premap=True)+ tf: In[TFMessage] # the tf stream: only a `world` -> `map` transform (the relocalization's fix) is readglobal_costmap: Out[OccupancyGrid]- def __init__(self, world_frame: str = "world", **kwargs: Any) -> None:+ def __init__(self, world_frame: str = "world", premap: bool = False, map_frame: str = "map",+ **kwargs: Any) -> None:super().__init__(**kwargs)self.world_frame = world_frame+ # the premap (module docstring): off, every loaded_map is ignored+ # (said once); on, it becomes lidar walls once a fix places it+ self.premap_on = bool(premap)+ self.map_frame = map_frame+ self._premap_xy: np.ndarray | None = None # the premap's points inside PREMAP_Z_BAND_M, (x, y) in `map`+ self._premap_points = 0 # ...out of this many in the loaded cloud+ self._premap_key: bytes | None = None # the loaded cloud's points as bytes: the same premap sent again is no news+ self._premap_fix: tuple[float, float, float] | None = None # the latest `world` -> `map` fix (x, y, yaw)+ self._premap_written: tuple[float, float, float] | None = None # the fix the premap was last written with+ self._premap_ignored_said = False+ self._premap_fix_said = False # "fix received" is said once, when the fix comes firstself._grid: ScoredGrid | None = Noneself._pose_xy: tuple[float, float] | None = Noneself._pose_yaw = 0.0 # the camera rides on the nose: its rays need the heading@@ -570,7 +739,8 @@ class MossCostMap(Module):lo, hi = carve_z_band()logger.info(f"costmap up: {RESOLUTION_M} m cells, hit cap {HIT_CAP}, free floor {FREE_FLOOR}, "f"occupied at {OCCUPIED_AT} (new viewpoint {NEW_VIEWPOINT_M:g} m), rays carved to {RAY_MAX_M:g} m, "- f"camera rays carve {lo:.3f}-{hi:.3f} m up, contact wall {'on' if CONTACT_WALL else 'off'}")+ f"camera rays carve {lo:.3f}-{hi:.3f} m up, contact wall {'on' if CONTACT_WALL else 'off'}, "+ f"premap {'on' if self.premap_on else 'off'}")async def handle_odom(self, msg: PoseStamped) -> None:self._pose_xy = (float(msg.position.x), float(msg.position.y))@@ -583,6 +753,7 @@ class MossCostMap(Module):if self._grid is None:self._grid = ScoredGrid(centre=self._pose_xy)logger.info(f"costmap: grid centred on ({self._pose_xy[0]:+.2f}, {self._pose_xy[1]:+.2f})")+ self._premap_update() # a premap and its fix that came before the first poseself._check_departure()if self._write_frozen():return # before the mission: not even body_clear@@ -694,12 +865,104 @@ class MossCostMap(Module):a run that has not started still waits for its order or its 5 cm."""if not bool(getattr(msg, "data", False)) or self._pose_xy is None:return+ had_premap = self._grid is not None and bool(self._grid.premap.any())self._grid = ScoredGrid(centre=self._pose_xy)self._revolutions = 0self._cam_key = Noneself._camera_pts = self._camera_cells = self._camera_held = self._camera_frames = self._camera_repeats = 0logger.info(f"costmap: map CLEARED on request - fresh grid centred on ({self._pose_xy[0]:+.2f}, "f"{self._pose_xy[1]:+.2f}), mission {'open' if not self._mission_frozen else 'not started'}")+ if had_premap:+ # the premap goes with the rest: a premap that walls the rover in+ # must have a way out. It comes back with a fix that moved.+ logger.info(f"costmap: premap cleared with the map - written again on a fix that moves more than "+ f"{PREMAP_MOVE_M * 100:g} cm or {PREMAP_TURN_DEG:g} deg")++ async def handle_loaded_map(self, msg: PointCloud2) -> None:+ """A relocalization's premap, in `map`. Off: ignored, said once. On:+ its points inside PREMAP_Z_BAND_M are kept, and written once a fix+ places them (_premap_update). The same premap sent again changes+ nothing; another one replaces it (the old walls cleared first)."""+ if not self.premap_on:+ if not self._premap_ignored_said:+ self._premap_ignored_said = True+ logger.info("costmap: premap ignored (MOSS_PREMAP=0)")+ return+ pts = np.asarray(msg.as_numpy()[0], dtype=np.float64).reshape(-1, 3)+ key = pts.tobytes()+ if key == self._premap_key:+ return+ lo, hi = PREMAP_Z_BAND_M+ self._premap_key = key+ self._premap_points = len(pts)+ self._premap_xy = pts[(pts[:, 2] > lo) & (pts[:, 2] < hi), :2]+ # said for each new premap: a run on MOSS_PREMAP=1 without this line+ # received none (the relocalization on another transport, or its one+ # publish lost)+ logger.info(f"costmap: premap received: {len(pts)} points, {len(self._premap_xy)} in the {lo:g}-{hi:g} m band"+ + ("" if self._premap_fix is not None else "; written when a world -> map fix arrives on tf"))+ if self._premap_written is not None:+ self._premap_clear()+ self._premap_written = None+ self._premap_update()++ async def handle_tf(self, msg: TFMessage) -> None:+ """The tf stream. Only a `world` -> `map` transform is read (the+ relocalization's fix, sent again every few seconds); the odometry's+ and the cameras' transforms pass by. Off: nothing is read."""+ if not self.premap_on:+ return+ fix = None+ for t in getattr(msg, "transforms", ()):+ if t.frame_id == self.world_frame and t.child_frame_id == self.map_frame:+ fix = planar_fix(t)+ if fix is None:+ return+ if self._premap_xy is None and not self._premap_fix_said:+ self._premap_fix_said = True+ logger.info(f"costmap: premap fix received on tf: ({fix[0]:+.2f}, {fix[1]:+.2f}, "+ f"{math.degrees(fix[2]):+.1f} deg); written when a loaded_map arrives")+ self._premap_fix = fix+ self._premap_update()++ def _premap_update(self) -> None:+ """Write the premap once the grid, the premap and a fix are all here;+ write it again only when the fix moved more than PREMAP_MOVE_M or+ turned more than PREMAP_TURN_DEG from the one it was written with+ (the same fix sent again changes nothing, and a fix creeping by small+ steps is measured against the written one, so it cannot drift away+ unnoticed). Published at once only when the map has already put a+ grid out since the mission opened (or since Clear map); before that,+ the premap goes out with the map's first grid (module docstring)."""+ if self._grid is None or self._premap_xy is None or self._premap_fix is None:+ return+ fix = self._premap_fix+ if self._premap_written is not None:+ moved_m, turned_deg = fix_moved(self._premap_written, fix)+ if moved_m <= PREMAP_MOVE_M and turned_deg <= PREMAP_TURN_DEG:+ return+ logger.info(f"costmap: premap re-written: tf moved by ({moved_m * 100:.1f} cm, {turned_deg:.1f} deg)")+ self._premap_clear()+ self._premap_written = fix+ x, y, yaw = fix+ c, s = math.cos(yaw), math.sin(yaw)+ mx, my = self._premap_xy[:, 0], self._premap_xy[:, 1]+ walls, kept_free = self._grid.write_premap(np.column_stack([x + c * mx - s * my, y + s * mx + c * my]))+ lo, hi = PREMAP_Z_BAND_M+ logger.info(f"costmap: premap written: {walls} walls from {self._premap_points} points, "+ f"tf ({x:+.2f}, {y:+.2f}, {math.degrees(yaw):+.1f} deg); {len(self._premap_xy)} points in the "+ f"{lo:g}-{hi:g} m band, {kept_free} cells the lidar has seen free left free")+ # _revolutions counts the revolutions written since the mission opened+ # (or since Clear map), and the first grid leaves at PUBLISH_EVERY: a+ # grid of premap walls alone would read to explorer2 as a walled-in start+ if self._revolutions >= PUBLISH_EVERY:+ self._publish()++ def _premap_clear(self) -> None:+ """Take the premap's walls out of the grid (ScoredGrid.clear_premap), said in the log."""+ n = self._grid.clear_premap() if self._grid is not None else 0+ if n:+ logger.info(f"costmap: premap cleared: {n} cells")def _camera_xy(self) -> tuple[float, float]:"""The ray origin for everything the camera saw (see camera_xy)."""