Skip to content

armgit / moss

Public Imported from GitHub
0

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.py
index 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: E402
from moss_dimos import costmap2d as cmod # noqa: E402
+from moss_dimos.robot_profile import current as _current_profile
from moss_dimos.costmap2d import ( # noqa: E402
FREE_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_s
test_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.py
index 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 all
CAMERA_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 width
CAMERA_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 0
return 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 the
camera 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 writing
explore_cmd: In[Bool] # the order to explore: opens the map before the first motion
clear_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 wall
global_costmap: Out[OccupancyGrid]
def __init__(self, world_frame: str = "world", **kwargs: Any) -> None:
@@ -505,6 +530,19 @@ class MossCostMap(Module):
self._mission_frozen = False
logger.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 map
learned goes, centred again on the current pose; the mission gate stays