diff --git a/.github/workflows/message-codegen.yml b/.github/workflows/message-codegen.yml index 987ba97880..00ab0db96b 100644 --- a/.github/workflows/message-codegen.yml +++ b/.github/workflows/message-codegen.yml @@ -215,3 +215,9 @@ jobs: MYPYPATH="$PWD/build/message-source/python" .native-review/bin/python -m mypy --follow-imports=silent dimos/perception/depth2depth_cloud/module.py cargo test --locked --manifest-path dimos/perception/depth2depth_cloud/rust/Cargo.toml -j 2 --lib cargo test --locked --manifest-path native/rust/dimos-module/Cargo.toml -j 2 pointcloud::tests + - name: Verify generated occupancy values and retained numeric helpers + if: runner.os == 'Linux' + run: | + .native-review/bin/python -I -c 'import dimos.msgs.occupancy as values; import pytest; import sys; assert "site-packages" in str(values.__file__); raise SystemExit(pytest.main(sys.argv[1:]))' --noconftest --import-mode=importlib -o addopts='' -q "$PWD/dimos/msgs/test_occupancy.py" + .native-review/bin/python -I -c 'import dimos.msgs.nav_msgs.OccupancyGrid; import pytest; import sys; raise SystemExit(pytest.main(sys.argv[1:]))' --noconftest --import-mode=importlib -o addopts='' -q "$PWD/dimos/msgs/nav_msgs/test_OccupancyGrid.py" -k block_max_reduce + MYPYPATH="$PWD/build/message-source/python" .native-review/bin/python -m mypy --follow-imports=silent dimos/msgs/occupancy.py diff --git a/dimos/models/segmentation/edge_tam.py b/dimos/models/segmentation/edge_tam.py index e240a6529d..b65977283c 100644 --- a/dimos/models/segmentation/edge_tam.py +++ b/dimos/models/segmentation/edge_tam.py @@ -20,7 +20,6 @@ import tempfile from typing import TYPE_CHECKING, Any, Protocol, TypedDict -from dimos.messages.sensor_msgs.msg import Image from hydra.utils import instantiate import numpy as np from numpy.typing import NDArray @@ -28,6 +27,7 @@ from PIL import Image as PILImage import torch +from dimos.messages.sensor_msgs.msg import Image from dimos.models.base import default_torch_device from dimos.msgs.image import image_to_bgr, image_to_jpeg from dimos.perception.detection.detectors.base import Detector diff --git a/dimos/models/segmentation/test_yoloe.py b/dimos/models/segmentation/test_yoloe.py index 345fc79ac1..5d2efaffa5 100644 --- a/dimos/models/segmentation/test_yoloe.py +++ b/dimos/models/segmentation/test_yoloe.py @@ -16,11 +16,11 @@ from typing import Any from unittest.mock import MagicMock -from dimos.messages.sensor_msgs.msg import Image -from dimos.messages.std_msgs.msg import Header import numpy as np from ultralytics.models.yolo.yoloe import YOLOEVPSegPredictor +from dimos.messages.sensor_msgs.msg import Image +from dimos.messages.std_msgs.msg import Header from dimos.models.segmentation.yoloe import YoloeBoxSegmenter from dimos.msgs.image import image_from_array, image_to_bgr from dimos.msgs.time import time_from_seconds, to_seconds diff --git a/dimos/models/segmentation/yoloe.py b/dimos/models/segmentation/yoloe.py index d19ecc1570..fd075227d2 100644 --- a/dimos/models/segmentation/yoloe.py +++ b/dimos/models/segmentation/yoloe.py @@ -19,11 +19,11 @@ from dataclasses import replace from typing import Protocol -from dimos.messages.sensor_msgs.msg import Image import numpy as np from numpy.typing import NDArray from ultralytics.models.yolo.yoloe import YOLOEVPSegPredictor +from dimos.messages.sensor_msgs.msg import Image from dimos.msgs.image import image_to_bgr from dimos.perception.detection.detectors.yoloe import Yoloe2DDetector, YoloePromptMode from dimos.perception.detection.type.detection2d.bbox import Bbox, Detection2DBBox diff --git a/dimos/msgs/nav_msgs/OccupancyGrid.py b/dimos/msgs/nav_msgs/OccupancyGrid.py index 7370cd6bf7..9aac765d58 100644 --- a/dimos/msgs/nav_msgs/OccupancyGrid.py +++ b/dimos/msgs/nav_msgs/OccupancyGrid.py @@ -14,7 +14,6 @@ from __future__ import annotations -from enum import IntEnum from functools import lru_cache import time from typing import TYPE_CHECKING, BinaryIO @@ -28,6 +27,7 @@ from dimos.msgs.geometry_msgs.Pose import Pose from dimos.msgs.geometry_msgs.Vector3 import Vector3, VectorLike +from dimos.msgs.occupancy import CostValues as CostValues, block_max_reduce as block_max_reduce from dimos.types.timestamped import Timestamped @@ -97,44 +97,6 @@ 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. - - These values follow the ROS nav_msgs/OccupancyGrid convention: - - 0: Free space - - 1-99: Occupied space with varying cost levels - - 100: Lethal obstacle (definitely occupied) - - -1: Unknown space - """ - - UNKNOWN = -1 # Unknown space - FREE = 0 # Free space - OCCUPIED = 100 # Occupied/lethal space - - class OccupancyGrid(Timestamped): """ Convenience wrapper for nav_msgs/OccupancyGrid with numpy array support. diff --git a/dimos/msgs/occupancy.py b/dimos/msgs/occupancy.py new file mode 100644 index 0000000000..aaf145b9db --- /dev/null +++ b/dimos/msgs/occupancy.py @@ -0,0 +1,158 @@ +# 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. + +"""Array operations on generated occupancy grids.""" + +from copy import deepcopy +from enum import IntEnum +import math +from pathlib import Path +from typing import Any + +import numpy as np +from numpy.typing import NDArray +from PIL import Image + +from dimos.messages.geometry_msgs.msg import Point, Pose, Quaternion +from dimos.messages.nav_msgs.msg import MapMetaData, OccupancyGrid +from dimos.messages.std_msgs.msg import Header +from dimos.msgs.geometry import pose_matrix + + +class CostValues(IntEnum): + """Standard cost values for occupancy grid cells. + + These values follow the ROS nav_msgs/OccupancyGrid convention: + - 0: Free space + - 1-99: Occupied space with varying cost levels + - 100: Lethal obstacle (definitely occupied) + - -1: Unknown space + """ + + UNKNOWN = -1 # Unknown space + FREE = 0 # Free space + OCCUPIED = 100 # Occupied/lethal space + + +def occupancy_view(msg: OccupancyGrid) -> NDArray[np.int8]: + """Borrow a read-only row-major occupancy grid, retaining its owner.""" + if len(msg.data) != msg.info.height * msg.info.width: + raise ValueError("occupancy data length does not match dimensions") + view = msg.data.view().reshape(msg.info.height, msg.info.width) + view.setflags(write=False) + return view + + +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 + + +def occupancy_extent(message: OccupancyGrid) -> tuple[float, float]: + """Return physical width and height, rejecting an invalid grid resolution.""" + info = message.info + if not math.isfinite(info.resolution) or info.resolution <= 0: + raise ValueError("OccupancyGrid resolution must be finite and positive") + return info.width * info.resolution, info.height * info.resolution + + +def world_to_grid(message: OccupancyGrid, point: Point) -> tuple[float, float]: + """Convert a world point to continuous grid coordinates, including origin rotation.""" + occupancy_extent(message) + matrix = pose_matrix(message.info.origin) + coordinates = np.array([point.x, point.y, point.z]) + if not np.isfinite(coordinates).all(): + raise ValueError("grid coordinates must be finite") + local = matrix[:3, :3].T @ (coordinates - matrix[:3, 3]) + if not np.isfinite(local).all(): + raise ValueError("grid coordinates must be finite") + return float(local[0] / message.info.resolution), float(local[1] / message.info.resolution) + + +def grid_to_world(message: OccupancyGrid, coordinates: tuple[float, float]) -> Point: + """Convert continuous grid coordinates to the grid plane in world coordinates.""" + occupancy_extent(message) + x, y = coordinates + if not math.isfinite(x) or not math.isfinite(y): + raise ValueError("grid coordinates must be finite") + result = pose_matrix(message.info.origin) @ np.array( + [x * message.info.resolution, y * message.info.resolution, 0.0, 1.0] + ) + return Point(x=float(result[0]), y=float(result[1]), z=float(result[2])) + + +def occupancy_from_file(path: Path, *, header: Header, resolution: float = 0.05) -> OccupancyGrid: + """Load a static .npy or grayscale PNG grid with explicit frame/time metadata. + + Cells must be ROS occupancy values (-1 unknown, 0..100 known); PNG 255 is + unknown. NumPy object/pickle loading is disabled. Row order is unchanged. + """ + if not math.isfinite(resolution) or resolution <= 0: + raise ValueError("OccupancyGrid resolution must be finite and positive") + cells: NDArray[Any] + if path.suffix == ".npy": + cells = np.load(path, allow_pickle=False) + elif path.suffix == ".png": + with Image.open(path) as image: + cells = np.asarray(image.convert("L")).astype(np.int16) + cells[cells == 255] = -1 + else: + raise ValueError(f"Unsupported occupancy file format: {path.suffix}") + return occupancy_from_array(cells, header=header, resolution=resolution) + + +def occupancy_from_array( + cells: NDArray[Any], *, header: Header, resolution: float, origin: Pose | None = None +) -> OccupancyGrid: + """Copy a 2D ROS occupancy array with explicit map metadata.""" + if not math.isfinite(resolution) or resolution <= 0: + raise ValueError("OccupancyGrid resolution must be finite and positive") + if cells.ndim != 2 or cells.dtype.kind not in "iu": + raise ValueError("Occupancy cells must be a 2D integer array") + if np.any((cells < -1) | (cells > 100)): + raise ValueError("Occupancy cells must be -1 or 0..100") + return OccupancyGrid( + header=deepcopy(header), + info=MapMetaData( + map_load_time=deepcopy(header.stamp), + resolution=resolution, + height=cells.shape[0], + width=cells.shape[1], + origin=deepcopy(origin) + if origin is not None + else Pose( + orientation=Quaternion(w=1, x=0.0, y=0.0, z=0.0), + position=Point(x=0.0, y=0.0, z=0.0), + ), + ), + data=np.asarray(cells.astype(np.int8).reshape(-1), dtype=np.int8), + ) diff --git a/dimos/msgs/test_occupancy.py b/dimos/msgs/test_occupancy.py new file mode 100644 index 0000000000..26b9a6a31a --- /dev/null +++ b/dimos/msgs/test_occupancy.py @@ -0,0 +1,245 @@ +# 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. + +import math + +from dimos_cdrgen.definitions import Definitions +from dimos_cdrgen.registry import decode as cdr_decode, encode as cdr_encode +from mcap.reader import make_reader +import numpy as np +from PIL import Image +import pytest + +from dimos.messages.builtin_interfaces.msg import Time +from dimos.messages.geometry_msgs.msg import Point, Pose, Quaternion +from dimos.messages.nav_msgs.msg import MapMetaData, OccupancyGrid +from dimos.messages.std_msgs.msg import Header +from dimos.msgs.geometry import quaternion_from_euler +from dimos.msgs.occupancy import ( + grid_to_world, + occupancy_extent, + occupancy_from_array, + occupancy_from_file, + occupancy_view, + world_to_grid, +) +from dimos.protocol.cdr_mcap import CdrMcapWriter + + +def test_occupancy_cells_keep_ros_row_order_and_do_not_alias_message(): + message = OccupancyGrid( + info=MapMetaData( + width=3, + height=2, + resolution=0.1, + map_load_time=Time(sec=0, nanosec=0), + origin=Pose( + position=Point(x=0.0, y=0.0, z=0.0), + orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), + ), + data=np.array([-1, 0, 100, 25, 50, 75], dtype=np.int8), + header=Header(stamp=Time(sec=0, nanosec=0), frame_id=""), + ) + decoded = cdr_decode(cdr_encode(message), OccupancyGrid) + values = occupancy_view(decoded) + np.testing.assert_array_equal(values, [[-1, 0, 100], [25, 50, 75]]) + with pytest.raises(ValueError, match="read-only"): + values[0, 0] = 0 + copied = values.copy() + copied[0, 0] = 0 + assert decoded.data[0] == -1 + + +def test_occupancy_rejects_dimensions_that_do_not_match_payload(): + message = OccupancyGrid( + info=MapMetaData( + width=2, + height=2, + resolution=1.0, + map_load_time=Time(sec=0, nanosec=0), + origin=Pose( + position=Point(x=0.0, y=0.0, z=0.0), + orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), + ), + data=np.array([0], dtype=np.int8), + header=Header(stamp=Time(sec=0, nanosec=0), frame_id=""), + ) + with pytest.raises(ValueError, match="data length"): + occupancy_view(message) + + +@pytest.mark.parametrize("resolution", [0.0, -0.1, math.nan, math.inf]) +def test_occupancy_rejects_invalid_resolution_for_nonempty_grid(resolution): + message = OccupancyGrid( + info=MapMetaData( + width=1, + height=1, + resolution=resolution, + map_load_time=Time(sec=0, nanosec=0), + origin=Pose( + position=Point(x=0.0, y=0.0, z=0.0), + orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), + ), + data=np.array([0], dtype=np.int8), + header=Header(stamp=Time(sec=0, nanosec=0), frame_id=""), + ) + with pytest.raises(ValueError, match="resolution"): + occupancy_extent(message) + + +def test_empty_default_occupancy_grid_has_no_cells(): + assert occupancy_view( + OccupancyGrid( + header=Header(stamp=Time(sec=0, nanosec=0), frame_id=""), + info=MapMetaData( + map_load_time=Time(sec=0, nanosec=0), + resolution=0.0, + width=0, + height=0, + origin=Pose( + position=Point(x=0.0, y=0.0, z=0.0), + orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), + ), + data=np.array([], dtype=np.int8), + ) + ).shape == (0, 0) + + +@pytest.mark.parametrize("suffix", [".npy", ".png"]) +def test_static_occupancy_loader_preserves_cells_header_and_identity(tmp_path, suffix): + cells = np.array([[-1, 0, 100], [25, 50, 75]], dtype=np.int8) + path = tmp_path / ("grid" + suffix) + if suffix == ".npy": + np.save(path, cells) + else: + Image.fromarray(cells.astype(np.uint8)).save(path) + header = Header(frame_id="map", stamp=Time(sec=123, nanosec=456)) + output = cdr_decode( + cdr_encode(occupancy_from_file(path, header=header, resolution=0.2)), + OccupancyGrid, + ) + np.testing.assert_array_equal(occupancy_view(output), cells) + assert output.header == header + assert output.info.map_load_time == header.stamp + assert output.info.origin.orientation.w == 1 + assert output.info.resolution == float(np.float32(0.2)) + + +@pytest.mark.parametrize( + "cells", + [np.zeros((2, 2), dtype=float), np.zeros(3, dtype=int), np.array([[101]]), np.array([[-2]])], +) +def test_static_occupancy_loader_rejects_invalid_cells(tmp_path, cells): + path = tmp_path / "grid.npy" + np.save(path, cells) + with pytest.raises(ValueError): + occupancy_from_file(path, header=Header(stamp=Time(sec=0, nanosec=0), frame_id="")) + + +@pytest.mark.parametrize("ns", [0, -1, 1700000000123456789]) +def test_array_constructor_copies_data_and_metadata_without_rounding_source_time(ns): + seconds, nanoseconds = divmod(ns, 1_000_000_000) + cells = np.array([[-1, 0], [100, 50]], dtype=np.int8) + header = Header(frame_id="map", stamp=Time(sec=seconds, nanosec=nanoseconds)) + origin = Pose( + position=Point(x=10, y=20, z=3), orientation=quaternion_from_euler(0, 0, math.pi / 2) + ) + + message = occupancy_from_array(cells, header=header, resolution=0.5, origin=origin) + header.stamp.sec = 77 + cells[0, 0] = 44 + origin.position.x = 99 + actual = cdr_decode(cdr_encode(message), OccupancyGrid) + + assert actual.header.stamp.sec == seconds + assert actual.header.stamp.nanosec == nanoseconds + assert actual.info.map_load_time == actual.header.stamp + assert actual.info.origin.position.x == 10 + np.testing.assert_array_equal(occupancy_view(actual), [[-1, 0], [100, 50]]) + + +def test_coordinate_helpers_account_for_rotated_origin_and_grid_plane(): + origin = Pose( + position=Point(x=10, y=20, z=3), orientation=quaternion_from_euler(0, 0, math.pi / 2) + ) + message = occupancy_from_array( + np.zeros((4, 4), dtype=np.int8), + header=Header(frame_id="map", stamp=Time(sec=0, nanosec=0)), + resolution=0.5, + origin=origin, + ) + + point = grid_to_world(message, (2, 3)) + + assert (point.x, point.y, point.z) == pytest.approx((8.5, 21, 3)) + assert world_to_grid(message, point) == pytest.approx((2, 3)) + assert occupancy_extent(message) == (2, 2) + + +@pytest.mark.parametrize("value", [math.nan, math.inf, -math.inf]) +def test_coordinate_helpers_reject_nonfinite_coordinates(value): + message = occupancy_from_array( + np.zeros((1, 1), dtype=np.int8), + header=Header(frame_id="map", stamp=Time(sec=0, nanosec=0)), + resolution=1, + ) + with pytest.raises(ValueError, match="finite"): + grid_to_world(message, (value, 0)) + with pytest.raises(ValueError, match="finite"): + world_to_grid(message, Point(x=value, y=0, z=0)) + + +def test_numpy_loader_never_unpickles_objects(tmp_path): + path = tmp_path / "objects.npy" + np.save(path, np.array([[{}]], dtype=object), allow_pickle=True) + with pytest.raises(ValueError, match="allow_pickle=False"): + occupancy_from_file(path, header=Header(frame_id="map", stamp=Time(sec=0, nanosec=0))) + + +def test_mcap_contains_generated_grid_and_full_viewer_schema_closure(tmp_path): + message = occupancy_from_array( + np.array([[-1, 0, 100]], dtype=np.int8), + header=Header(frame_id="map", stamp=Time(sec=1700000000, nanosec=123456789)), + resolution=0.25, + ) + name = "nav_msgs/msg/OccupancyGrid" + path = tmp_path / "map.mcap" + with CdrMcapWriter(path) as writer: + writer.write( + "/map", + cdr_encode(message), + schema_name=name, + schema=Definitions([], installed=True).schema(name), + log_time_ns=1700000000123456790, + publish_time_ns=1700000000123456789, + ) + + with path.open("rb") as source: + records = list(make_reader(source).iter_messages()) + + assert len(records) == 1 + schema, channel, record = records[0] + assert channel.message_encoding == "cdr" + assert schema.encoding == "ros2msg" + assert schema.name == name + assert "MSG: geometry_msgs/Pose" in schema.data.decode() + assert "MSG: builtin_interfaces/Time" in schema.data.decode() + assert record.publish_time == 1700000000123456789 + decoded = cdr_decode(record.data, OccupancyGrid) + assert decoded.header == message.header + np.testing.assert_array_equal(occupancy_view(decoded), [[-1, 0, 100]]) diff --git a/docs/capabilities/navigation/deep_dive.md b/docs/capabilities/navigation/deep_dive.md index 1b702a98f0..66577d36bd 100644 --- a/docs/capabilities/navigation/deep_dive.md +++ b/docs/capabilities/navigation/deep_dive.md @@ -80,7 +80,7 @@ Live column-carving has no loop closure. We trust Go2 odometry, which is stable The [`CostMapper`](/dimos/mapping/costmapper.py) converts the 3D voxel map into a 2D occupancy grid. The default algorithm (`height_cost`) maps rate of change of Z, with some smoothing. -Algorithm settings live in [`occupancy.py`](/dimos/mapping/pointclouds/occupancy.py) and can be configured per robot. +Algorithm settings live in [`mapping/pointclouds/occupancy.py`](/dimos/mapping/pointclouds/occupancy.py) and can be configured per robot. #### Configuration