WIP: feat(cockpit): Costmap Panel

dimos/msgs/nav_msgs/OccupancyGrid.py

@@ -24,9 +24,7 @@
     OccupancyGrid as LCMOccupancyGrid,
 )
 from dimos_lcm.std_msgs import Time as LCMTime
-import matplotlib.pyplot as plt
 import numpy as np
-from PIL import Image
 
 from dimos.msgs.geometry_msgs.Pose import Pose
 from dimos.msgs.geometry_msgs.Vector3 import Vector3, VectorLike
@@ -36,6 +34,11 @@
 @lru_cache(maxsize=16)
 def _get_matplotlib_cmap(name: str):  # type: ignore[no-untyped-def]
     """Get a matplotlib colormap by name (cached for performance)."""
+    # Lazy import: matplotlib serves only this plotting helper and costs
+    # ~0.5 s / ~28 MiB, which message-transport users (e.g. the relay
+    # bridge) must not pay at import.
+    import matplotlib.pyplot as plt
+
     return plt.get_cmap(name)
 
 
@@ -96,6 +99,29 @@ def _build_occupancy_lut(
     from rerun._baseclasses import Archetype
 
 
+def block_max_reduce(cells: NDArray[np.int8], factor: int) -> NDArray[np.int8]:
+    """Coarsen an occupancy grid by taking the max of factor x factor blocks.
+
+    Block maximum (not mean) so coarsening never erases an obstacle; a block
+    is unknown (-1) only when every cell in it is unknown. Trailing remainder
+    rows/cols are trimmed, keeping row 0/col 0 - the origin corner - so the
+    grid origin stays valid. Grids thinner than `factor` pass through.
+    """
+    h, w = cells.shape[:2]
+    new_h, new_w = h // factor, w // factor
+    if new_h == 0 or new_w == 0:
+        return cells
+    trimmed = cells[: new_h * factor, : new_w * factor]
+    blocks = trimmed.reshape(new_h, factor, new_w, factor)
+    # Sink unknown below every known value for the max, then map it back.
+    as_int = blocks.astype(np.int16)
+    known = np.where(as_int < 0, -1000, as_int)
+    reduced = known.max(axis=(1, 3))
+    reduced[reduced == -1000] = -1
+    result: NDArray[np.int8] = reduced.astype(np.int8)
+    return result
+
+
 class CostValues(IntEnum):
     """Standard cost values for occupancy grid cells.
 
@@ -244,6 +270,10 @@ def from_path(cls, path: Path) -> OccupancyGrid:
             case ".npy":
                 return cls(grid=np.load(path))
             case ".png":
+                # Lazy import: Pillow serves only this file loader; message
+                # transport must not depend on it.
+                from PIL import Image
+
                 img = Image.open(path).convert("L")
                 return cls(grid=np.array(img).astype(np.int8))
             case _:
@@ -349,13 +379,22 @@ def lcm_decode(cls, data: bytes | BinaryIO) -> OccupancyGrid:
         ts = lcm_msg.header.stamp.sec + (lcm_msg.header.stamp.nsec / 1_000_000_000)
         frame_id = lcm_msg.header.frame_id
 
-        # Extract grid data
+        # Extract grid data; empty stays 2-D so the constructor accepts it.
         if lcm_msg.data and lcm_msg.info.width > 0 and lcm_msg.info.height > 0:
             grid = np.array(lcm_msg.data, dtype=np.int8).reshape(
                 (lcm_msg.info.height, lcm_msg.info.width)
             )
         else:
-            grid = np.array([], dtype=np.int8)
+            grid = np.zeros((0, 0), dtype=np.int8)
+
+        # The wire origin decodes as a plain generated pose without the dimos
+        # Pose surface (.yaw etc.); rebuild it field by field so the origin
+        # property honors its declared type.
+        o = lcm_msg.info.origin
+        lcm_msg.info.origin = Pose(
+            position=[o.position.x, o.position.y, o.position.z],
+            orientation=[o.orientation.x, o.orientation.y, o.orientation.z, o.orientation.w],
+        )
 
         # Create new instance
         instance = cls(

dimos/msgs/nav_msgs/test_OccupancyGrid.py

@@ -24,7 +24,9 @@
 from dimos.mapping.occupancy.inflation import simple_inflate
 from dimos.mapping.pointclouds.occupancy import general_occupancy
 from dimos.msgs.geometry_msgs.Pose import Pose
-from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid
+from dimos.msgs.geometry_msgs.Quaternion import Quaternion
+from dimos.msgs.geometry_msgs.Vector3 import Vector3
+from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid, block_max_reduce
 from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2
 from dimos.utils.data import get_data
 
@@ -131,6 +133,27 @@ def test_lcm_encode_decode() -> None:
     assert decoded.grid[5, 10] == 50  # Value we set should be preserved in grid
 
 
+def test_lcm_decode_origin_is_dimos_pose() -> None:
+    """The decoded origin must be the dimos Pose (with .yaw), not the raw
+    generated pose from the wire (the relay costmap encoder reads origin.yaw)."""
+    quat = Quaternion.from_euler(Vector3(0.0, 0.0, 0.5))
+    origin = Pose(1.0, 2.0, 0.0, quat.x, quat.y, quat.z, quat.w)
+    grid = OccupancyGrid(grid=np.zeros((2, 3), dtype=np.int8), resolution=0.05, origin=origin)
+
+    decoded = OccupancyGrid.lcm_decode(grid.lcm_encode())
+
+    assert isinstance(decoded.origin, Pose)
+    assert decoded.origin.yaw == pytest.approx(0.5)
+
+
+def test_lcm_decode_empty_grid() -> None:
+    """An empty grid (mapper warming up) must survive the wire; the decoder
+    used to rebuild it as a 1-D array the constructor rejects."""
+    decoded = OccupancyGrid.lcm_decode(OccupancyGrid().lcm_encode())
+    assert decoded.grid.size == 0
+    assert isinstance(decoded.origin, Pose)
+
+
 def test_string_representation() -> None:
     """Test string representations."""
     grid = OccupancyGrid(width=10, height=10, resolution=0.1, frame_id="map")
@@ -363,3 +386,34 @@ def test_max() -> None:
     assert maxed.unknown_cells == 3  # Same as original
     assert maxed.occupied_cells == 13  # All non-unknown cells
     assert maxed.free_cells == 0  # No free cells
+
+
+def test_block_max_reduce_preserves_lone_obstacle() -> None:
+    cells = np.zeros((10, 10), dtype=np.int8)
+    cells[3, 4] = 100
+    reduced = block_max_reduce(cells, 5)
+    assert reduced.shape == (2, 2)
+    assert reduced.dtype == np.int8
+    assert reduced[0, 0] == 100
+    assert reduced[0, 1] == 0
+
+
+def test_block_max_reduce_unknown_only_when_whole_block_unknown() -> None:
+    cells = np.array([[-1, -1, -1, 50], [-1, -1, 0, -1]], dtype=np.int8)
+    reduced = block_max_reduce(cells, 2)
+    assert reduced.tolist() == [[-1, 50]]
+    assert reduced.dtype == np.int8
+
+
+def test_block_max_reduce_trims_remainder() -> None:
+    cells = np.arange(35, dtype=np.int8).reshape(7, 5)
+    reduced = block_max_reduce(cells, 2)
+    # 7x5 at factor 2 keeps rows 0-5 and cols 0-3 (origin corner side).
+    assert reduced.shape == (3, 2)
+    assert reduced[0, 0] == 6  # max of rows 0-1, cols 0-1
+    assert reduced[2, 1] == 28  # max of rows 4-5, cols 2-3
+
+
+def test_block_max_reduce_thin_grid_passes_through() -> None:
+    cells = np.zeros((1, 10), dtype=np.int8)
+    assert block_max_reduce(cells, 5) is cells

dimos/teleop/hosted/map_compress.py

@@ -33,7 +33,7 @@
 from dimos.core.module import Module, ModuleConfig
 from dimos.core.stream import In, Out
 from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped
-from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid
+from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid, block_max_reduce
 from dimos.utils.logging_config import setup_logger
 
 logger = setup_logger()
@@ -92,7 +92,7 @@ def _on_costmap(self, grid: OccupancyGrid) -> None:
             if 0 < res < self.config.map_min_resolution:
                 factor = max(1, round(self.config.map_min_resolution / res))
                 if factor > 1:
-                    img_cells = self._block_max(cells, factor)
+                    img_cells = block_max_reduce(cells, factor)
                     res = res * factor
 
             ok, buf = cv2.imencode(".png", self._occupancy_to_bgra(img_cells))
@@ -143,24 +143,6 @@ def _on_odom(self, pose: PoseStamped) -> None:
             return
         self._last_odom_pub = now
 
-    @staticmethod
-    def _block_max(cells: Any, factor: int) -> Any:
-        """Block maximum (not mean) so coarsening never erases an obstacle."""
-        import numpy as np
-
-        h, w = cells.shape[:2]
-        new_h, new_w = h // factor, w // factor
-        if new_h == 0 or new_w == 0:
-            return cells
-        trimmed = cells[: new_h * factor, : new_w * factor]
-        blocks = trimmed.reshape(new_h, factor, new_w, factor)
-        # Sink unknown below every known value for the max, then map it back.
-        as_int = blocks.astype(np.int16)
-        known = np.where(as_int < 0, -1000, as_int)
-        reduced = known.max(axis=(1, 3))
-        reduced[reduced == -1000] = -1
-        return reduced.astype(np.int8)
-
     @staticmethod
     def _occupancy_to_bgra(cells: Any) -> Any:
         """Occupancy int8 {-1,0,1..100} → BGRA for PNG; unknown transparent."""

dimos/web/relay_bridge/_wt_session.py

@@ -66,10 +66,12 @@
 # Realistic payload caps for channels whose encoding is known from the
 # watched robot's manifest (frames themselves carry no encoding);
 # MAX_DATA_FRAME_BYTES stays the outer bound for everything else. Generous:
-# a 4K quality-90 JPEG is ~4 MiB, a pose JSON object ~100 B.
+# a 4K quality-90 JPEG is ~4 MiB, a pose JSON object ~100 B, a compressed
+# long-run costmap ~10-30 KB (the cap leaves room for pathological grids).
 _MAX_PAYLOAD_BYTES = {
     "jpeg.v1": 8 * 1024 * 1024,
     "pose.json.v1": 64 * 1024,
+    "costmap.zlib.v1": 8 * 1024 * 1024,
 }
 
 # Relay-pushed control messages (subs snapshots, robots, manifest) waiting for

dimos/web/relay_bridge/gen_costmap_fixtures.py

@@ -0,0 +1,111 @@
+# Copyright 2026 Dimensional Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+"""Golden costmap.zlib.v1 vectors: the Python encoder is the reference.
+
+Writes web/shared/fixtures/costmap_frames.json, pinning the encoder's exact
+zlib bytes: vitest inflates payload_b64 and byte-compares against grid_b64,
+pytest (test_costmap_encoding.py) re-encodes and byte-compares against
+payload_b64, so drift on either side fails a suite.
+
+Regenerate with:  uv run python -m dimos.web.relay_bridge.gen_costmap_fixtures
+
+gen.ts does not write this file: the payloads must be Python zlib output
+(CompressionStream compresses to different bytes).
+"""
+
+from __future__ import annotations
+
+import base64
+import json
+from typing import TYPE_CHECKING, Any, cast
+
+import numpy as np
+
+from dimos.msgs.geometry_msgs.Pose import Pose
+from dimos.msgs.geometry_msgs.Quaternion import Quaternion
+from dimos.msgs.geometry_msgs.Vector3 import Vector3
+from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid
+from dimos.web.relay_bridge.locate import find_web_dir
+from dimos.web.relay_bridge.relay_bridge_module import _encode_costmap
+
+if TYPE_CHECKING:
+    from dimos.web.relay_bridge.relay_bridge_module import RelayBridgeModule
+
+
+def grid_msg(rows: list[list[int]], res: float, x: float, y: float, yaw: float) -> OccupancyGrid:
+    """Fixture-shaped OccupancyGrid; also used by test_costmap_encoding.py."""
+    quat = Quaternion.from_euler(Vector3(0.0, 0.0, yaw))
+    origin = Pose(x, y, 0.0, quat.x, quat.y, quat.z, quat.w)
+    grid = np.array(rows, dtype=np.int8)
+    return OccupancyGrid(grid=grid, resolution=res, origin=origin, ts=1752576000.5)
+
+
+# name -> (rows, res, origin x, origin y, origin yaw). small_map covers every
+# value class of the wire contract (-1 unknown, 0 free, graded, 100 lethal).
+CASES: dict[str, tuple[list[list[int]], float, float, float, float]] = {
+    "small_map": (
+        [
+            [-1, -1, 0, 0, 0],
+            [-1, 0, 1, 50, 0],
+            [0, 0, 99, 100, 0],
+            [0, -1, -1, 0, 100],
+        ],
+        0.05,
+        -1.25,
+        2.5,
+        0.0,
+    ),
+    "with_yaw": (
+        [
+            [0, 100, -1],
+            [0, 50, 0],
+            [-1, 0, 25],
+        ],
+        0.1,
+        1.5,
+        -0.75,
+        0.25,
+    ),
+    "single_row": ([[0, -1, 1, 99, 100, 0]], 0.25, -0.5, 0.125, 0.0),
+}
+
+
+def build_vectors() -> list[dict[str, Any]]:
+    vectors: list[dict[str, Any]] = []
+    for name, (rows, res, x, y, yaw) in CASES.items():
+        msg = grid_msg(rows, res, x, y, yaw)
+        encoded = _encode_costmap(cast("RelayBridgeModule", None), msg)  # module unused
+        assert encoded is not None
+        payload, meta = encoded
+        cells = np.where(msg.grid == -1, 255, msg.grid).astype(np.uint8)
+        vectors.append(
+            {
+                "name": name,
+                "meta": meta,
+                "grid_b64": base64.b64encode(cells.tobytes()).decode(),
+                "payload_b64": base64.b64encode(payload).decode(),
+            }
+        )
+    return vectors
+
+
+def main() -> None:
+    path = find_web_dir() / "shared" / "fixtures" / "costmap_frames.json"
+    path.write_text(json.dumps({"vectors": build_vectors()}, indent=2) + "\n")
+    print(f"wrote {path}")
+
+
+if __name__ == "__main__":
+    main()

dimos/web/relay_bridge/manifest.py

@@ -18,7 +18,7 @@
 from both pytest and deno test). The transport (protocol.py) checks only
 field shapes; this module owns the domain rules: bounded unique ids, positive
 rates, panel/layout references that resolve, and kind-specific panel rules
-(video). Panels and layout are minimal until T7 (the layout is a flat
+(video, map2d). Panels and layout are minimal until T7 (the layout is a flat
 panel-id order, not a tree).
 """
 
@@ -144,6 +144,24 @@ def parse_manifest(data: Any) -> Manifest:
                 raise ManifestError(
                     "invalid_video_panel", f"video panel {panel.id} needs a jpeg.v1 latest channel"
                 )
+        if panel.kind == "map2d":
+            # channels[0] is the costmap; channels[1] (optional) the pose overlay.
+            if len(panel.channels) not in (1, 2):
+                raise ManifestError(
+                    "invalid_map2d_panel",
+                    f"map2d panel {panel.id} must bind one or two channels",
+                )
+            costmap = ch_ids[panel.channels[0]]
+            if costmap.encoding != "costmap.zlib.v1" or costmap.delivery != "latest":
+                raise ManifestError(
+                    "invalid_map2d_panel",
+                    f"map2d panel {panel.id} needs a costmap.zlib.v1 latest channel first",
+                )
+            if len(panel.channels) == 2 and ch_ids[panel.channels[1]].encoding != "pose.json.v1":
+                raise ManifestError(
+                    "invalid_map2d_panel",
+                    f"map2d panel {panel.id} pose channel must be pose.json.v1",
+                )
 
     for panel_id in manifest.layout:
         if panel_id not in panel_ids: