MOSS: a small, mostly 3D-printed litter-picking rover. Cleaner streets, one maker at a time.
Python
costmap: contact memory. A bump (the stall guard) marks the band just ahead of the body as a wall at once, so the planner routes around what the lidar and the camera did not see instead of driving into it again; freed by seen floor or the body
metrox67a6987522e7
diff --git a/benches/bench_costmap2d_cold.py b/benches/bench_costmap2d_cold.pyindex cbcf686..c00ce60 100644--- a/benches/bench_costmap2d_cold.py+++ b/benches/bench_costmap2d_cold.py@@ -80,9 +80,10 @@ for _var in [v for v in os.environ if v.startswith("MOSS_")]:from dimos_lcm.std_msgs import Bool # noqa: E402from moss_dimos import costmap2d as cmod # noqa: E402+from moss_dimos.robot_profile import current as _current_profilefrom moss_dimos.costmap2d import ( # noqa: E402FREE_FLOOR, HIT_CAP, MISSION_START_M, OCCUPIED_AT, PUBLISH_EVERY, RAY_EVERY, RESOLUTION_M,- MossCostMap, ScoredGrid, lidar_returns,+ CONTACT_BAND_M, MossCostMap, ScoredGrid, lidar_returns,)from moss_dimos.robot_profile import MOSS # noqa: E402@@ -444,6 +445,36 @@ def test_clear_map_starts_a_fresh_grid_and_keeps_the_gate() -> None:print(" clear_map_cmd: fresh grid centred on the pose, open mission stays open, unstarted one stays shut, False ignored")+def test_a_contact_marks_the_band_ahead_as_a_wall() -> None:+ """The stall guard fired: the band just beyond the body's front becomes a wall+ at once, in the heading direction, and the planner's occupancy shows it;+ the body itself and the floor behind stay clear; a bump before any pose or+ with the mission shut does nothing; the camera's bare floor frees it."""+ m = costmap()+ asyncio.run(m.handle_bump(Bool(data=True))) # no pose yet+ assert m._grid is None+ asyncio.run(m.handle_odom(_odom(0.0, 0.0, yaw=0.0)))+ asyncio.run(m.handle_bump(Bool(data=True))) # mission not started: nothing written+ assert m._grid.value_at(0.35, 0.0) != 100+ asyncio.run(m.handle_explore_cmd(Bool(data=True)))+ asyncio.run(m.handle_bump(Bool(data=True)))+ hx, _ = _current_profile().body_clear_m+ assert m._grid.value_at(hx + 0.10, 0.0) == 100, "the band ahead is a wall"+ assert m._grid.value_at(hx + 0.10, 0.12) == 100 and m._grid.value_at(hx + 0.10, -0.12) == 100, "across the body's width"+ assert m._grid.value_at(0.0, 0.0) != 100 and m._grid.value_at(-0.3, 0.0) != 100, "the body and the floor behind are not"+ assert m._grid.value_at(hx + 0.40, 0.0) != 100, "the wall is a short band, not a ray"+ # heading east, the band lies east; turned north, a new contact marks north+ asyncio.run(m.handle_odom(_odom(0.0, 0.0, yaw=math.pi / 2)))+ asyncio.run(m.handle_bump(Bool(data=True)))+ assert m._grid.value_at(0.0, hx + 0.10) == 100, "a contact while heading north marks the band north"+ floor = np.array([[hx + 0.10, 0.0, 0.0], [hx + 0.12, 0.02, 0.0]])+ for _ in range(3):+ asyncio.run(m.handle_camera_floor(_cloud(floor)))+ assert m._grid.value_at(hx + 0.10, 0.0) != 100, "bare floor seen by the camera frees the contact mark"+ asyncio.run(m.handle_bump(Bool(data=False)))+ print(f" contact: the band {CONTACT_BAND_M[0]:.2f}-{CONTACT_BAND_M[1]:.2f} m beyond the body is a wall at once, in the heading, freed by seen floor")++def test_what_goes_out_on_global_costmap() -> None:"""One cropped OccupancyGrid every PUBLISH_EVERY revolutions."""from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid@@ -485,7 +516,7 @@ def test_the_mission_signals_cross_the_process_boundary() -> None:cm = {(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",+ ("clear_map_cmd", Bool): "in", ("bump", Bool): "in",("explore_done", Bool): "in", ("stop_explore_cmd", Bool): "in",("global_costmap", OccupancyGrid): "out"}assert cm == want, f"stream table: extra {set(cm) - set(want)}, missing {set(want) - set(cm)}"@@ -892,6 +923,7 @@ TESTS = (test_leg_needs_two_viewpoints_and_ray_is_free, test_ramp_hit_from_one_stest_parked_rover_never_erases_an_established_wall, test_body_clear_is_the_moss_footprint,test_the_lidar_tag_is_one_exact_height, test_the_map_writes_only_during_the_mission,test_explore_order_opens_the_map_before_any_motion, test_clear_map_starts_a_fresh_grid_and_keeps_the_gate,+ test_a_contact_marks_the_band_ahead_as_a_wall,test_what_goes_out_on_global_costmap,test_the_mission_signals_cross_the_process_boundary, test_revolution_cost,test_camera_one_viewpoint_never_becomes_a_wall, test_camera_wall_no_longer_seen_is_carved_free,diff --git a/moss_dimos/costmap2d.py b/moss_dimos/costmap2d.pyindex 1ed77a5..92a9719 100644--- a/moss_dimos/costmap2d.py+++ b/moss_dimos/costmap2d.py@@ -150,6 +150,7 @@ RAY_MAX_M = _env_float("MOSS_RAY_MAX_M", 4.0, 0.0, GRID_SPAN_M) # rays are car# 4.5 m away stays until the rover comes within RAY_MAX_M of where they stood. 0 = no carving.RAY_EVERY = 2 # carve on every other revolution; hits are taken on allCAMERA_HFOV_DEG = 87.0 # the D455's horizontal field of view: the rays carve the camera layer only inside it+CONTACT_BAND_M = (0.02, 0.20) # a contact marks the band this far beyond the body's front edge, across its widthCAMERA_CARVE_MAX_M = 3.0 # ...and only this close to the rover. Coupled to lidar_odometry's DEPTH_MAX_M (depth# range along the camera's axis) and OBSTACLE_MAX_M (camera obstacles, measured from# base_link), both 3.0 m: measured from base_link, a cell within this range ahead of the@@ -302,6 +303,29 @@ class ScoredGrid:return 0return self._hit(self.camera, hits_xy[:, 0], hits_xy[:, 1], from_xy)+ def contact(self, pose: tuple) -> int:+ """The rover pushed against something its sensors did not map (the stall+ guard fired): the cells of a short band right ahead of the body become a+ wall on the camera layer at once (no viewpoint rule: a contact is a+ physical certainty, like body_clear). The planner then routes around the+ spot instead of driving back into it. Freed like any camera wall: by the+ camera seeing bare floor there, or by the body driving over it."""+ x, y, yaw = pose+ c, s = np.cos(yaw), np.sin(yaw)+ hx, hy = _current_profile().body_clear_m+ ax = np.arange(hx + CONTACT_BAND_M[0], hx + CONTACT_BAND_M[1] + 1e-9, self.res / 2)+ ay = np.arange(-hy, hy + 1e-9, self.res / 2)+ bx, by = np.meshgrid(ax, ay)+ gx, gy = self.cell((x + c * bx - s * by).ravel(), (y + s * bx + c * by).ravel())+ if len(gx) == 0:+ return 0+ flat = np.unique(gy * self.n + gx)+ gx, gy = flat % self.n, flat // self.n+ self.camera[gy, gx] = np.maximum(self.camera[gy, gx], np.int8(OCCUPIED_AT))+ self.camera_wall[gy, gx] = True+ self.seen[gy, gx] = True+ return int(len(gx))+def camera_floor(self, floor_xy: np.ndarray) -> None:"""World-frame bare-floor samples from the depth camera: a miss on thecamera layer (a shoe that was taken away stops being an obstacle once@@ -411,6 +435,7 @@ class MossCostMap(Module):stop_explore_cmd: In[Bool] # exploration stopped on request (explore_ctl.py stop, the cockpit): logged, the map keeps writingexplore_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 is+ bump: In[Bool] # a contact (the base's stall guard, the explorer's back-off): the band ahead becomes a wallglobal_costmap: Out[OccupancyGrid]def __init__(self, world_frame: str = "world", **kwargs: Any) -> None:@@ -505,6 +530,19 @@ class MossCostMap(Module):self._mission_frozen = Falselogger.info("costmap: explore_cmd received - map writing OPEN for the mission")+ async def handle_bump(self, msg: Bool) -> None:+ """A contact: the cells just ahead of the body become a wall at once, so the+ planner routes around what the sensors did not see (a cable, a step, a+ bin edge below the camera's view) instead of driving into it again."""+ if not bool(getattr(msg, "data", False)) or self._grid is None or self._pose_xy is None:+ return+ if self._write_frozen():+ return+ n = self._grid.contact((self._pose_xy[0], self._pose_xy[1], self._pose_yaw))+ logger.info(f"costmap: contact at ({self._pose_xy[0]:+.2f}, {self._pose_xy[1]:+.2f}) heading "+ f"{np.degrees(self._pose_yaw):+.0f} deg: {n} cells ahead marked as a wall")+ self._publish()+async def handle_clear_map_cmd(self, msg: Bool) -> None:"""A fresh grid on request (the cockpit's Clear map): everything the maplearned goes, centred again on the current pose; the mission gate stays