From c8df1912f75a482f2ef4c8e5d42c1cac2f81ed7d Mon Sep 17 00:00:00 2001 From: Paul Nechifor Date: Thu, 30 Jul 2026 08:28:41 +0300 Subject: [PATCH] t4 --- dimos/msgs/nav_msgs/OccupancyGrid.py | 47 +- dimos/msgs/nav_msgs/test_OccupancyGrid.py | 56 ++- dimos/teleop/hosted/map_compress.py | 22 +- dimos/web/relay_bridge/_wt_session.py | 4 +- .../web/relay_bridge/gen_costmap_fixtures.py | 111 +++++ dimos/web/relay_bridge/manifest.py | 20 +- dimos/web/relay_bridge/relay_bridge_module.py | 154 ++++++- .../web/relay_bridge/test_costmap_encoding.py | 111 +++++ .../web/relay_bridge/test_relay_bridge_e2e.py | 152 ++++++- .../relay_bridge/test_relay_bridge_module.py | 248 ++++++++++- dimos/web/relay_bridge/test_wt_session.py | 16 +- dimos/web/relay_bridge/wt_client.py | 20 +- web/README.md | 3 +- web/cockpit/src/panels/MapPanel.module.css | 51 +++ web/cockpit/src/panels/MapPanel.tsx | 256 +++++++++++ web/cockpit/src/panels/mapRenderer.test.ts | 194 ++++++++ web/cockpit/src/panels/mapRenderer.ts | 178 ++++++++ web/cockpit/src/panels/panels.test.tsx | 417 +++++++++++++++++- web/cockpit/src/panels/registry.ts | 2 + web/cockpit/src/session/decoders/costmap.ts | 112 +++++ .../src/session/decoders/decoders.test.ts | 83 +++- web/cockpit/src/session/decoders/index.ts | 4 +- web/cockpit/src/session/session.test.ts | 85 +++- web/cockpit/src/session/session.ts | 7 +- web/shared/fixtures/costmap_frames.json | 49 ++ web/shared/fixtures/gen.ts | 38 ++ web/shared/fixtures/manifests.json | 232 ++++++++++ web/shared/manifest.ts | 29 +- 28 files changed, 2622 insertions(+), 79 deletions(-) create mode 100644 dimos/web/relay_bridge/gen_costmap_fixtures.py create mode 100644 dimos/web/relay_bridge/test_costmap_encoding.py create mode 100644 web/cockpit/src/panels/MapPanel.module.css create mode 100644 web/cockpit/src/panels/MapPanel.tsx create mode 100644 web/cockpit/src/panels/mapRenderer.test.ts create mode 100644 web/cockpit/src/panels/mapRenderer.ts create mode 100644 web/cockpit/src/session/decoders/costmap.ts create mode 100644 web/shared/fixtures/costmap_frames.json diff --git a/dimos/msgs/nav_msgs/OccupancyGrid.py b/dimos/msgs/nav_msgs/OccupancyGrid.py index bad3fc9ab2..9395243bad 100644 --- a/dimos/msgs/nav_msgs/OccupancyGrid.py +++ b/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( diff --git a/dimos/msgs/nav_msgs/test_OccupancyGrid.py b/dimos/msgs/nav_msgs/test_OccupancyGrid.py index 7aae8abfac..98297f6f39 100644 --- a/dimos/msgs/nav_msgs/test_OccupancyGrid.py +++ b/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 diff --git a/dimos/teleop/hosted/map_compress.py b/dimos/teleop/hosted/map_compress.py index b1533bcd0f..0928080aa5 100644 --- a/dimos/teleop/hosted/map_compress.py +++ b/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.""" diff --git a/dimos/web/relay_bridge/_wt_session.py b/dimos/web/relay_bridge/_wt_session.py index ef7c3c8115..00b5d82b76 100644 --- a/dimos/web/relay_bridge/_wt_session.py +++ b/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 diff --git a/dimos/web/relay_bridge/gen_costmap_fixtures.py b/dimos/web/relay_bridge/gen_costmap_fixtures.py new file mode 100644 index 0000000000..9af315592b --- /dev/null +++ b/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() diff --git a/dimos/web/relay_bridge/manifest.py b/dimos/web/relay_bridge/manifest.py index ee10ec76ba..fb9ed49ada 100644 --- a/dimos/web/relay_bridge/manifest.py +++ b/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: diff --git a/dimos/web/relay_bridge/relay_bridge_module.py b/dimos/web/relay_bridge/relay_bridge_module.py index 583d288155..0d3f109b82 100644 --- a/dimos/web/relay_bridge/relay_bridge_module.py +++ b/dimos/web/relay_bridge/relay_bridge_module.py @@ -16,9 +16,12 @@ Registers the robot (id + channel manifest) with a relay - spawned locally in --local-relay mode, or a remote one via relay_url - and forwards robot streams -to it. Encoding is lazy: an input is subscribed, and frames are encoded, only -while the relay reports at least one viewer subscribed to that channel, so a -robot with no open cockpit does no encode work at all. +to it. Encoding is lazy: an input is subscribed for encoding, and frames are +encoded, only while the relay reports at least one viewer subscribed to that +channel, so a robot with no open cockpit does no encode work. Channels with +resend_on_subscribe additionally keep one always-on raw subscription (decode +only, never encode) so the newest message can be replayed the moment a channel +gains its first viewer, even when the producer went quiet before that. Threading: input callbacks fire on the transport (LCM) thread, which gates on maxHz and encodes there (RerunBridge precedent, ~3 ms per JPEG), then hands @@ -39,13 +42,17 @@ import time from typing import Any, TypeVar import webbrowser +import zlib +import numpy as np from pydantic import Field +from reactivex.disposable import Disposable from dimos.core.coordination.blueprints import Blueprint, autoconnect from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid, block_max_reduce from dimos.msgs.sensor_msgs.Image import Image from dimos.utils.logging_config import setup_logger from dimos.web.relay_bridge.locate import find_web_dir @@ -67,7 +74,9 @@ _T = TypeVar("_T") _FrameMeta = dict[str, Any] | None -_Sender = Callable[[bytes, _FrameMeta], None] +# (payload, meta, ts); ts is None for live frames (stamped at send) and the +# source arrival time for replays, so a stale replay is honest about its age. +_Sender = Callable[[bytes, _FrameMeta, float | None], None] _RECONNECT_PAUSE_S = 2.0 @@ -151,6 +160,8 @@ class RelayBridgeConfig(ModuleConfig): # frames into an every-other-frame pattern. image_max_hz: float = Field(default=30.0, gt=0.0) odom_max_hz: float = Field(default=20.0, gt=0.0) + costmap_max_hz: float = Field(default=5.0, gt=0.0) + """Full-grid zlib frames; the go2 mapper publishes at ~7.6 Hz.""" available_channels: tuple[str, ...] | None = None """Composition-provided channel allowlist; None derives from bound inputs.""" @@ -176,16 +187,63 @@ def _encode_odom( return json.dumps(pose, separators=(",", ":")).encode(), None +# The historical costmap encoder's choice (websocket_vis/optimized_costmap.py); +# full grids compress to ~10-30 KB at <= 5 Hz, so speed over ratio is fine. +_COSTMAP_ZLIB_LEVEL = 6 +# Render budget shared with the cockpit decoder (MAX_COSTMAP_DIM in +# costmap.ts): larger grids are block-max downsampled before compression so +# every frame stays within what consumers accept and render. 2048^2 raw is +# 4 MiB, and zlib worst case adds ~0.01%, so the 8 MiB payload caps +# (_wt_session._MAX_PAYLOAD_BYTES and the cockpit's) are unreachable. +_COSTMAP_MAX_SIDE = 2048 + + +def _encode_costmap( + module: RelayBridgeModule, msg: OccupancyGrid +) -> tuple[bytes, dict[str, Any] | None] | None: + grid = msg.grid + if grid.size == 0: + return None # mapper still warming up; nothing to draw + res = msg.resolution + side = max(grid.shape) + if side > _COSTMAP_MAX_SIDE: + factor = -(-side // _COSTMAP_MAX_SIDE) + grid = block_max_reduce(grid, factor) + res *= factor + h, w = grid.shape + # Wire contract (costmap.zlib.v1): uint8 cells, ROS -1 unknown -> 255. + # int8 -1 is byte 0xff and 0..100 are byte-identical, so the raw buffer + # already is the wire payload - no mask/astype/tobytes copies. + cells = np.ascontiguousarray(grid) + origin = msg.origin + meta = { + "w": w, + "h": h, + "res": res, + "origin": [origin.position.x, origin.position.y, origin.yaw], + } + return zlib.compress(cells, _COSTMAP_ZLIB_LEVEL), meta + + @dataclass(frozen=True) class ChannelDef: ch: str encoding: str delivery: Delivery max_hz: Callable[[RelayBridgeConfig], float] - encode: Callable[[RelayBridgeModule, Any], tuple[bytes, _FrameMeta]] + # Returning None skips the frame (an empty grid before mapping starts). + encode: Callable[[RelayBridgeModule, Any], tuple[bytes, _FrameMeta] | None] # Cockpit panel-component kind rendered for this channel (None: raw row # only). One panel per channel until the T7 authoring API. panel_kind: str | None = None + # Extra channel ids appended to the panel binding when advertised (the + # map2d panel's pose overlay); silently dropped when not advertised. + panel_extra_channels: tuple[str, ...] = () + # Keep an always-on raw-input cache (decode only, no encode) and replay + # the newest message when the channel goes from zero viewers to some + # viewer: a new session must not wait for the next publish (the producer + # may have gone quiet, possibly before the first viewer ever attached). + resend_on_subscribe: bool = False def _passes_rate_gate( @@ -216,12 +274,23 @@ class _Session: "color_image", "jpeg.v1", "latest", lambda c: c.image_max_hz, _encode_image, "video" ), ChannelDef("odom", "pose.json.v1", "reliable", lambda c: c.odom_max_hz, _encode_odom), + ChannelDef( + "global_costmap", + "costmap.zlib.v1", + "latest", + lambda c: c.costmap_max_hz, + _encode_costmap, + "map2d", + panel_extra_channels=("odom",), + resend_on_subscribe=True, + ), ) def build_manifest(config: RelayBridgeConfig, channels: tuple[ChannelDef, ...]) -> RobotManifest: # Routed through the domain parser so an invalid channel table (duplicate # ids, bad rates) fails module start instead of poisoning the relay. + live = {cd.ch for cd in channels} manifest = parse_manifest( { "channels": [ @@ -234,7 +303,11 @@ def build_manifest(config: RelayBridgeConfig, channels: tuple[ChannelDef, ...]) for cd in channels ], "panels": [ - {"id": cd.ch, "kind": cd.panel_kind, "channels": [cd.ch]} + { + "id": cd.ch, + "kind": cd.panel_kind, + "channels": [cd.ch, *(c for c in cd.panel_extra_channels if c in live)], + } for cd in channels if cd.panel_kind is not None ], @@ -256,9 +329,11 @@ class RelayBridgeModule(Module): """Bridges robot streams to the relay; encodes only while viewers watch.""" config: RelayBridgeConfig - # Exact producer types (GO2Connection outputs) so autoconnect matches. + # Exact producer types (GO2Connection/CostMapper outputs) so autoconnect + # matches. color_image: In[Image] odom: In[PoseStamped] + global_costmap: In[OccupancyGrid] # NEVER add handle_color_image/handle_odom methods here: _auto_bind_handlers # subscribes any handle_ eagerly at start(), defeating lazy encode. @@ -273,6 +348,12 @@ def __init__(self, **kwargs: Any) -> None: self._channel_defs: tuple[ChannelDef, ...] = () self._min_interval: dict[str, float] = {} self._last_input: dict[str, float] = {} + # Newest raw message (+ arrival wall time, the replay's frame ts) per + # resend_on_subscribe channel; written on the transport thread, read + # on the loop (GIL-atomic dict swap), and kept across sessions so a + # reconnect replays too. Pins the full grid (MBs, one per channel); + # encoding stays lazy. + self._last_msg: dict[str, tuple[Any, float]] = {} self.encoded: dict[str, int] = {cd.ch: 0 for cd in CHANNELS} async def main(self) -> AsyncIterator[None]: @@ -291,6 +372,16 @@ async def main(self) -> AsyncIterator[None]: and self.inputs[cd.ch].transport is not None ) self._min_interval = {cd.ch: 1.0 / cd.max_hz(self.config) for cd in self._channel_defs} + for cd in self._channel_defs: + if cd.resend_on_subscribe: + # Always-on raw cache: grids published before the first + # viewer, or while nobody watches, must still be + # replayable on the next 0->1 subscribe. + self.register_disposable( + Disposable( + self.inputs[cd.ch].subscribe(functools.partial(self._cache_input, cd)) + ) + ) self._manifest = build_manifest(self.config, self._channel_defs) self._url = self.config.relay_url or self.config.g.relay_url if self._url is None: @@ -389,9 +480,14 @@ def _build_senders(self, client: RelayClient) -> dict[str, _Sender]: return senders def _send_reliable( - self, client: RelayClient, ch: str, payload: bytes, meta: dict[str, Any] | None + self, + client: RelayClient, + ch: str, + payload: bytes, + meta: dict[str, Any] | None, + ts: float | None = None, ) -> None: - client.send_frame(ch, payload, delivery="reliable", meta=meta) + client.send_frame(ch, payload, delivery="reliable", meta=meta, ts=ts) async def _supervise(self, session: _Session) -> None: """Consume subs snapshots; on session loss, reconnect (and respawn a @@ -476,6 +572,25 @@ def _reconcile(self, session: _Session, want: set[str]) -> None: active = cd.ch in session.unsubs should = cd.ch in want if should and not active: + cached = self._last_msg.get(cd.ch) + if cached is not None: + # Replay precedes the subscribe: this offer runs + # synchronously on the loop, so a live frame - possible + # only once subscribed - always queues behind it and wins + # the 1-slot mailbox. Fires on 0->1 transitions only: the + # relay reports sub-set changes and stays cache-free, so + # an extra viewer on an already-active channel waits for + # the next publish (review issue 2, deferred). + msg, recv_ts = cached + try: + encoded = cd.encode(self, msg) + except Exception: + logger.exception(f"relay bridge: replaying {cd.ch} failed") + encoded = None + if encoded is not None: + # self.encoded counts live-path encodes only; the + # arrival ts keeps a stale replay honest about its age. + self._offer(session, session.senders[cd.ch], *encoded, recv_ts) session.unsubs[cd.ch] = self.inputs[cd.ch].subscribe( functools.partial(self._on_input, session, cd, session.senders[cd.ch]) ) @@ -497,20 +612,35 @@ def _on_input(self, session: _Session, cd: ChannelDef, sender: _Sender, msg: Any if not _passes_rate_gate(self._last_input, cd.ch, now, self._min_interval[cd.ch]): return try: - payload, meta = cd.encode(self, msg) + encoded = cd.encode(self, msg) except Exception: logger.exception(f"relay bridge: encoding {cd.ch} failed") return + if encoded is None: + return + payload, meta = encoded self.encoded[cd.ch] += 1 loop = self._loop if loop is not None and loop.is_running(): loop.call_soon_threadsafe(self._offer, session, sender, payload, meta) - def _offer(self, session: _Session, sender: _Sender, payload: bytes, meta: _FrameMeta) -> None: + def _cache_input(self, cd: ChannelDef, msg: Any) -> None: + """Transport-thread callback: remember the newest raw message so a + 0->1 subscribe can replay it (its arrival time becomes the frame ts).""" + self._last_msg[cd.ch] = (msg, time.time()) + + def _offer( + self, + session: _Session, + sender: _Sender, + payload: bytes, + meta: _FrameMeta, + ts: float | None = None, + ) -> None: if session.retired.is_set() or self._session is not session: return try: - sender(payload, meta) + sender(payload, meta, ts) except Exception: # Session mid-teardown (dead writer pump / closed connection): the # supervisor is already reconnecting and will rebuild the senders. diff --git a/dimos/web/relay_bridge/test_costmap_encoding.py b/dimos/web/relay_bridge/test_costmap_encoding.py new file mode 100644 index 0000000000..8a33d6587d --- /dev/null +++ b/dimos/web/relay_bridge/test_costmap_encoding.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. + +"""costmap.zlib.v1 golden vectors: the encoder pinned byte-exact. + +The mirror of the cockpit's decoders.test.ts vectors block: pytest re-encodes +each vector, vitest inflates it, so drift on either side fails a suite. +""" + +import base64 +import json +from typing import Any +import zlib + +import numpy as np +import pytest + +from dimos.msgs.geometry_msgs.Pose import Pose +from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid +from dimos.web.relay_bridge.gen_costmap_fixtures import grid_msg +from dimos.web.relay_bridge.locate import find_web_dir +from dimos.web.relay_bridge.relay_bridge_module import _encode_costmap + +with open(find_web_dir() / "shared" / "fixtures" / "costmap_frames.json") as f: + VECTORS: list[dict[str, Any]] = json.load(f)["vectors"] + + +@pytest.mark.parametrize("vec", VECTORS, ids=lambda v: v["name"]) +def test_encoder_reproduces_golden_vector(vec: dict[str, Any]) -> None: + """Compressed-byte equality pins CPython's bundled zlib at level 6. If a + future CPython legitimately shifts those bytes, regenerate the fixture + (gen_costmap_fixtures) - the decoded-grid assertions on both sides are + the actual wire contract.""" + meta = vec["meta"] + cells = np.frombuffer(base64.b64decode(vec["grid_b64"]), dtype=np.uint8) + rows = np.where(cells == 255, -1, cells).astype(np.int8).reshape(meta["h"], meta["w"]) + ox, oy, yaw = meta["origin"] + msg = grid_msg(rows.tolist(), meta["res"], ox, oy, yaw) + encoded = _encode_costmap(None, msg) # the costmap encoder ignores the module + assert encoded is not None + payload, out_meta = encoded + assert payload == base64.b64decode(vec["payload_b64"]) + assert zlib.decompress(payload) == cells.tobytes() + assert (out_meta["w"], out_meta["h"], out_meta["res"]) == (meta["w"], meta["h"], meta["res"]) + assert out_meta["origin"][:2] == meta["origin"][:2] + assert out_meta["origin"][2] == pytest.approx(yaw, abs=1e-12) + + +def test_encoder_handles_wire_decoded_grid() -> None: + """The bridge encodes grids that arrived over LCM; the decoded origin must + still expose .yaw (regression: the raw wire pose did not, and every + global_costmap frame failed to encode).""" + msg = grid_msg([[0, 100], [-1, 50]], 0.05, -1.25, 2.5, 0.5) + decoded = OccupancyGrid.lcm_decode(msg.lcm_encode()) + encoded = _encode_costmap(None, decoded) + assert encoded is not None + payload, meta = encoded + assert meta["origin"] == pytest.approx([-1.25, 2.5, 0.5]) + assert zlib.decompress(payload) == bytes([0, 100, 255, 50]) + + +def test_empty_grid_encodes_to_none() -> None: + assert _encode_costmap(None, OccupancyGrid()) is None + + +def test_oversized_grid_is_downsampled_within_budget() -> None: + rows = np.zeros((4096, 4096), dtype=np.int8) + rows[0, 1] = 100 # lone obstacle: the block max must keep it + rows[2:4, 2:4] = -1 # a fully unknown block stays unknown + msg = OccupancyGrid(grid=rows, resolution=0.05, origin=Pose(1.0, 2.0, 0.0), ts=1.0) + encoded = _encode_costmap(None, msg) + assert encoded is not None + payload, meta = encoded + assert (meta["w"], meta["h"]) == (2048, 2048) + assert meta["res"] == pytest.approx(0.1) + assert meta["origin"][:2] == [1.0, 2.0] + cells = np.frombuffer(zlib.decompress(payload), dtype=np.uint8).reshape(2048, 2048) + assert cells[0, 0] == 100 + assert cells[1, 1] == 255 + assert cells[2, 2] == 0 + + +def test_grid_at_exactly_max_side_is_not_downsampled() -> None: + rows = np.full((4, 2048), 7, dtype=np.int8) + msg = OccupancyGrid(grid=rows, resolution=0.05, origin=Pose(0.0, 0.0, 0.0), ts=1.0) + encoded = _encode_costmap(None, msg) + assert encoded is not None + payload, meta = encoded + assert (meta["w"], meta["h"], meta["res"]) == (2048, 4, 0.05) + assert zlib.decompress(payload) == rows.astype(np.uint8).tobytes() + + +def test_non_square_oversized_grid_uses_ceil_factor() -> None: + rows = np.zeros((100, 4100), dtype=np.int8) + msg = OccupancyGrid(grid=rows, resolution=0.05, origin=Pose(0.0, 0.0, 0.0), ts=1.0) + encoded = _encode_costmap(None, msg) + assert encoded is not None + _, meta = encoded + assert (meta["w"], meta["h"]) == (1366, 33) + assert meta["res"] == pytest.approx(0.15) diff --git a/dimos/web/relay_bridge/test_relay_bridge_e2e.py b/dimos/web/relay_bridge/test_relay_bridge_e2e.py index 860ae00204..31f17fbcd9 100644 --- a/dimos/web/relay_bridge/test_relay_bridge_e2e.py +++ b/dimos/web/relay_bridge/test_relay_bridge_e2e.py @@ -33,12 +33,16 @@ import sys import threading import time +from typing import Any +import zlib import numpy as np import pytest from dimos.core.transport import pLCMTransport +from dimos.msgs.geometry_msgs.Pose import Pose from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid from dimos.msgs.sensor_msgs.Image import Image from dimos.web.relay_bridge.e2e_support import attach_viewer, collect_until, stop_module from dimos.web.relay_bridge.protocol import Unsub @@ -47,6 +51,13 @@ ROBOT_ID = "bridge-e2e" POSE = PoseStamped(ts=42.5, position=[1.5, -2.5, 0.25], orientation=[0.0, 0.0, 0.0, 1.0]) +COSTMAP_GRID = OccupancyGrid( + grid=np.array([[-1, 0, 50], [100, 0, -1]], dtype=np.int8), + resolution=0.05, + origin=Pose(-1.25, 2.5, 0.0), + ts=42.5, +) +COSTMAP_CELLS = bytes([255, 0, 50, 100, 0, 255]) _LISTENER = """ import socket import time @@ -91,24 +102,28 @@ def close(self) -> None: self.image.stop() -def _start_bridge() -> tuple[RelayBridgeModule, pLCMTransport, pLCMTransport]: +def _start_bridge() -> tuple[RelayBridgeModule, tuple[pLCMTransport, ...]]: # cockpit_build=False: tests must never trigger the npm-downloading build. module = RelayBridgeModule( local_port=0, open_browser=False, cockpit_build=False, robot_id=ROBOT_ID ) - odom_tr = pLCMTransport("/rb_e2e/odom") - image_tr = pLCMTransport("/rb_e2e/color_image") - odom_tr.start() - image_tr.start() - module.odom.transport = odom_tr - module.color_image.transport = image_tr + transports = ( + pLCMTransport("/rb_e2e/odom"), + pLCMTransport("/rb_e2e/color_image"), + pLCMTransport("/rb_e2e/global_costmap"), + ) + for transport in transports: + transport.start() + module.odom.transport = transports[0] + module.color_image.transport = transports[1] + module.global_costmap.transport = transports[2] module.start() # spawns the Deno relay, connects, registers - return module, odom_tr, image_tr + return module, transports @pytest.fixture(scope="module") def bridge() -> Iterator[RelayBridgeModule]: - module, _, _ = _start_bridge() + module, _ = _start_bridge() try: yield module finally: @@ -124,13 +139,13 @@ def respawn_bridge() -> Iterator[RelayBridgeModule]: conftest thread-leak check. Owning the bridge scopes those threads to the test, with everything reaped here. """ - module, odom_tr, image_tr = _start_bridge() + module, transports = _start_bridge() try: yield module finally: stop_module(module) - odom_tr.stop() - image_tr.stop() + for transport in transports: + transport.stop() @pytest.fixture(scope="module") @@ -219,6 +234,119 @@ async def flow() -> None: asyncio.run(flow()) +class _CostmapPublisher: + """Stoppable costmap publisher: the resend tests must prove a frame + arrives with no producer running, so it cannot ride the always-on + _Publisher.""" + + def __init__(self, grid: OccupancyGrid = COSTMAP_GRID) -> None: + self.grid = grid + self.transport = pLCMTransport("/rb_e2e/global_costmap") + self.stop = threading.Event() + self._thread = threading.Thread(target=self._run, daemon=True) + + def start(self) -> None: + self.transport.start() + self._thread.start() + + def _run(self) -> None: + while not self.stop.is_set(): + self.transport.publish(self.grid) + time.sleep(0.05) + + def close(self) -> None: + self.stop.set() + self._thread.join(timeout=2) + self.transport.stop() + + +async def _watch_costmap(bridge: RelayBridgeModule, expected_cells: bytes = COSTMAP_CELLS) -> Any: + """Attach a fresh viewer, wait for one costmap frame, return that frame.""" + assert bridge._url is not None + async with await RelayClient.connect(bridge._url, "viewer") as viewer: + await viewer.hello() + await attach_viewer(viewer, ROBOT_ID, ["global_costmap"]) + frames = await collect_until( + viewer, + lambda fs: any(f.header.ch == "global_costmap" for f in fs), + timeout=15.0, + ) + frame = next(f for f in frames if f.header.ch == "global_costmap") + assert frame.header.delivery == "latest" + meta = frame.header.meta + assert meta is not None + assert (meta["w"], meta["h"]) == (3, 2) + # The LCM leg narrows resolution to float32; exact 0.05 is not owed. + assert meta["res"] == pytest.approx(0.05) + assert meta["origin"][:2] == [-1.25, 2.5] + assert meta["origin"][2] == pytest.approx(0.0, abs=1e-12) + assert zlib.decompress(bytes(frame.payload)) == expected_cells + return frame + + +def _wait_costmap_unsubscribed(bridge: RelayBridgeModule) -> None: + """Wait until the relay reported the shrunken sub set and encoding stopped.""" + deadline = time.monotonic() + 10 + while ( + bridge._session is not None + and "global_costmap" in bridge._session.unsubs + and time.monotonic() < deadline + ): + time.sleep(0.05) + assert bridge._session is not None + assert "global_costmap" not in bridge._session.unsubs, "bridge never heard the unsub" + + +def test_costmap_full_grid_arrives_and_resends_on_subscribe(bridge: RelayBridgeModule) -> None: + publisher = _CostmapPublisher() + publisher.start() + try: + first = asyncio.run(_watch_costmap(bridge)) + finally: + publisher.close() + + _wait_costmap_unsubscribed(bridge) + + # A fresh subscription with the producer stopped: the bridge must replay + # the cached message instead of waiting for a publish that never comes. + encoded_before = bridge.encoded["global_costmap"] + second = asyncio.run(_watch_costmap(bridge)) + assert bytes(second.payload) == bytes(first.payload) + # Re-encoded from the raw cache; the counter tracks live encodes only. + assert bridge.encoded["global_costmap"] == encoded_before + + +def test_costmap_replay_reflects_publishes_while_unwatched(bridge: RelayBridgeModule) -> None: + # A different grid published with zero viewers must land in the raw cache, + # so the next subscriber gets it - stamped with its arrival time (honest + # staleness on the wire), not the replay time. + _wait_costmap_unsubscribed(bridge) + grid_b = OccupancyGrid( + grid=np.array([[100, 100, 100], [0, 0, -1]], dtype=np.int8), + resolution=0.05, + origin=Pose(-1.25, 2.5, 0.0), + ts=43.0, + ) + t0 = time.time() + publisher = _CostmapPublisher(grid_b) + publisher.start() + try: + deadline = time.monotonic() + 10 + while time.monotonic() < deadline: + cached = bridge._last_msg.get("global_costmap") + if cached is not None and cached[0].grid[0, 0] == 100: + break + time.sleep(0.05) + finally: + publisher.close() + t1 = time.time() + cached = bridge._last_msg.get("global_costmap") + assert cached is not None and cached[0].grid[0, 0] == 100, "cache never saw grid B" + + frame = asyncio.run(_watch_costmap(bridge, expected_cells=bytes([100, 100, 100, 0, 0, 255]))) + assert t0 <= frame.header.ts <= t1 + + def test_relay_child_death_respawns_and_recovers( respawn_bridge: RelayBridgeModule, publisher: _Publisher ) -> None: diff --git a/dimos/web/relay_bridge/test_relay_bridge_module.py b/dimos/web/relay_bridge/test_relay_bridge_module.py index 1c16ed4211..064d7903d7 100644 --- a/dimos/web/relay_bridge/test_relay_bridge_module.py +++ b/dimos/web/relay_bridge/test_relay_bridge_module.py @@ -27,9 +27,12 @@ import json from pathlib import Path import socket +import subprocess +import sys import threading import time from typing import Any +import zlib import numpy as np from pydantic import ValidationError @@ -38,7 +41,9 @@ from dimos.core.coordination.blueprints import autoconnect from dimos.core.module import Module, ModuleConfig from dimos.core.stream import Out +from dimos.msgs.geometry_msgs.Pose import Pose from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid from dimos.msgs.sensor_msgs.Image import Image from dimos.simulation.mujoco.constants import VIDEO_FPS from dimos.web.relay_bridge import relay_bridge_module @@ -58,9 +63,15 @@ class FakeWriter: def __init__(self) -> None: self.offers: list[tuple[bytes, dict[str, Any] | None]] = [] + # ts kept apart so offer-list equality asserts ignore it (a replay + # carries its source arrival time, a live frame None). + self.tss: list[float | None] = [] - def offer(self, payload: bytes, meta: dict[str, Any] | None = None) -> None: + def offer( + self, payload: bytes, meta: dict[str, Any] | None = None, ts: float | None = None + ) -> None: self.offers.append((payload, meta)) + self.tss.append(ts) class FakeClient: @@ -244,16 +255,25 @@ def odom_transport(module: RelayBridgeModule) -> FakeTransport: return transport +def costmap_transport(module: RelayBridgeModule) -> FakeTransport: + transport = module.global_costmap.transport + assert isinstance(transport, FakeTransport) + return transport + + def test_manifest_and_robot_info_content() -> None: config = RelayBridgeConfig(robot_id="go2-lab", robot_name="Lab", image_max_hz=12.0) manifest = build_manifest(config, CHANNELS) - assert [c.ch for c in manifest.channels] == ["color_image", "odom"] - image, odom = manifest.channels + assert [c.ch for c in manifest.channels] == ["color_image", "odom", "global_costmap"] + image, odom, costmap = manifest.channels assert (image.encoding, image.delivery, image.maxHz) == ("jpeg.v1", "latest", 12.0) assert (odom.encoding, odom.delivery, odom.maxHz) == ("pose.json.v1", "reliable", 20.0) - # One video panel for the camera; odom stays a raw channel row. + assert (costmap.encoding, costmap.delivery, costmap.maxHz) == ("costmap.zlib.v1", "latest", 5.0) + # A video panel for the camera and a map2d panel binding costmap + pose; + # odom additionally stays a raw channel row. assert [(p.id, p.kind, p.channels) for p in manifest.panels] == [ - ("color_image", "video", ["color_image"]) + ("color_image", "video", ["color_image"]), + ("global_costmap", "map2d", ["global_costmap", "odom"]), ] info = resolve_robot_info(config) @@ -271,6 +291,8 @@ def test_manifest_and_robot_info_content() -> None: ("image_max_hz", -1.0), ("odom_max_hz", 0.0), ("odom_max_hz", -1.0), + ("costmap_max_hz", 0.0), + ("costmap_max_hz", -1.0), ("jpeg_quality", -1), ("jpeg_quality", 101), ], @@ -396,6 +418,208 @@ def spy(self: Image, quality: int = 75) -> bytes: assert calls["n"] == 1 +# Covers every value class of the wire contract: -1 unknown -> 255, 0 free, +# graded cost, 100 lethal. +COSTMAP_GRID = OccupancyGrid( + grid=np.array([[-1, 0, 50], [100, 0, -1]], dtype=np.int8), + resolution=0.05, + origin=Pose(-1.25, 2.5, 0.0), + ts=42.5, +) +COSTMAP_CELLS = bytes([255, 0, 50, 100, 0, 255]) + + +@pytest.fixture +def costmap_bridge(monkeypatch): + module, clients = _make_bridge(monkeypatch, wire=("color_image", "odom", "global_costmap")) + try: + yield module, clients + finally: + stop_module(module) + + +def test_costmap_encode_roundtrip_and_meta(costmap_bridge) -> None: + module, clients = costmap_bridge + client = clients[0] + push(module, client, Subs(chs=["global_costmap"], n=1)) + # 2 subscribers = the always-on raw cache + the viewer-driven encoder. + assert wait_until(lambda: len(costmap_transport(module).subscribers) == 2) + + costmap_transport(module).publish(COSTMAP_GRID) + assert module.encoded["global_costmap"] == 1 + assert wait_until(lambda: client.writers["global_costmap"].offers) + payload, meta = client.writers["global_costmap"].offers[0] + assert zlib.decompress(payload) == COSTMAP_CELLS + assert meta is not None + assert (meta["w"], meta["h"], meta["res"]) == (3, 2, 0.05) + assert meta["origin"][:2] == [-1.25, 2.5] + assert meta["origin"][2] == pytest.approx(0.0, abs=1e-12) + + +def test_costmap_empty_grid_is_skipped(costmap_bridge) -> None: + module, clients = costmap_bridge + push(module, clients[0], Subs(chs=["global_costmap"], n=1)) + assert wait_until(lambda: len(costmap_transport(module).subscribers) == 2) + + costmap_transport(module).publish(OccupancyGrid()) + flush_loop(module) + assert module.encoded["global_costmap"] == 0 + assert clients[0].writers["global_costmap"].offers == [] + + +def test_no_costmap_encode_while_unsubscribed(costmap_bridge, monkeypatch) -> None: + # The ticket-mandated spy, mirroring the jpeg one: compression must not + # happen without viewers, independent of the module.encoded bookkeeping. + module, clients = costmap_bridge + calls = {"n": 0} + real = zlib.compress + + def spy(data: Any, level: int = -1) -> bytes: + calls["n"] += 1 + return real(data, level) + + monkeypatch.setattr(relay_bridge_module.zlib, "compress", spy) + costmap_transport(module).publish(COSTMAP_GRID) + costmap_transport(module).publish(COSTMAP_GRID) + flush_loop(module) + assert calls["n"] == 0 + assert module.encoded["global_costmap"] == 0 + + push(module, clients[0], Subs(chs=["global_costmap"], n=1)) + assert wait_until(lambda: len(costmap_transport(module).subscribers) == 2) + costmap_transport(module).publish(COSTMAP_GRID) + # Two compresses: the subscribe replayed the cached grid, then the live + # frame encoded. + assert calls["n"] == 2 + assert module.encoded["global_costmap"] == 1 + + +def test_costmap_resent_on_resubscribe(costmap_bridge) -> None: + # A channel going 0 -> 1 viewers replays the cached frame: a fresh + # subscription must not wait for the next publish (the producer may have + # gone quiet). + module, clients = costmap_bridge + client = clients[0] + push(module, client, Subs(chs=["global_costmap"], n=1)) + assert wait_until(lambda: len(costmap_transport(module).subscribers) == 2) + costmap_transport(module).publish(COSTMAP_GRID) + assert wait_until(lambda: len(client.writers["global_costmap"].offers) == 1) + + push(module, client, Subs(chs=[], n=2)) + assert wait_until(lambda: len(costmap_transport(module).subscribers) == 1) + + push(module, client, Subs(chs=["global_costmap"], n=3)) + assert wait_until(lambda: len(client.writers["global_costmap"].offers) == 2) + # Re-encoded from the raw cache; the counter tracks live encodes only. + assert module.encoded["global_costmap"] == 1 + first, second = client.writers["global_costmap"].offers + assert first == second + + +def test_costmap_cache_survives_reconnect(costmap_bridge) -> None: + module, clients = costmap_bridge + push(module, clients[0], Subs(chs=["global_costmap"], n=1)) + assert wait_until(lambda: len(costmap_transport(module).subscribers) == 2) + costmap_transport(module).publish(COSTMAP_GRID) + assert wait_until(lambda: clients[0].writers["global_costmap"].offers) + + kill_session(module, clients[0]) + assert wait_until(lambda: len(clients) == 2) + push(module, clients[1], Subs(chs=["global_costmap"], n=1)) + assert wait_until(lambda: clients[1].writers["global_costmap"].offers) + assert ( + clients[1].writers["global_costmap"].offers == clients[0].writers["global_costmap"].offers + ) + assert module.encoded["global_costmap"] == 1 + + +def test_costmap_cold_start_replay_on_first_subscribe(costmap_bridge) -> None: + # A grid published before any viewer exists must reach the first + # subscriber: the raw cache is always on, not tied to viewer state. + module, clients = costmap_bridge + client = clients[0] + t0 = time.time() + costmap_transport(module).publish(COSTMAP_GRID) + t1 = time.time() + assert module.encoded["global_costmap"] == 0 + + push(module, client, Subs(chs=["global_costmap"], n=1)) + assert wait_until(lambda: client.writers["global_costmap"].offers) + payload, meta = client.writers["global_costmap"].offers[0] + assert zlib.decompress(payload) == COSTMAP_CELLS + assert meta is not None and (meta["w"], meta["h"]) == (3, 2) + assert module.encoded["global_costmap"] == 0 # a replay is not a live encode + ts = client.writers["global_costmap"].tss[0] + assert ts is not None and t0 <= ts <= t1 # arrival time, not replay time + + +def test_costmap_replay_uses_message_published_while_unsubscribed(costmap_bridge) -> None: + # Cache map A with a viewer, drop to zero viewers, publish map B: the + # next subscriber must get B, not a stale A. + module, clients = costmap_bridge + client = clients[0] + push(module, client, Subs(chs=["global_costmap"], n=1)) + assert wait_until(lambda: len(costmap_transport(module).subscribers) == 2) + costmap_transport(module).publish(COSTMAP_GRID) + assert wait_until(lambda: len(client.writers["global_costmap"].offers) == 1) + + push(module, client, Subs(chs=[], n=2)) + assert wait_until(lambda: len(costmap_transport(module).subscribers) == 1) + grid_b = OccupancyGrid( + grid=np.array([[100, 100, 100], [0, 0, 0]], dtype=np.int8), + resolution=0.05, + origin=Pose(-1.25, 2.5, 0.0), + ts=43.0, + ) + costmap_transport(module).publish(grid_b) + + push(module, client, Subs(chs=["global_costmap"], n=3)) + assert wait_until(lambda: len(client.writers["global_costmap"].offers) == 2) + payload, _ = client.writers["global_costmap"].offers[1] + assert zlib.decompress(payload) == bytes([100, 100, 100, 0, 0, 0]) + assert module.encoded["global_costmap"] == 1 # only A's live encode + + +def test_costmap_empty_cached_grid_is_not_replayed(costmap_bridge) -> None: + module, clients = costmap_bridge + client = clients[0] + costmap_transport(module).publish(OccupancyGrid()) + push(module, client, Subs(chs=["global_costmap"], n=1)) + assert wait_until(lambda: len(costmap_transport(module).subscribers) == 2) + flush_loop(module) + assert client.writers["global_costmap"].offers == [] + + +def test_stop_disposes_costmap_cache_subscription(monkeypatch) -> None: + module, _clients = _make_bridge(monkeypatch, wire=("color_image", "odom", "global_costmap")) + transport = costmap_transport(module) + assert len(transport.subscribers) == 1 # the always-on raw cache + stop_module(module) + assert transport.unsubscribed == 1 + + +def test_bridge_import_does_not_pull_matplotlib() -> None: + # OccupancyGrid's visualization imports are lazy so the bridge does not + # cost every relay worker matplotlib's ~0.5 s / ~28 MiB (review issue 9). + code = ( + "import sys; import dimos.web.relay_bridge.relay_bridge_module; " + "assert 'matplotlib' not in sys.modules" + ) + subprocess.run([sys.executable, "-c", code], check=True) + + +def test_manifest_omits_pose_binding_when_odom_unwired(monkeypatch) -> None: + module, clients = _make_bridge(monkeypatch, wire=("color_image", "global_costmap")) + try: + _, manifest = clients[0].hello_args + assert isinstance(manifest, RobotManifest) + assert [c.ch for c in manifest.channels] == ["color_image", "global_costmap"] + map_panel = next(p for p in manifest.panels if p.kind == "map2d") + assert map_panel.channels == ["global_costmap"] + finally: + stop_module(module) + + def test_session_loss_stops_encoders_and_reconnects(bridge) -> None: module, clients = bridge push(module, clients[0], Subs(chs=["odom"], n=5)) @@ -779,6 +1003,11 @@ class _ImageProducer(Module): color_image: Out[Image] +class _CostmapProducer(Module): + config: _EmptyConfig + global_costmap: Out[OccupancyGrid] + + class _BareModule(Module): config: _EmptyConfig @@ -791,6 +1020,15 @@ def test_composition_adds_relay_to_non_visual_blueprint() -> None: assert relay_atoms[0].kwargs["available_channels"] == ("color_image",) +def test_composition_includes_costmap_producer() -> None: + blueprint = with_relay_bridge( + autoconnect(_ImageProducer.blueprint(), _CostmapProducer.blueprint()) + ) + relay_atom = next(atom for atom in blueprint.blueprints if atom.module is RelayBridgeModule) + + assert relay_atom.kwargs["available_channels"] == ("color_image", "global_costmap") + + def test_composition_ignores_disabled_producers() -> None: source = _ImageProducer.blueprint().disabled_modules(_ImageProducer) blueprint = with_relay_bridge(source) diff --git a/dimos/web/relay_bridge/test_wt_session.py b/dimos/web/relay_bridge/test_wt_session.py index d9b65bddb6..5135ea1ddb 100644 --- a/dimos/web/relay_bridge/test_wt_session.py +++ b/dimos/web/relay_bridge/test_wt_session.py @@ -94,6 +94,9 @@ async def test_per_encoding_payload_caps(): channels=[ ChannelSpec(ch="odom", encoding="pose.json.v1", delivery="reliable", maxHz=20.5), ChannelSpec(ch="cam", encoding="jpeg.v1", delivery="latest", maxHz=15.5), + ChannelSpec( + ch="global_costmap", encoding="costmap.zlib.v1", delivery="latest", maxHz=5.5 + ), ], ) ) @@ -104,14 +107,19 @@ async def test_per_encoding_payload_caps(): session._stream_data_received(4, _frame_bytes("odom", 1, 128 * 1024), False) assert session.frames.qsize() == 0 assert session.frames_oversized == 1 + # Over the costmap cap: dropped too. + session._stream_data_received(8, _frame_bytes("global_costmap", 1, 9 * 1024 * 1024), False) + assert session.frames.qsize() == 0 + assert session.frames_oversized == 2 # Under the caps: queued. session._stream_data_received(4, _frame_bytes("odom", 2, 100), False) session._stream_data_received(8, _frame_bytes("cam", 1, 1024 * 1024), False) - assert session.frames.qsize() == 2 - # Channels with no known encoding only get the outer frame-size bound. - session._stream_data_received(12, _frame_bytes("mystery", 1, 9 * 1024 * 1024), True) + session._stream_data_received(12, _frame_bytes("global_costmap", 2, 30 * 1024), False) assert session.frames.qsize() == 3 - assert session.frames_oversized == 1 + # Channels with no known encoding only get the outer frame-size bound. + session._stream_data_received(16, _frame_bytes("mystery", 1, 9 * 1024 * 1024), True) + assert session.frames.qsize() == 4 + assert session.frames_oversized == 2 async def test_stream_reset_drops_the_frame_reader(): diff --git a/dimos/web/relay_bridge/wt_client.py b/dimos/web/relay_bridge/wt_client.py index 5f0b4eb056..4d7ddace99 100644 --- a/dimos/web/relay_bridge/wt_client.py +++ b/dimos/web/relay_bridge/wt_client.py @@ -325,7 +325,9 @@ def __init__(self, client: RelayClient, ch: str, *, stale_after: float) -> None: self.dropped = 0 self.sent = 0 self.resets = 0 - self._mailbox: asyncio.Queue[tuple[bytes, dict[str, Any] | None]] = asyncio.Queue(maxsize=1) + self._mailbox: asyncio.Queue[tuple[bytes, dict[str, Any] | None, float | None]] = ( + asyncio.Queue(maxsize=1) + ) self._task = asyncio.create_task(self._pump()) self._task.add_done_callback(self._on_pump_done) @@ -338,10 +340,14 @@ def _on_pump_done(self, task: asyncio.Future[None]) -> None: if exc is not None: logger.error(f"latest-wins writer for {self.ch} died", exc_info=exc) - def offer(self, payload: bytes, meta: dict[str, Any] | None = None) -> None: + def offer( + self, payload: bytes, meta: dict[str, Any] | None = None, ts: float | None = None + ) -> None: """Queue the newest frame, dropping any not-yet-sent predecessor. - Event-loop only; producers on other threads must go through + `ts` overrides the frame-header timestamp (a replayed frame carries + its source time); None stamps send time. Event-loop only; producers + on other threads must go through `loop.call_soon_threadsafe(writer.offer, ...)`. Raises RuntimeError if the pump is no longer running (session closed, stopped, or died) so a dead channel is visible at the producer instead of silently dropping. @@ -351,7 +357,7 @@ def offer(self, payload: bytes, meta: dict[str, Any] | None = None) -> None: if self._mailbox.full(): self._mailbox.get_nowait() self.dropped += 1 - self._mailbox.put_nowait((payload, meta)) + self._mailbox.put_nowait((payload, meta, ts)) def stop(self) -> None: self._task.cancel() @@ -366,12 +372,14 @@ async def _pump(self) -> None: await asyncio.wait({get, closed}, return_when=asyncio.FIRST_COMPLETED) if not get.done(): break # session closed with an empty mailbox - payload, meta = get.result() + payload, meta, ts = get.result() finally: get.cancel() if session.closed.is_set(): break - stream_id = self._client.send_frame(self.ch, payload, delivery="latest", meta=meta) + stream_id = self._client.send_frame( + self.ch, payload, delivery="latest", meta=meta, ts=ts + ) self.sent += 1 started = time.monotonic() while session.stream_in_flight(stream_id): diff --git a/web/README.md b/web/README.md index 18e94bea55..6be1400f63 100644 --- a/web/README.md +++ b/web/README.md @@ -48,7 +48,8 @@ WebTransport stacks differ; see bug 11). The CI `web` job runs it; locally it ne The framing is defined once in `shared/protocol.ts`, mirrored in Python, and pinned by golden vectors in `shared/fixtures/` (regenerate via `deno run --allow-write=shared/fixtures shared/fixtures/gen.ts`; tested from both `deno test` and -pytest). +pytest). The one exception is `costmap_frames.json`: its payloads pin the Python encoder's zlib +bytes, so it is generated by `uv run python -m dimos.web.relay_bridge.gen_costmap_fixtures`. Several choices are workarounds for upstream bugs, verified 2026-07-10..15 on Deno 2.6.10 + aioquic 1.3 (details and probes in the spike branch `paul/experiment/webtransport`): diff --git a/web/cockpit/src/panels/MapPanel.module.css b/web/cockpit/src/panels/MapPanel.module.css new file mode 100644 index 0000000000..bd85f1ff7d --- /dev/null +++ b/web/cockpit/src/panels/MapPanel.module.css @@ -0,0 +1,51 @@ +.panel { + border: 1px solid #30363d; + border-radius: 6px; + display: flex; + flex-direction: column; + overflow: hidden; +} + +.head { + align-items: center; + background: #1b1f24; + display: flex; + gap: 0.5rem; + justify-content: space-between; + padding: 0.3rem 0.6rem; +} + +.title { + font-weight: 600; +} + +.badge, +.badgeStale { + color: #8b949e; + font-variant-numeric: tabular-nums; +} + +.badgeStale { + color: #f85149; +} + +.body { + align-items: center; + background: #000; + display: flex; + justify-content: center; + min-height: 120px; + position: relative; +} + +.canvas { + aspect-ratio: 4 / 3; + display: block; + max-height: 70vh; + width: 100%; +} + +.waiting { + color: #8b949e; + position: absolute; +} diff --git a/web/cockpit/src/panels/MapPanel.tsx b/web/cockpit/src/panels/MapPanel.tsx new file mode 100644 index 0000000000..87efc3fc6b --- /dev/null +++ b/web/cockpit/src/panels/MapPanel.tsx @@ -0,0 +1,256 @@ +// Live 2D costmap: canvas drawing driven by the store's direct-subscribe path +// (React is not involved at grid or pose rate; the badge rides the 500 ms UI +// tick). channels[0] is the costmap, channels[1] (optional) the pose overlay +// - both bindings come from the manifest, never hardcoded stream names. + +import { useEffect, useRef } from "react"; +import { type CostmapValue, inflateCostmap } from "../session/decoders/costmap.ts"; +import { useChannel } from "../session/hooks.ts"; +import type { ChannelStore } from "../session/store.ts"; +import styles from "./MapPanel.module.css"; +import { + drawPose, + fitTransform, + gridBlit, + type GridPlacement, + gridToImageData, + type Pose2d, +} from "./mapRenderer.ts"; +import type { PanelProps } from "./registry.ts"; +import type { DrawHealth } from "./VideoPanel.tsx"; + +// The costmap ticks at ~5 Hz; staleness only trips on real silence (mapper +// down, replay ended). New sessions never start stale: the bridge replays +// the last grid on subscribe. +export const MAP_STALE_MS = 5000; + +/** Test seams; real inflate is DecompressionStream, real resize an observer. */ +export interface MapSinkDeps { + inflate?: (value: CostmapValue) => Promise; + hidden?: () => boolean; + /** Calls back on element size changes; returns the disposer. */ + observeResize?: (el: Element, cb: () => void) => () => void; +} + +function isCostmapValue(v: unknown): v is CostmapValue { + return typeof v === "object" && v !== null && + (v as CostmapValue).bytes instanceof Uint8Array && + typeof (v as CostmapValue).w === "number"; +} + +function readPose(v: unknown): Pose2d | null { + if (typeof v !== "object" || v === null) return null; + const { x, y, yaw } = v as Record; + if (typeof x !== "number" || typeof y !== "number" || typeof yaw !== "number") return null; + return { x, y, yaw }; +} + +/** + * Drive `canvas` from the costmap channel's slot: at most one inflate in + * flight, and on completion the pump re-checks the slot, so a burst of grids + * costs one inflate of the newest (latest-wins, same shedding rule as + * everywhere else in the pipeline). The inflated grid lands on an offscreen + * bitmap at cell resolution; the display canvas redraws (scaled blit + pose + * triangle) on new grids, pose ingests, resizes, and visibility changes, + * which is how two consumers share the odom channel without extra + * subscriptions upstream. While the document is hidden nothing inflates or + * draws. Returns the cleanup function. + */ +export function startMapSink( + store: ChannelStore, + costmapCh: string, + poseCh: string | undefined, + canvas: HTMLCanvasElement, + health: DrawHealth, + deps: MapSinkDeps = {}, +): () => void { + const inflate = deps.inflate ?? inflateCostmap; + const hidden = deps.hidden ?? (() => document.hidden); + const observeResize = deps.observeResize ?? ((el, cb) => { + const observer = new ResizeObserver(cb); + observer.observe(el); + return () => observer.disconnect(); + }); + const ctx = canvas.getContext("2d"); + // Grid bitmap at native cell resolution, rewritten only on new grids; the + // ImageData buffer is reused while the grid dimensions hold. + const grid = document.createElement("canvas"); + const gridCtx = grid.getContext("2d"); + let imageData: ImageData | undefined; + let place: GridPlacement | null = null; + let inflating = false; + let drawnVersion = -1; + let stopped = false; + // A fresh mount is never instantly "stalled". + health.lastDrawOkAtMs = Date.now(); + health.failures = 0; + + const draw = (): void => { + if (stopped || hidden() || ctx === null || place === null) return; + const cssW = canvas.clientWidth; + const cssH = canvas.clientHeight; + if (cssW === 0 || cssH === 0) return; + const dpr = globalThis.devicePixelRatio || 1; + const w = Math.round(cssW * dpr); + const h = Math.round(cssH * dpr); + if (canvas.width !== w || canvas.height !== h) { + canvas.width = w; + canvas.height = h; + } + ctx.clearRect(0, 0, w, h); + const t = fitTransform(place, w, h); + ctx.imageSmoothingEnabled = false; // crisp cells when zoomed in + const { ax, ay, rot, dw, dh } = gridBlit(t, place); + ctx.save(); + ctx.translate(ax, ay); + ctx.rotate(rot); + ctx.drawImage(grid, 0, -dh, dw, dh); + ctx.restore(); + const pose = poseCh === undefined ? null : readPose(store.get(poseCh)?.value); + if (pose !== null) drawPose(ctx, t, pose, dpr); + }; + + const pump = (): void => { + if (stopped || inflating || hidden()) return; + const slot = store.get(costmapCh); + if (slot === null || slot.version === drawnVersion) return; + if (!isCostmapValue(slot.value)) return; // undecoded channel: nothing to draw + const value = slot.value; + const version = slot.version; + inflating = true; + inflate(value) + .then((cells) => { + if (stopped) return; + if (grid.width !== value.w || grid.height !== value.h) { + // Assigning a canvas dimension resets its backing store even when + // the value is unchanged, so only touch it on real changes. + grid.width = value.w; + grid.height = value.h; + } + imageData = gridToImageData(cells, value.w, value.h, imageData); + gridCtx?.putImageData(imageData, 0, 0); + place = { w: value.w, h: value.h, res: value.res, origin: value.origin }; + draw(); + health.lastDrawOkAtMs = Date.now(); + health.failures = 0; + }) + .catch(() => { + // Inflate rejection or draw throw: skip this grid but count it, so + // the badge can surface a pipeline that never draws. + health.failures += 1; + }) + .finally(() => { + drawnVersion = version; + inflating = false; + pump(); // newer grids may have landed during the inflate + }); + }; + + const unsubscribeGrid = store.subscribe(costmapCh, pump); + // Pose redraws reuse the cached grid bitmap: no inflate at odom rate. + const unsubscribePose = poseCh === undefined ? null : store.subscribe(poseCh, draw); + const disposeResize = observeResize(canvas, draw); + const onVisibility = (): void => { + pump(); + draw(); // a resize while hidden must repaint even without a new grid + }; + document.addEventListener("visibilitychange", onVisibility); + pump(); // a slot may predate the mount + return () => { + stopped = true; + unsubscribeGrid(); + unsubscribePose?.(); + disposeResize(); + document.removeEventListener("visibilitychange", onVisibility); + }; +} + +function Badge({ store, ch, health }: { store: ChannelStore; ch: string; health: DrawHealth }) { + // Same badge shape as VideoPanel's, per-panel copies until the shared + // chrome arrives with T7; `health` is mutated by the sink at draw rate and + // simply sampled here on the 500 ms UI tick (intended coupling). + const { stats } = useChannel(store, ch); + let text: string; + let error = false; + let stale = false; + if (stats.frames === 0) { + // Nothing ever arrived; a corrupt first frame is an error, not "waiting". + text = "waiting"; + } else if (stats.decodeFailing || health.failures > 0) { + text = "decode failing"; + error = true; + } else if (stats.ageMs !== null && stats.ageMs > MAP_STALE_MS) { + text = `stale ${(stats.ageMs / 1000).toFixed(1)} s`; + stale = true; + } else if (stats.lastFrameAtMs - health.lastDrawOkAtMs > MAP_STALE_MS) { + // Grids arrive but nothing draws; both operands are browser milliseconds. + text = "stalled"; + stale = true; + } else { + text = `${stats.hz.toFixed(1)} Hz`; + } + return ( + + {text} + + ); +} + +export function MapPanel({ spec, store }: PanelProps) { + const costmapCh = spec.channels[0] as string | undefined; + if (costmapCh === undefined) { + // A map panel without a costmap channel is a bridge authoring mistake; + // render it visibly instead of crashing the grid. + return
map2d panel {spec.id}: no channel bound
; + } + return ( + + ); +} + +function MapCanvas( + { spec, store, costmapCh, poseCh }: PanelProps & { + costmapCh: string; + poseCh: string | undefined; + }, +) { + const canvasRef = useRef(null); + const health = useRef({ lastDrawOkAtMs: Date.now(), failures: 0 }).current; + const { slot } = useChannel(store, costmapCh); + + useEffect(() => { + const canvas = canvasRef.current; + if (canvas === null) return; + return startMapSink(store, costmapCh, poseCh, canvas, health); + }, [store, costmapCh, poseCh, health]); + + return ( +
+
+ {spec.id} + +
+
+ + {slot === null && waiting for data...} +
+
+ ); +} diff --git a/web/cockpit/src/panels/mapRenderer.test.ts b/web/cockpit/src/panels/mapRenderer.test.ts new file mode 100644 index 0000000000..923f5a3866 --- /dev/null +++ b/web/cockpit/src/panels/mapRenderer.test.ts @@ -0,0 +1,194 @@ +import { describe, expect, it } from "vitest"; +import { + canvasToWorld, + fitTransform, + gridBlit, + gridToImageData, + OCCUPANCY_PALETTE, + posePath, + worldToCanvas, +} from "./mapRenderer.ts"; + +function rgba(imageData: ImageData, x: number, y: number): number[] { + const i = (y * imageData.width + x) * 4; + return [...imageData.data.slice(i, i + 4)]; +} + +describe("occupancy palette", () => { + it("maps the wire value classes to the operator scheme", () => { + const at = (i: number) => [...OCCUPANCY_PALETTE.slice(i * 4, i * 4 + 4)]; + expect(at(0)).toEqual([0x1e, 0x3a, 0x44, 255]); // free + expect(at(1)).toEqual([0x8f, 0xdc, 0xef, 255]); // cost + expect(at(99)).toEqual([0x8f, 0xdc, 0xef, 255]); + expect(at(100)).toEqual([255, 255, 255, 255]); // lethal + expect(at(254)).toEqual([255, 255, 255, 255]); // out of contract: lethal + expect(at(255)).toEqual([0, 0, 0, 0]); // unknown: transparent + }); +}); + +describe("gridToImageData", () => { + it("blits through the palette with grid row 0 at the bottom", () => { + // Row-major cells: grid row 0 = [free, cost, unknown] sits at world + // min-y, so it must land on the BOTTOM canvas row. + const cells = Uint8Array.from([0, 50, 255, 100, 0, 255]); + const img = gridToImageData(cells, 3, 2); + expect([img.width, img.height]).toEqual([3, 2]); + expect(rgba(img, 0, 0)).toEqual([255, 255, 255, 255]); // top-left: grid row 1 lethal + expect(rgba(img, 1, 0)).toEqual([0x1e, 0x3a, 0x44, 255]); + expect(rgba(img, 0, 1)).toEqual([0x1e, 0x3a, 0x44, 255]); // bottom-left: grid row 0 free + expect(rgba(img, 1, 1)).toEqual([0x8f, 0xdc, 0xef, 255]); + expect(rgba(img, 2, 1)).toEqual([0, 0, 0, 0]); + }); + + it("rejects a cell count that does not match the dimensions", () => { + expect(() => gridToImageData(new Uint8Array(5), 3, 2)).toThrow(/expected 3x2/); + }); + + it("reuses the caller's ImageData when dimensions match", () => { + const first = gridToImageData(Uint8Array.from([0, 100, 50, 255]), 2, 2); + expect(rgba(first, 0, 0)).toEqual([0x8f, 0xdc, 0xef, 255]); // grid row 1: cost + const second = gridToImageData(Uint8Array.from([255, 0, 255, 0]), 2, 2, first); + expect(second).toBe(first); + // Every pixel is overwritten, alpha included: the previously opaque + // top-left is now the transparent unknown. + expect(rgba(second, 0, 0)).toEqual([0, 0, 0, 0]); + expect(rgba(second, 1, 0)).toEqual([0x1e, 0x3a, 0x44, 255]); + }); + + it("allocates fresh when dimensions differ", () => { + const first = gridToImageData(new Uint8Array(4), 2, 2); + const second = gridToImageData(new Uint8Array(6), 3, 2, first); + expect(second).not.toBe(first); + expect([second.width, second.height]).toEqual([3, 2]); + }); +}); + +describe("map transform", () => { + // 4x2 cells at 0.5 m: a 2x1 m world rect with the lower-left at (-1, 2). + const place = { w: 4, h: 2, res: 0.5, origin: [-1.0, 2.0, 0.0] as [number, number, number] }; + + it("fits, centers, and letterboxes the grid", () => { + const t = fitTransform(place, 200, 200); + expect(t.scale).toBe(100); // width-bound: 200 px / 2 m + expect(worldToCanvas(t, -1.0, 2.0)).toEqual([0, 150]); // lower-left corner + expect(worldToCanvas(t, 1.0, 3.0)).toEqual([200, 50]); // upper-right corner + const blit = gridBlit(t, place); + expect([blit.ax, blit.ay, blit.dw, blit.dh]).toEqual([0, 150, 200, 100]); + expect(blit.rot).toBeCloseTo(0, 12); // -0 at yaw 0, so no toEqual + }); + + it("keeps the y-flip consistent: larger world y is smaller canvas y", () => { + const t = fitTransform(place, 200, 200); + const [, low] = worldToCanvas(t, 0, 2.0); + const [, high] = worldToCanvas(t, 0, 3.0); + expect(high).toBeLessThan(low); + }); + + it("round-trips world -> canvas -> world exactly", () => { + const t = fitTransform(place, 317, 203); // deliberately awkward canvas + for (const [wx, wy] of [[-1.0, 2.0], [0.25, 2.75], [1.0, 3.0], [-0.125, 2.5]]) { + const [cx, cy] = worldToCanvas(t, wx, wy); + const [rx, ry] = canvasToWorld(t, cx, cy); + expect(rx).toBeCloseTo(wx, 9); + expect(ry).toBeCloseTo(wy, 9); + } + }); +}); + +describe("map transform with yaw", () => { + // The with_yaw golden vector's placement (costmap_frames.json): a 0.3 m + // square rotated 0.25 rad CCW about its origin corner (1.5, -0.75). All + // expectations below are hand-computed, not derived from the transform. + const yaw = 0.25; + const place = { w: 3, h: 3, res: 0.1, origin: [1.5, -0.75, yaw] as [number, number, number] }; + + it("fits the rotated grid's bounding box", () => { + // AABB side: 0.3*(cos+sin)(0.25) = 0.3648949; height-bound in 200x100. + const t = fitTransform(place, 200, 100); + expect(t.scale).toBeCloseTo(274.05150, 4); + expect(t.originX).toBeCloseTo(1.4257788, 6); // ox - 0.3*sin(yaw) + expect(t.originY).toBeCloseTo(-0.75, 9); // all corner y-offsets are >= 0 + expect(t.cx0).toBeCloseTo(50, 6); // square AABB letterboxed in x + expect(t.cy0).toBeCloseTo(100, 6); + }); + + it("anchors the blit at the origin corner, rotated by -yaw", () => { + const t = fitTransform(place, 200, 100); + const blit = gridBlit(t, place); + expect(blit.ax).toBeCloseTo(70.34043, 4); + expect(blit.ay).toBeCloseTo(100, 6); + expect(blit.rot).toBeCloseTo(-yaw, 12); + expect(blit.dw).toBeCloseTo(82.21545, 4); + expect(blit.dh).toBeCloseTo(82.21545, 4); + }); + + it("lands every bitmap corner on its world corner", () => { + // Bitmap corners pushed through translate(ax, ay) + rotate(rot) must + // coincide with worldToCanvas of the true world corners; any sign error + // in the yaw handling breaks this closure. + const t = fitTransform(place, 200, 100); + const { ax, ay, rot, dw, dh } = gridBlit(t, place); + const u = [0.3 * Math.cos(yaw), 0.3 * Math.sin(yaw)]; // grid +x in world + const v = [-0.3 * Math.sin(yaw), 0.3 * Math.cos(yaw)]; // grid +y in world + const world: [number, number][] = [ + [1.5, -0.75], + [1.5 + u[0], -0.75 + u[1]], + [1.5 + u[0] + v[0], -0.75 + u[1] + v[1]], + [1.5 + v[0], -0.75 + v[1]], + ]; + const local: [number, number][] = [[0, 0], [dw, 0], [dw, -dh], [0, -dh]]; + for (let i = 0; i < 4; i++) { + const [lx, ly] = local[i]; + const cx = ax + lx * Math.cos(rot) - ly * Math.sin(rot); + const cy = ay + lx * Math.sin(rot) + ly * Math.cos(rot); + const [ex, ey] = worldToCanvas(t, world[i][0], world[i][1]); + expect(cx).toBeCloseTo(ex, 6); + expect(cy).toBeCloseTo(ey, 6); + } + }); + + it("round-trips world -> canvas -> world under yaw", () => { + const t = fitTransform(place, 317, 203); + for (const [wx, wy] of [[1.5, -0.75], [1.6, -0.6], [1.79, -0.45]]) { + const [cx, cy] = worldToCanvas(t, wx, wy); + const [rx, ry] = canvasToWorld(t, cx, cy); + expect(rx).toBeCloseTo(wx, 9); + expect(ry).toBeCloseTo(wy, 9); + } + }); +}); + +describe("posePath", () => { + // 100x100 cells at 1 m in a 100x100 canvas: scale 1, no letterbox. + const t = fitTransform( + { w: 100, h: 100, res: 1.0, origin: [0.0, 0.0, 0.0] }, + 100, + 100, + ); + + it("points the nose along +x world for yaw 0", () => { + const [nose, left, right] = posePath(t, { x: 50, y: 50, yaw: 0 }); + const [cx, cy] = worldToCanvas(t, 50, 50); + expect(nose[0]).toBeGreaterThan(cx); + expect(nose[1]).toBeCloseTo(cy, 9); + expect(left[0]).toBeLessThan(cx); + expect(right[0]).toBeLessThan(cx); + }); + + it("points the nose up the canvas for yaw +pi/2 (world +y)", () => { + const [nose] = posePath(t, { x: 50, y: 50, yaw: Math.PI / 2 }); + const [cx, cy] = worldToCanvas(t, 50, 50); + expect(nose[1]).toBeLessThan(cy); // canvas y grows down + expect(nose[0]).toBeCloseTo(cx, 9); + }); + + it("scales the marker by dpr so its on-screen size is constant", () => { + const [nose, left, right] = posePath(t, { x: 50, y: 50, yaw: 0 }, 2); + const [cx, cy] = worldToCanvas(t, 50, 50); + expect(nose[0] - cx).toBeCloseTo(14.4, 9); // 12 css px * 0.6 * dpr 2 + expect(nose[1]).toBeCloseTo(cy, 9); + expect(left[0] - cx).toBeCloseTo(-9.6, 9); + expect(left[1] - cy).toBeCloseTo(8.4, 9); + expect(right[1] - cy).toBeCloseTo(-8.4, 9); + }); +}); diff --git a/web/cockpit/src/panels/mapRenderer.ts b/web/cockpit/src/panels/mapRenderer.ts new file mode 100644 index 0000000000..5642d28c64 --- /dev/null +++ b/web/cockpit/src/panels/mapRenderer.ts @@ -0,0 +1,178 @@ +// Pure rendering pieces for the map2d panel: occupancy palette, grid blit, +// and the world<->canvas transform. Nothing here touches the DOM beyond +// ImageData and calls on a caller-supplied 2D context, so it all unit-tests +// without a canvas. + +import type { CostmapValue } from "../session/decoders/costmap.ts"; + +export type GridPlacement = Pick; + +function buildPalette(): Uint8ClampedArray { + const lut = new Uint8ClampedArray(256 * 4); + const set = (i: number, r: number, g: number, b: number, a: number): void => { + lut[i * 4] = r; + lut[i * 4 + 1] = g; + lut[i * 4 + 2] = b; + lut[i * 4 + 3] = a; + }; + set(0, 0x1e, 0x3a, 0x44, 255); // free: dark cyan + for (let v = 1; v <= 99; v++) set(v, 0x8f, 0xdc, 0xef, 255); // cost: bright cyan + // 101..254 are outside the wire contract; rendering them lethal is the + // conservative reading. + for (let v = 100; v <= 254; v++) set(v, 255, 255, 255, 255); + set(255, 0, 0, 0, 0); // unknown: transparent, the panel background shows + return lut; +} + +// Operator scheme from the hosted-teleop map (_occupancy_to_bgra in +// dimos/teleop/hosted/map_compress.py), indexed by the wire's uint8 cells. +export const OCCUPANCY_PALETTE: Uint8ClampedArray = buildPalette(); + +// The same palette as native-endian u32 pixels: one read + one write per +// cell keeps budget-sized grids (2048^2) off the frame budget. +const PALETTE32 = new Uint32Array(OCCUPANCY_PALETTE.buffer); + +/** + * Cells to a screen-oriented RGBA bitmap. Grid row 0 sits at world min-y + * (ROS row-major with the origin at the lower-left corner) while canvas row 0 + * is the top, so rows flip here and the transform below stays purely metric. + * Pass the previous frame's ImageData back in to reuse its buffer while the + * dimensions are unchanged; every pixel (alpha included) is overwritten, so + * stale content cannot leak through. + */ +export function gridToImageData( + cells: Uint8Array, + w: number, + h: number, + reuse?: ImageData, +): ImageData { + if (cells.length !== w * h) { + throw new Error(`grid is ${cells.length} cells, expected ${w}x${h}`); + } + const out = reuse !== undefined && reuse.width === w && reuse.height === h + ? reuse + : new ImageData(w, h); + const px = new Uint32Array(out.data.buffer, out.data.byteOffset, w * h); + for (let row = 0; row < h; row++) { + const src = (h - 1 - row) * w; + const dst = row * w; + for (let col = 0; col < w; col++) { + px[dst + col] = PALETTE32[cells[src + col]]; + } + } + return out; +} + +/** + * Aspect-preserving whole-grid fit, letterboxed and centered: the world-space + * bounding box of the (possibly yaw-rotated) grid rectangle fills the canvas. + * The canvas stays world-axis-aligned - only the grid blit rotates (gridBlit) + * - so worldToCanvas needs no rotation term. T10 extends this transform + * (zoom, minimap crop) rather than the callers. + */ +export interface MapTransform { + /** Device pixels per world meter. */ + scale: number; + /** World coordinates of the fitted bounding box's min corner (the grid's + * lower-left origin corner when yaw is 0). */ + originX: number; + originY: number; + /** Canvas position of that corner (canvas y grows downward). */ + cx0: number; + cy0: number; +} + +export function fitTransform(place: GridPlacement, canvasW: number, canvasH: number): MapTransform { + const worldW = place.w * place.res; + const worldH = place.h * place.res; + const c = Math.cos(place.origin[2]); + const s = Math.sin(place.origin[2]); + // Corner offsets from the origin corner are u = worldW*(c, s) and + // v = worldH*(-s, c); the AABB spans their per-axis extremes. + const bw = Math.abs(worldW * c) + Math.abs(worldH * s); + const bh = Math.abs(worldW * s) + Math.abs(worldH * c); + const scale = Math.min(canvasW / bw, canvasH / bh); + return { + scale, + originX: place.origin[0] + Math.min(0, worldW * c) + Math.min(0, -worldH * s), + originY: place.origin[1] + Math.min(0, worldW * s) + Math.min(0, worldH * c), + cx0: (canvasW - bw * scale) / 2, + cy0: canvasH - (canvasH - bh * scale) / 2, + }; +} + +/** World meters to canvas pixels; the y-flip lives here. */ +export function worldToCanvas(t: MapTransform, wx: number, wy: number): [number, number] { + return [t.cx0 + (wx - t.originX) * t.scale, t.cy0 - (wy - t.originY) * t.scale]; +} + +/** Exact inverse of worldToCanvas (T10 click-to-goal reads this). */ +export function canvasToWorld(t: MapTransform, cx: number, cy: number): [number, number] { + return [t.originX + (cx - t.cx0) / t.scale, t.originY + (t.cy0 - cy) / t.scale]; +} + +/** + * Placement for the (screen-oriented) grid bitmap: translate to (ax, ay) - + * the canvas position of the grid's origin corner - rotate by `rot`, then + * draw the bitmap into [0, -dh, dw, dh]. World yaw is CCW with y up while + * canvas y grows down, so the canvas angle is -yaw; at yaw 0 this reduces to + * the axis-aligned blit [ax, ay - dh, dw, dh]. + */ +export function gridBlit( + t: MapTransform, + place: GridPlacement, +): { ax: number; ay: number; rot: number; dw: number; dh: number } { + const [ax, ay] = worldToCanvas(t, place.origin[0], place.origin[1]); + return { + ax, + ay, + rot: -place.origin[2], + dw: place.w * place.res * t.scale, + dh: place.h * place.res * t.scale, + }; +} + +export const POSE_COLOR = "#ff5c5c"; +// Triangle length in CSS pixels: screen-constant so the marker stays legible +// however far the fit zooms out. The canvas backing store is DPR-scaled, so +// callers pass their dpr to keep the on-screen size constant. +const POSE_PX = 12; + +export interface Pose2d { + x: number; + y: number; + yaw: number; +} + +/** + * Triangle vertices ([nose, left, right]) for the pose marker. World yaw is + * CCW-positive with y up; canvas y grows down, so the canvas angle is -yaw. + */ +export function posePath(t: MapTransform, pose: Pose2d, dpr = 1): [number, number][] { + const [cx, cy] = worldToCanvas(t, pose.x, pose.y); + const cos = Math.cos(-pose.yaw); + const sin = Math.sin(-pose.yaw); + const size = POSE_PX * dpr; + const local: [number, number][] = [ + [size * 0.6, 0], + [-size * 0.4, size * 0.35], + [-size * 0.4, -size * 0.35], + ]; + return local.map(([px, py]) => [cx + px * cos - py * sin, cy + px * sin + py * cos]); +} + +export function drawPose( + ctx: CanvasRenderingContext2D, + t: MapTransform, + pose: Pose2d, + dpr = 1, +): void { + const [nose, left, right] = posePath(t, pose, dpr); + ctx.beginPath(); + ctx.moveTo(nose[0], nose[1]); + ctx.lineTo(left[0], left[1]); + ctx.lineTo(right[0], right[1]); + ctx.closePath(); + ctx.fillStyle = POSE_COLOR; + ctx.fill(); +} diff --git a/web/cockpit/src/panels/panels.test.tsx b/web/cockpit/src/panels/panels.test.tsx index cd28f95b8f..a2ffe3ef26 100644 --- a/web/cockpit/src/panels/panels.test.tsx +++ b/web/cockpit/src/panels/panels.test.tsx @@ -1,9 +1,12 @@ // @vitest-environment happy-dom -import { afterEach, beforeEach, describe, expect, it, vi } from "vitest"; +import { afterEach, beforeAll, beforeEach, describe, expect, it, vi } from "vitest"; import { act } from "react"; import { createRoot, type Root } from "react-dom/client"; import type { FrameHeader, PanelSpec } from "@dimos/shared"; +import type { CostmapValue } from "../session/decoders/costmap.ts"; import { ChannelStore } from "../session/store.ts"; +import { MapPanel, startMapSink } from "./MapPanel.tsx"; +import { fitTransform, posePath } from "./mapRenderer.ts"; import { PanelGrid } from "./PanelGrid.tsx"; import { getPanel } from "./registry.ts"; import { type DrawHealth, startVideoSink, VideoPanel } from "./VideoPanel.tsx"; @@ -345,8 +348,418 @@ describe("PanelGrid", () => { expect(container.innerHTML).toBe(""); }); - it("has the video panel registered", () => { + it("has the video and map2d panels registered", () => { expect(getPanel("video")).toBe(VideoPanel); + expect(getPanel("map2d")).toBe(MapPanel); expect(getPanel("hologram")).toBeUndefined(); }); }); + +// --------------------------------------------------------------------------- +// Map panel + +const MAP_CH = "global_costmap"; +const POSE_CH = "odom"; + +function costmapValue(seq: number, w = 2, h = 2): CostmapValue { + return { bytes: new Uint8Array([seq]), w, h, res: 0.5, origin: [0.25, -0.5, 0.0] }; +} + +function gridFrame(store: ChannelStore, seq: number, ts = seq): CostmapValue { + const value = costmapValue(seq); + store.ingest(MAP_CH, { ch: MAP_CH, seq, ts, delivery: "latest" }, value, true); + return value; +} + +function poseFrame(store: ChannelStore, seq: number): void { + const value = { x: 0.5, y: 0.5, z: 0.1, yaw: 0.25, ts: seq }; + store.ingest(POSE_CH, { ch: POSE_CH, seq, ts: seq, delivery: "reliable" }, value, true); +} + +/** Inflate stub whose promises settle only when the test says so. */ +function deferredInflate() { + const calls: CostmapValue[] = []; + const settlers: { resolve: (cells: Uint8Array) => void; reject: (e: Error) => void }[] = []; + const inflate = (value: CostmapValue): Promise => { + calls.push(value); + return new Promise((resolve, reject) => settlers.push({ resolve, reject })); + }; + return { inflate, calls, settlers }; +} + +/** happy-dom has no layout; pin the CSS size the sink reads. */ +function defineSize(canvas: HTMLCanvasElement, w: number, h: number): void { + Object.defineProperty(canvas, "clientWidth", { configurable: true, value: w }); + Object.defineProperty(canvas, "clientHeight", { configurable: true, value: h }); +} + +describe("startMapSink", () => { + interface FakeCtx { + drawImage: ReturnType; + putImageData: ReturnType; + clearRect: ReturnType; + save: ReturnType; + restore: ReturnType; + translate: ReturnType; + rotate: ReturnType; + beginPath: ReturnType; + moveTo: ReturnType; + lineTo: ReturnType; + closePath: ReturnType; + fill: ReturnType; + } + let store: ChannelStore; + let canvas: HTMLCanvasElement; + let contexts: FakeCtx[]; + let health: DrawHealth; + let stop: (() => void) | null; + // getContext creation order in startMapSink: display canvas, then backing. + const display = () => contexts[0]; + const backing = () => contexts[1]; + + beforeEach(() => { + store = new ChannelStore(); + canvas = document.createElement("canvas"); + defineSize(canvas, 100, 80); + contexts = []; + vi.spyOn(HTMLCanvasElement.prototype, "getContext").mockImplementation(() => { + const fake: FakeCtx = { + drawImage: vi.fn(), + putImageData: vi.fn(), + clearRect: vi.fn(), + save: vi.fn(), + restore: vi.fn(), + translate: vi.fn(), + rotate: vi.fn(), + beginPath: vi.fn(), + moveTo: vi.fn(), + lineTo: vi.fn(), + closePath: vi.fn(), + fill: vi.fn(), + }; + contexts.push(fake); + return fake as unknown as CanvasRenderingContext2D; + }); + health = { lastDrawOkAtMs: 0, failures: 0 }; + stop = null; + }); + + afterEach(() => { + stop?.(); + vi.restoreAllMocks(); + vi.unstubAllGlobals(); + }); + + it("inflates one grid at a time and skips straight to the newest", async () => { + const { inflate, calls, settlers } = deferredInflate(); + stop = startMapSink(store, MAP_CH, POSE_CH, canvas, health, { inflate, hidden: () => false }); + + const first = gridFrame(store, 1); + expect(calls).toEqual([first]); + gridFrame(store, 2); + const newest = gridFrame(store, 3); + expect(calls.length).toBe(1); // one inflate in flight, burst sheds + + settlers[0].resolve(new Uint8Array(4)); + await flush(); + expect(backing().putImageData).toHaveBeenCalledTimes(1); + const img = backing().putImageData.mock.calls[0][0] as ImageData; + expect([img.width, img.height]).toEqual([2, 2]); + expect(display().drawImage).toHaveBeenCalledTimes(1); + expect(canvas.width).toBe(100); // sized from the layout, not the grid + expect(calls.length).toBe(2); + expect(calls[1]).toBe(newest); // frame 2 was never inflated + + settlers[1].resolve(new Uint8Array(4)); + await flush(); + expect(calls.length).toBe(2); // caught up + }); + + it("redraws the pose from the cached bitmap without a new inflate", async () => { + const { inflate, calls, settlers } = deferredInflate(); + stop = startMapSink(store, MAP_CH, POSE_CH, canvas, health, { inflate, hidden: () => false }); + gridFrame(store, 1); + settlers[0].resolve(new Uint8Array(4)); + await flush(); + expect(display().fill).not.toHaveBeenCalled(); // no pose yet, no triangle + + poseFrame(store, 1); + expect(display().drawImage).toHaveBeenCalledTimes(2); + expect(display().fill).toHaveBeenCalledTimes(1); // the triangle + expect(backing().putImageData).toHaveBeenCalledTimes(1); // bitmap reused + expect(calls.length).toBe(1); + }); + + it("ignores pose frames until a grid has drawn", () => { + const { inflate } = deferredInflate(); + stop = startMapSink(store, MAP_CH, POSE_CH, canvas, health, { inflate, hidden: () => false }); + poseFrame(store, 1); + expect(display().drawImage).not.toHaveBeenCalled(); + }); + + it("counts inflate rejections and recovers on the next grid", async () => { + const { inflate, settlers } = deferredInflate(); + stop = startMapSink(store, MAP_CH, POSE_CH, canvas, health, { inflate, hidden: () => false }); + const stamp = health.lastDrawOkAtMs; + gridFrame(store, 1); + settlers[0].reject(new Error("corrupt zlib")); + await flush(); + expect(health.failures).toBe(1); + expect(health.lastDrawOkAtMs).toBe(stamp); // only successes stamp it + + gridFrame(store, 2); + settlers[1].resolve(new Uint8Array(4)); + await flush(); + expect(health.failures).toBe(0); + }); + + it("skips a slot that is not a costmap value without spinning", () => { + const { inflate, calls } = deferredInflate(); + stop = startMapSink(store, MAP_CH, POSE_CH, canvas, health, { inflate, hidden: () => false }); + store.ingest( + MAP_CH, + { ch: MAP_CH, seq: 1, ts: 1, delivery: "latest" }, + new Uint8Array(3), + true, + ); + expect(calls.length).toBe(0); + }); + + it("does not inflate while hidden and catches up on visibilitychange", async () => { + const { inflate, calls, settlers } = deferredInflate(); + let hidden = true; + stop = startMapSink(store, MAP_CH, POSE_CH, canvas, health, { inflate, hidden: () => hidden }); + gridFrame(store, 1); + const newest = gridFrame(store, 2); + expect(calls.length).toBe(0); // a backgrounded panel costs no inflate + + hidden = false; + document.dispatchEvent(new Event("visibilitychange")); + expect(calls.length).toBe(1); + expect(calls[0]).toBe(newest); + settlers[0].resolve(new Uint8Array(4)); + await flush(); + }); + + it("redraws on resize with the cached bitmap and disposes the observer", async () => { + const { inflate, calls, settlers } = deferredInflate(); + let resize: (() => void) | null = null; + const dispose = vi.fn(); + stop = startMapSink(store, MAP_CH, POSE_CH, canvas, health, { + inflate, + hidden: () => false, + observeResize: (_el, cb) => { + resize = cb; + return dispose; + }, + }); + gridFrame(store, 1); + settlers[0].resolve(new Uint8Array(4)); + await flush(); + expect(canvas.width).toBe(100); + + defineSize(canvas, 250, 80); + resize!(); + expect(canvas.width).toBe(250); // backing store follows the layout size + expect(display().drawImage).toHaveBeenCalledTimes(2); + expect(calls.length).toBe(1); // no re-inflate on resize + + stop!(); + stop = null; + expect(dispose).toHaveBeenCalledTimes(1); + }); + + it("stops inflating and drawing after cleanup", async () => { + const { inflate, calls, settlers } = deferredInflate(); + stop = startMapSink(store, MAP_CH, POSE_CH, canvas, health, { inflate, hidden: () => false }); + gridFrame(store, 1); + stop(); + stop = null; + + settlers[0].resolve(new Uint8Array(4)); + await flush(); + expect(backing().putImageData).not.toHaveBeenCalled(); // in-flight inflate must not paint + + gridFrame(store, 2); + poseFrame(store, 1); + expect(calls.length).toBe(1); + expect(display().drawImage).not.toHaveBeenCalled(); + }); + + it("rotates the grid blit by -yaw and restores before the pose", async () => { + const { inflate, settlers } = deferredInflate(); + stop = startMapSink(store, MAP_CH, POSE_CH, canvas, health, { inflate, hidden: () => false }); + const value = { ...costmapValue(1), origin: [0.25, -0.5, 0.25] as [number, number, number] }; + store.ingest(MAP_CH, { ch: MAP_CH, seq: 1, ts: 1, delivery: "latest" }, value, true); + settlers[0].resolve(new Uint8Array(4)); + await flush(); + poseFrame(store, 1); + + expect(display().rotate.mock.calls[0][0]).toBeCloseTo(-0.25, 9); + const [, dx, dy, dw, dh] = display().drawImage.mock.calls[0]; + expect(dx).toBe(0); + expect(dy).toBeCloseTo(-dh, 9); // drawn upward from the rotated anchor + expect(dw).toBeCloseTo(dh, 9); // square grid + // The pose triangle must not inherit the grid rotation: the last restore + // (this draw's) precedes the triangle fill. + const restores = display().restore.mock.invocationCallOrder; + expect(restores[restores.length - 1]).toBeLessThan( + display().fill.mock.invocationCallOrder[0], + ); + }); + + it("reuses the ImageData buffer across same-size grids", async () => { + const { inflate, settlers } = deferredInflate(); + stop = startMapSink(store, MAP_CH, POSE_CH, canvas, health, { inflate, hidden: () => false }); + gridFrame(store, 1); + settlers[0].resolve(new Uint8Array(4)); + await flush(); + gridFrame(store, 2); + settlers[1].resolve(new Uint8Array(4)); + await flush(); + const first = backing().putImageData.mock.calls[0][0]; + expect(backing().putImageData.mock.calls[1][0]).toBe(first); + + const wider = costmapValue(3, 3, 2); + store.ingest(MAP_CH, { ch: MAP_CH, seq: 3, ts: 3, delivery: "latest" }, wider, true); + settlers[2].resolve(new Uint8Array(6)); + await flush(); + expect(backing().putImageData.mock.calls[2][0]).not.toBe(first); + }); + + it("sizes the backing store and pose marker by devicePixelRatio", async () => { + vi.stubGlobal("devicePixelRatio", 2); + const { inflate, settlers } = deferredInflate(); + stop = startMapSink(store, MAP_CH, POSE_CH, canvas, health, { inflate, hidden: () => false }); + gridFrame(store, 1); + settlers[0].resolve(new Uint8Array(4)); + await flush(); + expect([canvas.width, canvas.height]).toEqual([200, 160]); // css 100x80 * dpr 2 + + poseFrame(store, 1); + const t = fitTransform({ w: 2, h: 2, res: 0.5, origin: [0.25, -0.5, 0] }, 200, 160); + const [ex, ey] = posePath(t, { x: 0.5, y: 0.5, yaw: 0.25 }, 2)[0]; + const [nx, ny] = display().moveTo.mock.calls[0]; + expect(nx).toBeCloseTo(ex, 9); // the sink passed its dpr to the marker + expect(ny).toBeCloseTo(ey, 9); + }); +}); + +describe("MapPanel", () => { + const SPEC: PanelSpec = { id: "map", kind: "map2d", channels: [MAP_CH, POSE_CH] }; + let container: HTMLElement; + let root: Root; + let now: number; + let store: ChannelStore; + let deflated: Uint8Array; + const badge = () => container.querySelector(`[data-testid="map2d-${MAP_CH}-badge"]`)!; + + beforeAll(async () => { + // Real zlib bytes so the panel's default inflate path runs end to end. + const stream = new Blob([Uint8Array.from([0, 50, 100, 255]) as BlobPart]).stream() + .pipeThrough(new CompressionStream("deflate")); + deflated = new Uint8Array(await new Response(stream).arrayBuffer()); + }); + + beforeEach(() => { + container = document.createElement("div"); + document.body.appendChild(container); + root = createRoot(container); + now = 1_000_000; + store = new ChannelStore(() => now); + }); + + afterEach(() => { + act(() => root.unmount()); + container.remove(); + vi.restoreAllMocks(); + }); + + function realGridFrame(seq: number, ts = seq): void { + const value: CostmapValue = { + bytes: deflated, + w: 2, + h: 2, + res: 0.5, + origin: [0.25, -0.5, 0.0], + }; + store.ingest(MAP_CH, { ch: MAP_CH, seq, ts, delivery: "latest" }, value, true); + } + + it("shows waiting, then the Hz badge, then flags staleness", async () => { + act(() => root.render()); + expect(container.textContent).toContain("waiting for data"); + expect(badge().textContent).toBe("waiting"); + expect(badge().getAttribute("role")).toBe("status"); + const canvas = container.querySelector("canvas")!; + expect(canvas.getAttribute("role")).toBe("img"); + expect(canvas.getAttribute("aria-label")).toBe("map"); + + // Grids at 5 Hz of source time, arriving with zero skew. + await act(async () => { + for (let i = 0; i < 5; i++) realGridFrame(i, now / 1000 - (4 - i) / 5); + await flush(); + store.publishUi(); + }); + expect(container.textContent).not.toContain("waiting for data"); + expect(badge().textContent).toMatch(/Hz$/); + expect(badge().getAttribute("data-stale")).toBeNull(); + + // Silence: source age climbs past the threshold on a later UI tick. + act(() => { + now += 12_000; + store.publishUi(); + }); + expect(badge().textContent).toMatch(/^stale/); + expect(badge().getAttribute("data-stale")).toBe("true"); + }); + + it("flags a failing inflate in the badge and recovers on the next grid", async () => { + act(() => root.render()); + await act(async () => { + // Not a zlib stream: the panel's real inflate rejects. + store.ingest( + MAP_CH, + { ch: MAP_CH, seq: 1, ts: now / 1000, delivery: "latest" }, + costmapValue(9), + true, + ); + await flush(); + await flush(); + store.publishUi(); + }); + expect(badge().textContent).toBe("decode failing"); + expect(badge().getAttribute("data-error")).toBe("true"); + + await act(async () => { + realGridFrame(2, now / 1000); + await flush(); + await flush(); + store.publishUi(); + }); + expect(badge().textContent).toMatch(/Hz$/); + expect(badge().getAttribute("data-error")).toBeNull(); + }); + + it("renders without a pose binding (single-channel spec)", async () => { + act(() => + root.render( + , + ) + ); + await act(async () => { + realGridFrame(1, now / 1000); + await flush(); + store.publishUi(); + }); + expect(badge().textContent).toMatch(/Hz$/); + }); + + it("renders a visible note instead of a canvas when no channel is bound", () => { + act(() => + root.render() + ); + expect(container.textContent).toContain("no channel bound"); + expect(container.querySelector("canvas")).toBeNull(); + }); +}); diff --git a/web/cockpit/src/panels/registry.ts b/web/cockpit/src/panels/registry.ts index 34f24d4379..57ec87a730 100644 --- a/web/cockpit/src/panels/registry.ts +++ b/web/cockpit/src/panels/registry.ts @@ -6,6 +6,7 @@ import type { ComponentType } from "react"; import type { PanelSpec } from "@dimos/shared"; import type { ChannelStore } from "../session/store.ts"; +import { MapPanel } from "./MapPanel.tsx"; import { VideoPanel } from "./VideoPanel.tsx"; export interface PanelProps { @@ -24,3 +25,4 @@ export function getPanel(kind: string): ComponentType | undefined { } registerPanel("video", VideoPanel); +registerPanel("map2d", MapPanel); diff --git a/web/cockpit/src/session/decoders/costmap.ts b/web/cockpit/src/session/decoders/costmap.ts new file mode 100644 index 0000000000..2daa63aabf --- /dev/null +++ b/web/cockpit/src/session/decoders/costmap.ts @@ -0,0 +1,112 @@ +import type { FrameHeader } from "@dimos/shared"; +import type { Decoded } from "./index.ts"; + +// Mirrors the bridge's costmap.zlib.v1 ingress cap (_MAX_PAYLOAD_BYTES in +// dimos/web/relay_bridge/_wt_session.py). +export const MAX_COSTMAP_PAYLOAD_BYTES = 8 * 1024 * 1024; + +// Per-axis bound doubling as the render budget: 2048^2 is 4 Mi cells, 16 MiB +// as RGBA, which the main-thread palette convert + canvas upload can afford +// at 5 Hz. Must stay >= the bridge's _COSTMAP_MAX_SIDE (relay_bridge_module), +// which block-max downsamples larger grids before sending. +export const MAX_COSTMAP_DIM = 2048; + +/** Validated `costmap.zlib.v1` slot value: deflated cells plus placement. */ +export interface CostmapValue { + /** zlib-deflated uint8 cells, row-major, row 0 at world min-y. */ + bytes: Uint8Array; + w: number; + h: number; + /** Cell edge in meters. */ + res: number; + /** Grid lower-left corner in world coordinates: [x, y, yaw]. */ + origin: [number, number, number]; +} + +function metaNumber(meta: Record, key: string): number { + const v = meta[key]; + if (typeof v !== "number" || !Number.isFinite(v)) { + throw new Error(`costmap meta ${key} is not a finite number`); + } + return v; +} + +/** + * Decoder for `costmap.zlib.v1`: the value is the still-deflated payload plus + * the validated placement meta folded in (slots carry no header, and the + * panel-paced inflate needs w/h/res/origin). The meta is robot-controlled and + * cannot be verified against compressed bytes cheaply, so this bounds it + * instead: a lying meta allocates at most MAX_COSTMAP_DIM^2 cells and then + * fails the inflate length check. Inflate itself is async (DecompressionStream) and + * lives with the panel (jpeg.ts precedent): the store must always hold the + * newest bytes even when inflate falls behind, and inflate must never run for + * frames nobody draws. + */ +export function costmapDecoder(payload: Uint8Array, header: FrameHeader): Decoded { + if (payload.byteLength > MAX_COSTMAP_PAYLOAD_BYTES) { + throw new Error( + `oversized costmap payload: ${payload.byteLength} B, cap ${MAX_COSTMAP_PAYLOAD_BYTES} B`, + ); + } + const meta = header.meta; + if (meta === undefined) throw new Error("costmap frame has no meta"); + const w = metaNumber(meta, "w"); + const h = metaNumber(meta, "h"); + const res = metaNumber(meta, "res"); + if (!Number.isInteger(w) || !Number.isInteger(h) || w < 1 || h < 1) { + throw new Error(`costmap dimensions ${w}x${h} are not positive integers`); + } + if (w > MAX_COSTMAP_DIM || h > MAX_COSTMAP_DIM) { + throw new Error(`costmap dimensions ${w}x${h} out of bounds`); + } + if (res <= 0) throw new Error(`costmap resolution ${res} must be positive`); + const origin = meta.origin; + if ( + !Array.isArray(origin) || origin.length !== 3 || + !origin.every((n) => typeof n === "number" && Number.isFinite(n)) + ) { + throw new Error("costmap origin must be [x, y, yaw]"); + } + const value: CostmapValue = { + bytes: payload, + w, + h, + res, + origin: [origin[0], origin[1], origin[2]], + }; + return { value, preview: `(costmap ${w}x${h}, ${payload.byteLength} B)` }; +} + +/** + * Inflate a validated slot value to exactly w*h cells. Python zlib.compress + * emits RFC 1950 zlib framing, which is DecompressionStream("deflate"); + * "deflate-raw" is RFC 1951 and would reject every frame (the golden vectors + * in shared/fixtures/costmap_frames.json pin this pairing). Output beyond + * w*h throws mid-stream (decompression-bomb guard), a short stream throws at + * the end, and a corrupt stream rejects from read(). + */ +export async function inflateCostmap(value: CostmapValue): Promise { + const expected = value.w * value.h; + const cells = new Uint8Array(expected); + let written = 0; + const inflated = new Blob([value.bytes as BlobPart]).stream() + .pipeThrough(new DecompressionStream("deflate")); + const reader = inflated.getReader(); + try { + while (true) { + const { done, value: chunk } = await reader.read(); + if (done) break; + if (written + chunk.length > expected) { + throw new Error(`costmap inflates beyond ${expected} cells`); + } + cells.set(chunk, written); + written += chunk.length; + } + } finally { + void reader.cancel().catch(() => {}); + } + if (written !== expected) { + throw new Error(`costmap inflated to ${written} cells, expected ${expected}`); + } + return cells; +} diff --git a/web/cockpit/src/session/decoders/decoders.test.ts b/web/cockpit/src/session/decoders/decoders.test.ts index cfd8577356..f1a32fad5b 100644 --- a/web/cockpit/src/session/decoders/decoders.test.ts +++ b/web/cockpit/src/session/decoders/decoders.test.ts @@ -1,11 +1,22 @@ import { describe, expect, it } from "vitest"; import type { FrameHeader } from "@dimos/shared"; +import costmapFrames from "../../../../shared/fixtures/costmap_frames.json"; +import { + type CostmapValue, + inflateCostmap, + MAX_COSTMAP_DIM, + MAX_COSTMAP_PAYLOAD_BYTES, +} from "./costmap.ts"; import { getDecoder, registerDecoder } from "./index.ts"; import { MAX_JPEG_DIM, MAX_JPEG_PAYLOAD_BYTES } from "./jpeg.ts"; import { JSON_PREVIEW_MAX_CHARS, MAX_JSON_PAYLOAD_BYTES } from "./json.ts"; const HEADER: FrameHeader = { ch: "x", seq: 1, ts: 0, delivery: "latest" }; +function b64ToBytes(b64: string): Uint8Array { + return Uint8Array.from(atob(b64), (c) => c.charCodeAt(0)); +} + /** Minimal scannable JPEG: SOI + SOF0 declaring w x h (no scan data). */ function jpegBytes(w: number, h: number): Uint8Array { // SOI, then SOF0 (FF C0) with length 11: precision 8, height BE, width BE, @@ -28,7 +39,6 @@ describe("decoder registry", () => { }); it("returns undefined for unknown encodings (unsupported, not an error)", () => { - expect(getDecoder("costmap.zlib.v1")).toBeUndefined(); expect(getDecoder("h264.v1")).toBeUndefined(); expect(getDecoder(undefined)).toBeUndefined(); }); @@ -110,3 +120,74 @@ describe("decoder registry", () => { expect(preview!.length).toBeLessThan(JSON_PREVIEW_MAX_CHARS + 50); }); }); + +describe("costmap decoder", () => { + const decode = getDecoder("costmap.zlib.v1")!; + const header = (meta: Record): FrameHeader => ({ ...HEADER, meta }); + const META = { w: 3, h: 2, res: 0.05, origin: [-1.25, 2.5, 0.25] }; + + async function deflate(bytes: Uint8Array): Promise { + const stream = new Blob([bytes as BlobPart]).stream() + .pipeThrough(new CompressionStream("deflate")); + return new Uint8Array(await new Response(stream).arrayBuffer()); + } + + it("decodes and inflates every golden vector byte-exactly", async () => { + // The pytest mirror (test_costmap_encoding.py) re-encodes these same + // vectors; together they pin the Python-zlib -> DecompressionStream pair. + for (const vec of costmapFrames.vectors) { + const payload = b64ToBytes(vec.payload_b64); + const decoded = decode(payload, header(vec.meta)); + const value = decoded.value as CostmapValue; + expect(decoded.preview).toBe( + `(costmap ${vec.meta.w}x${vec.meta.h}, ${payload.byteLength} B)`, + ); + expect(value.bytes).toBe(payload); // the bytes stay deflated, no copy + expect({ w: value.w, h: value.h, res: value.res, origin: value.origin }).toEqual(vec.meta); + expect(await inflateCostmap(value)).toEqual(b64ToBytes(vec.grid_b64)); + } + }); + + it("rejects missing or malformed meta", () => { + const payload = new Uint8Array([1, 2, 3]); + expect(() => decode(payload, HEADER)).toThrow(/no meta/); + expect(() => decode(payload, header({ ...META, w: 2.5 }))).toThrow(/positive integers/); + expect(() => decode(payload, header({ ...META, h: 0 }))).toThrow(/positive integers/); + expect(() => decode(payload, header({ ...META, res: -0.5 }))).toThrow(/positive/); + expect(() => decode(payload, header({ ...META, res: "0.05" }))).toThrow(/finite/); + expect(() => decode(payload, header({ ...META, origin: [1.5, 2.5] }))).toThrow(/origin/); + expect(() => decode(payload, header({ ...META, origin: [1.5, 2.5, Infinity] }))).toThrow( + /origin/, + ); + }); + + it("rejects out-of-bounds dimensions and oversized payloads", () => { + const payload = new Uint8Array(8); + expect(() => decode(payload, header({ ...META, w: MAX_COSTMAP_DIM + 1, h: 1 }))).toThrow( + /out of bounds/, + ); + expect(() => decode(payload, header({ ...META, w: 1, h: MAX_COSTMAP_DIM + 1 }))).toThrow( + /out of bounds/, + ); + expect(() => decode(new Uint8Array(MAX_COSTMAP_PAYLOAD_BYTES + 1), header(META))).toThrow( + /oversized/, + ); + }); + + it("rejects an inflate length mismatch in either direction", async () => { + const deflated = await deflate(new Uint8Array(24).fill(7)); + // Declared 3x2=6 cells but the stream inflates to 24: the bomb guard + // fires mid-stream instead of allocating past the declaration. + const bomb = decode(deflated, header({ ...META, w: 3, h: 2 })).value as CostmapValue; + await expect(inflateCostmap(bomb)).rejects.toThrow(/beyond/); + const short = decode(deflated, header({ ...META, w: 5, h: 5 })).value as CostmapValue; + await expect(inflateCostmap(short)).rejects.toThrow(/expected/); + }); + + it("rejects a truncated deflate stream", async () => { + const vec = costmapFrames.vectors[0]; + const payload = b64ToBytes(vec.payload_b64).slice(0, 6); + const value = decode(payload, header(vec.meta)).value as CostmapValue; + await expect(inflateCostmap(value)).rejects.toThrow(); + }); +}); diff --git a/web/cockpit/src/session/decoders/index.ts b/web/cockpit/src/session/decoders/index.ts index 5b2b31b3a6..3333146b9d 100644 --- a/web/cockpit/src/session/decoders/index.ts +++ b/web/cockpit/src/session/decoders/index.ts @@ -1,9 +1,10 @@ // Payload decoder registry, keyed by the manifest's encoding id. An encoding // without a decoder is not an error: the channel renders as "unsupported" // (forward compatibility with newer bridges). Binary decoders -// (costmap.zlib.v1, ...) arrive with their panels. +// (h264.v1, ...) arrive with their panels. import type { FrameHeader } from "@dimos/shared"; +import { costmapDecoder } from "./costmap.ts"; import { jpegDecoder } from "./jpeg.ts"; import { jsonDecoder } from "./json.ts"; @@ -30,3 +31,4 @@ export function getDecoder(encoding: string | undefined): Decoder | undefined { } registerDecoder("jpeg.v1", jpegDecoder); +registerDecoder("costmap.zlib.v1", costmapDecoder); diff --git a/web/cockpit/src/session/session.test.ts b/web/cockpit/src/session/session.test.ts index 57a07a14b5..e6aeb20d07 100644 --- a/web/cockpit/src/session/session.test.ts +++ b/web/cockpit/src/session/session.test.ts @@ -10,6 +10,7 @@ import { type RobotInfo, } from "@dimos/shared"; import type { Manifest } from "@dimos/shared/manifest"; +import type { CostmapValue } from "./decoders/costmap.ts"; import { channelSubscribable, manifestsEqual, @@ -80,8 +81,10 @@ describe("pickAutoWatch", () => { describe("subscribableChannels", () => { const odom = spec(); const jpeg = spec({ ch: "color_image", encoding: "jpeg.v1", delivery: "latest" }); + const costmap = spec({ ch: "global_costmap", encoding: "costmap.zlib.v1", delivery: "latest" }); const future = spec({ ch: "voxels", encoding: "voxels.bin.v9", delivery: "latest" }); const videoPanel: PanelSpec = { id: "cam", kind: "video", channels: ["color_image"] }; + const mapPanel: PanelSpec = { id: "map", kind: "map2d", channels: ["global_costmap", "odom"] }; it("keeps only channels with a decoder (undecodable ones waste bandwidth)", () => { expect(subscribableChannels([odom, jpeg, future], [videoPanel])).toEqual([odom, jpeg]); @@ -96,6 +99,12 @@ describe("subscribableChannels", () => { // Cheap JSON channels are subscribed with or without a panel. expect(channelSubscribable(odom, [])).toBe(true); }); + + it("gates the costmap encoding like jpeg (grids nobody renders stay unencoded)", () => { + expect(channelSubscribable(costmap, [])).toBe(false); + expect(channelSubscribable(costmap, [mapPanel])).toBe(true); + expect(channelSubscribable(costmap, [{ ...mapPanel, kind: "hologram" }])).toBe(false); + }); }); // --------------------------------------------------------------------------- @@ -116,6 +125,10 @@ const ROBOT_B: RobotInfo = { id: "b", name: "B", model: "go2" }; class FakeRelayEnd { readonly sent: Msg[] = []; + /** Awaited per decoded viewer message: lets a test react at the exact + * moment a message is observed while holding the session's control writer + * (e.g. to land a data frame mid-sub-loop). */ + onMsg: ((msg: Msg) => void | Promise) | null = null; readonly wt: WebTransportLike; #control!: ReadableStreamDefaultController; #uni!: ReadableStreamDefaultController>; @@ -128,8 +141,11 @@ class FakeRelayEnd { }, }); const writable = new WritableStream({ - write: (chunk) => { - this.sent.push(...inbound.push(chunk)); + write: async (chunk) => { + for (const msg of inbound.push(chunk)) { + this.sent.push(msg); + await this.onMsg?.(msg); + } }, }); let closeWt = () => {}; @@ -159,8 +175,8 @@ class FakeRelayEnd { } /** One data frame on its own uni stream, arbitrary payload bytes. */ - pushRaw(seq: number, payload: Uint8Array, ch: string): void { - const frame = encodeDataFrame({ ch, seq, ts: seq, delivery: "reliable" }, payload); + pushRaw(seq: number, payload: Uint8Array, ch: string, meta?: Record): void { + const frame = encodeDataFrame({ ch, seq, ts: seq, delivery: "reliable", meta }, payload); this.#uni.enqueue( new ReadableStream({ start: (c) => { @@ -270,6 +286,67 @@ describe("Session over a fake WebTransport", () => { expect(relay.subs()).toEqual(["odom"]); }); + it("subs the costmap channel when a map2d panel binds it", async () => { + const { relay, handle } = start(); + await goLive( + relay, + handle, + ROBOT_A, + [spec(), spec({ ch: "global_costmap", encoding: "costmap.zlib.v1", delivery: "latest" })], + [{ id: "map", kind: "map2d", channels: ["global_costmap", "odom"] }], + ); + expect(relay.subs()).toEqual(["odom", "global_costmap"]); + }); + + it("keeps the costmap channel unsubscribed under an unrenderable panel kind", async () => { + const { relay, handle } = start(); + await goLive( + relay, + handle, + ROBOT_A, + [spec(), spec({ ch: "global_costmap", encoding: "costmap.zlib.v1", delivery: "latest" })], + [{ id: "holo", kind: "hologram", channels: ["global_costmap"] }], + ); + expect(relay.subs()).toEqual(["odom"]); + }); + + it("stores a costmap frame that arrives while subs are still being sent", async () => { + // The bridge replays the cached grid the moment a sub lands, so the frame + // can beat the control loop's continuation. The hook injects it + // synchronously at the sub and then holds the control writer for one + // macrotask, letting the whole uni-stream ingest chain drain first: with + // adoption after the sub loop, #ingest would drop it as manifest-less. + const { relay, handle } = start(); + relay.push({ t: "welcome", v: PROTOCOL_VERSION }); + relay.push({ t: "robots", robots: [ROBOT_A] }); + await until(() => relay.watches("a") === 1, "watch"); + + relay.onMsg = (msg) => { + if (msg.t !== "sub" || msg.ch !== "global_costmap") return; + relay.pushRaw(1, new Uint8Array([1, 2, 3]), "global_costmap", { + w: 2, + h: 2, + res: 0.5, + origin: [0, 0, 0], + }); + return new Promise((resolve) => setTimeout(resolve, 0)); + }; + relay.push({ + t: "manifest", + robotId: "a", + channels: [ + spec(), + spec({ ch: "global_costmap", encoding: "costmap.zlib.v1", delivery: "latest" }), + ], + panels: [{ id: "map", kind: "map2d", channels: ["global_costmap", "odom"] }], + }); + + await until(() => handle.channels.get("global_costmap") !== null, "stored costmap"); + const slot = handle.channels.get("global_costmap")!; + expect((slot.value as CostmapValue).w).toBe(2); + expect(slot.seq).toBe(1); + }); + it("counts a corrupt jpeg frame as a decode error instead of storing it", async () => { const { relay, handle } = start(); await goLive( diff --git a/web/cockpit/src/session/session.ts b/web/cockpit/src/session/session.ts index 3b1e03942f..a9efb38d79 100644 --- a/web/cockpit/src/session/session.ts +++ b/web/cockpit/src/session/session.ts @@ -65,7 +65,7 @@ export function pickAutoWatch(robots: RobotInfo[]): RobotInfo | null { // Encodings whose subscription costs real encode CPU and bandwidth; // subscribed only when a panel this build can render binds them. T7 moves // all subscription decisions to panels. -const PANEL_ONLY_ENCODINGS = new Set(["jpeg.v1"]); +const PANEL_ONLY_ENCODINGS = new Set(["jpeg.v1", "costmap.zlib.v1"]); /** True when this build can put the channel to use: it has a decoder, and a * panel-only encoding is additionally bound by a renderable panel. */ @@ -167,10 +167,13 @@ class Session { break; } this.status.update({ lastError: null }); + // Adopt before subscribing: a sub can trigger an immediate + // frame (the bridge replays the cached costmap), and #ingest + // drops everything while no manifest is adopted. + this.#applyManifest(manifest); for (const spec of subscribableChannels(manifest.channels, manifest.panels)) { await send({ t: "sub", ch: spec.ch }); } - this.#applyManifest(manifest); break; } case "error": { diff --git a/web/shared/fixtures/costmap_frames.json b/web/shared/fixtures/costmap_frames.json new file mode 100644 index 0000000000..e9ffa3d305 --- /dev/null +++ b/web/shared/fixtures/costmap_frames.json @@ -0,0 +1,49 @@ +{ + "vectors": [ + { + "name": "small_map", + "meta": { + "w": 5, + "h": 4, + "res": 0.05, + "origin": [ + -1.25, + 2.5, + 0.0 + ] + }, + "grid_b64": "//8AAAD/AAEyAAAAY2QAAP//AGQ=", + "payload_b64": "eJz7/5+BgeE/A6MRkEpOATL/M6QAAEV0Blo=" + }, + { + "name": "with_yaw", + "meta": { + "w": 3, + "h": 3, + "res": 0.1, + "origin": [ + 1.5, + -0.75, + 0.25 + ] + }, + "grid_b64": "AGT/ADIA/wAZ", + "payload_b64": "eJxjSPnPYMTwn0ESAA4yAq4=" + }, + { + "name": "single_row", + "meta": { + "w": 6, + "h": 1, + "res": 0.25, + "origin": [ + -0.5, + 0.125, + 0.0 + ] + }, + "grid_b64": "AP8BY2QA", + "payload_b64": "eJxj+M+YnMIAAAb2Acg=" + } + ] +} diff --git a/web/shared/fixtures/gen.ts b/web/shared/fixtures/gen.ts index e7fca24b62..3e8dcc48f0 100644 --- a/web/shared/fixtures/gen.ts +++ b/web/shared/fixtures/gen.ts @@ -8,6 +8,10 @@ // and at least one non-ASCII string (pins ensure_ascii=False on the Python // side). Message literals below are written in canonical field order - the // same order the Python dataclasses declare. +// +// costmap_frames.json is NOT written here: its payloads are the Python +// encoder's zlib bytes (CompressionStream output differs byte-wise), so it is +// generated by `uv run python -m dimos.web.relay_bridge.gen_costmap_fixtures`. import { encodeControlFrame, @@ -101,6 +105,12 @@ const dataFrames: Record = // Python mirror against them. const chOdom = { ch: "odom", encoding: "pose.json.v1", delivery: "reliable", maxHz: 20.5 }; const chImage = { ch: "color_image", encoding: "jpeg.v1", delivery: "latest", maxHz: 15.5 }; +const chCostmap = { + ch: "global_costmap", + encoding: "costmap.zlib.v1", + delivery: "latest", + maxHz: 5.5, +}; const longId = "x".repeat(65); const manifestCases: Record = { channels_only: { channels: [chImage, chOdom] }, @@ -168,6 +178,34 @@ const manifestCases: Record = { channels: [{ ...chImage, delivery: "reliable" }], panels: [{ id: "cam", kind: "video", channels: ["color_image"] }], }, + map2d_panel_full: { + channels: [chCostmap, chOdom], + panels: [{ id: "map", kind: "map2d", channels: ["global_costmap", "odom"] }], + }, + map2d_panel_no_pose: { + channels: [chCostmap], + panels: [{ id: "map", kind: "map2d", channels: ["global_costmap"] }], + }, + map2d_panel_no_channel: { + channels: [chCostmap], + panels: [{ id: "map", kind: "map2d", channels: [] }], + }, + map2d_panel_three_channels: { + channels: [chCostmap, chOdom, chImage], + panels: [{ id: "map", kind: "map2d", channels: ["global_costmap", "odom", "color_image"] }], + }, + map2d_panel_wrong_encoding: { + channels: [chOdom], + panels: [{ id: "map", kind: "map2d", channels: ["odom"] }], + }, + map2d_panel_wrong_delivery: { + channels: [{ ...chCostmap, delivery: "reliable" }], + panels: [{ id: "map", kind: "map2d", channels: ["global_costmap"] }], + }, + map2d_panel_bad_pose: { + channels: [chCostmap, chImage], + panels: [{ id: "map", kind: "map2d", channels: ["global_costmap", "color_image"] }], + }, layout_not_list: { channels: [chOdom], layout: "row" }, layout_not_strings: { channels: [chOdom], layout: [1.5] }, layout_unknown_panel: { channels: [chOdom], layout: ["ghost"] }, diff --git a/web/shared/fixtures/manifests.json b/web/shared/fixtures/manifests.json index d8c6c1f10d..a7e6c6a8b7 100644 --- a/web/shared/fixtures/manifests.json +++ b/web/shared/fixtures/manifests.json @@ -612,6 +612,238 @@ }, "error": "invalid_video_panel" }, + { + "name": "map2d_panel_full", + "data": { + "channels": [ + { + "ch": "global_costmap", + "encoding": "costmap.zlib.v1", + "delivery": "latest", + "maxHz": 5.5 + }, + { + "ch": "odom", + "encoding": "pose.json.v1", + "delivery": "reliable", + "maxHz": 20.5 + } + ], + "panels": [ + { + "id": "map", + "kind": "map2d", + "channels": [ + "global_costmap", + "odom" + ] + } + ] + }, + "manifest": { + "channels": [ + { + "ch": "global_costmap", + "encoding": "costmap.zlib.v1", + "delivery": "latest", + "maxHz": 5.5 + }, + { + "ch": "odom", + "encoding": "pose.json.v1", + "delivery": "reliable", + "maxHz": 20.5 + } + ], + "panels": [ + { + "id": "map", + "kind": "map2d", + "channels": [ + "global_costmap", + "odom" + ] + } + ], + "layout": [] + } + }, + { + "name": "map2d_panel_no_pose", + "data": { + "channels": [ + { + "ch": "global_costmap", + "encoding": "costmap.zlib.v1", + "delivery": "latest", + "maxHz": 5.5 + } + ], + "panels": [ + { + "id": "map", + "kind": "map2d", + "channels": [ + "global_costmap" + ] + } + ] + }, + "manifest": { + "channels": [ + { + "ch": "global_costmap", + "encoding": "costmap.zlib.v1", + "delivery": "latest", + "maxHz": 5.5 + } + ], + "panels": [ + { + "id": "map", + "kind": "map2d", + "channels": [ + "global_costmap" + ] + } + ], + "layout": [] + } + }, + { + "name": "map2d_panel_no_channel", + "data": { + "channels": [ + { + "ch": "global_costmap", + "encoding": "costmap.zlib.v1", + "delivery": "latest", + "maxHz": 5.5 + } + ], + "panels": [ + { + "id": "map", + "kind": "map2d", + "channels": [] + } + ] + }, + "error": "invalid_map2d_panel" + }, + { + "name": "map2d_panel_three_channels", + "data": { + "channels": [ + { + "ch": "global_costmap", + "encoding": "costmap.zlib.v1", + "delivery": "latest", + "maxHz": 5.5 + }, + { + "ch": "odom", + "encoding": "pose.json.v1", + "delivery": "reliable", + "maxHz": 20.5 + }, + { + "ch": "color_image", + "encoding": "jpeg.v1", + "delivery": "latest", + "maxHz": 15.5 + } + ], + "panels": [ + { + "id": "map", + "kind": "map2d", + "channels": [ + "global_costmap", + "odom", + "color_image" + ] + } + ] + }, + "error": "invalid_map2d_panel" + }, + { + "name": "map2d_panel_wrong_encoding", + "data": { + "channels": [ + { + "ch": "odom", + "encoding": "pose.json.v1", + "delivery": "reliable", + "maxHz": 20.5 + } + ], + "panels": [ + { + "id": "map", + "kind": "map2d", + "channels": [ + "odom" + ] + } + ] + }, + "error": "invalid_map2d_panel" + }, + { + "name": "map2d_panel_wrong_delivery", + "data": { + "channels": [ + { + "ch": "global_costmap", + "encoding": "costmap.zlib.v1", + "delivery": "reliable", + "maxHz": 5.5 + } + ], + "panels": [ + { + "id": "map", + "kind": "map2d", + "channels": [ + "global_costmap" + ] + } + ] + }, + "error": "invalid_map2d_panel" + }, + { + "name": "map2d_panel_bad_pose", + "data": { + "channels": [ + { + "ch": "global_costmap", + "encoding": "costmap.zlib.v1", + "delivery": "latest", + "maxHz": 5.5 + }, + { + "ch": "color_image", + "encoding": "jpeg.v1", + "delivery": "latest", + "maxHz": 15.5 + } + ], + "panels": [ + { + "id": "map", + "kind": "map2d", + "channels": [ + "global_costmap", + "color_image" + ] + } + ] + }, + "error": "invalid_map2d_panel" + }, { "name": "layout_not_list", "data": { diff --git a/web/shared/manifest.ts b/web/shared/manifest.ts index 43a82daad8..06f4ca2a6f 100644 --- a/web/shared/manifest.ts +++ b/web/shared/manifest.ts @@ -5,8 +5,9 @@ // // The transport (protocol.ts) 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 panel-id order, not a tree). +// that resolve, and kind-specific panel rules (video, map2d). Panels and +// layout are minimal until T7 (the layout is a flat panel-id order, not a +// tree). export type Delivery = "latest" | "reliable"; @@ -160,6 +161,30 @@ export function parseManifest(value: unknown): Manifest { ); } } + if (panel.kind === "map2d") { + // channels[0] is the costmap; channels[1] (optional) the pose overlay. + if (panel.channels.length !== 1 && panel.channels.length !== 2) { + throw new ManifestError( + "invalid_map2d_panel", + `map2d panel ${panel.id} must bind one or two channels`, + ); + } + const costmap = chIds.get(panel.channels[0])!; + if (costmap.encoding !== "costmap.zlib.v1" || costmap.delivery !== "latest") { + throw new ManifestError( + "invalid_map2d_panel", + `map2d panel ${panel.id} needs a costmap.zlib.v1 latest channel first`, + ); + } + if ( + panel.channels.length === 2 && chIds.get(panel.channels[1])!.encoding !== "pose.json.v1" + ) { + throw new ManifestError( + "invalid_map2d_panel", + `map2d panel ${panel.id} pose channel must be pose.json.v1`, + ); + } + } } for (const id of layout) {