From 2cd3a5f0469616a4a9c72d5acd438c9ef42b431d Mon Sep 17 00:00:00 2001 From: cc Date: Mon, 19 Oct 2026 23:02:41 -0700 Subject: [PATCH 1/2] refactor: render generated CDR occupancy maps and path masks --- .github/workflows/message-codegen.yml | 5 + dimos/mapping/occupancy/operations.py | 15 +- dimos/mapping/occupancy/path_mask.py | 28 +-- .../occupancy/test_generated_rendering.py | 189 ++++++++++++++++++ dimos/mapping/occupancy/visualizations.py | 92 ++++++--- dimos/mapping/occupancy/visualize_path.py | 64 ++++-- 6 files changed, 329 insertions(+), 64 deletions(-) create mode 100644 dimos/mapping/occupancy/test_generated_rendering.py diff --git a/.github/workflows/message-codegen.yml b/.github/workflows/message-codegen.yml index e912c677b33..a144ff85dfb 100644 --- a/.github/workflows/message-codegen.yml +++ b/.github/workflows/message-codegen.yml @@ -267,3 +267,8 @@ jobs: run: | .native-review/bin/python -I -c 'from dimos.mapping.occupancy import path_resampling; import pytest; import sys; assert "site-packages" in str(path_resampling.__file__); raise SystemExit(pytest.main(sys.argv[1:]))' --noconftest --import-mode=importlib -o addopts='' -q "$PWD/dimos/mapping/occupancy/test_generated_path_resampling.py" MYPYPATH="$PWD/build/message-source/python" .native-review/bin/python -m mypy --follow-imports=silent dimos/mapping/occupancy/path_resampling.py + - name: Verify generated occupancy/path rendering and safety masks + if: runner.os == 'Linux' + run: | + .native-review/bin/python -I -c 'from dimos.mapping.occupancy import operations, path_mask, visualizations, visualize_path; import pytest; import sys; assert all("site-packages" in str(value.__file__) for value in (operations, path_mask, visualizations, visualize_path)); raise SystemExit(pytest.main(sys.argv[1:]))' --noconftest --import-mode=importlib -o addopts='' -q "$PWD/dimos/mapping/occupancy/test_generated_rendering.py" + MYPYPATH="$PWD/build/message-source/python" .native-review/bin/python -m mypy --follow-imports=silent dimos/mapping/occupancy/operations.py dimos/mapping/occupancy/path_mask.py dimos/mapping/occupancy/visualizations.py dimos/mapping/occupancy/visualize_path.py diff --git a/dimos/mapping/occupancy/operations.py b/dimos/mapping/occupancy/operations.py index 6b9107f9f0c..285239acf46 100644 --- a/dimos/mapping/occupancy/operations.py +++ b/dimos/mapping/occupancy/operations.py @@ -19,19 +19,30 @@ from numpy.typing import NDArray from scipy import ndimage +from dimos.messages.geometry_msgs.msg import Point from dimos.messages.nav_msgs.msg import OccupancyGrid as CdrOccupancyGrid +from dimos.msgs.geometry_msgs.Vector3 import Vector3 from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid -from dimos.msgs.occupancy import CostValues, occupancy_view +from dimos.msgs.occupancy import CostValues, occupancy_view, world_to_grid _GridT = TypeVar("_GridT", OccupancyGrid, CdrOccupancyGrid) -def _grid_values(message: _GridT) -> NDArray[np.int8]: +def _grid_values(message: OccupancyGrid | CdrOccupancyGrid) -> NDArray[np.int8]: if isinstance(message, CdrOccupancyGrid): return occupancy_view(message) return message.grid +def _grid_coordinates( + message: OccupancyGrid | CdrOccupancyGrid, point: Point | Vector3 +) -> tuple[float, float]: + if isinstance(message, CdrOccupancyGrid): + return world_to_grid(message, Point(x=point.x, y=point.y, z=point.z)) + result = message.world_to_grid((point.x, point.y, point.z)) + return result.x, result.y + + def _with_grid(message: _GridT, cells: NDArray[np.int8]) -> _GridT: if isinstance(message, CdrOccupancyGrid): if cells.shape != (message.info.height, message.info.width): diff --git a/dimos/mapping/occupancy/path_mask.py b/dimos/mapping/occupancy/path_mask.py index b71372bbf62..88f9ee4f8b0 100644 --- a/dimos/mapping/occupancy/path_mask.py +++ b/dimos/mapping/occupancy/path_mask.py @@ -15,13 +15,16 @@ import numpy as np from numpy.typing import NDArray -from dimos.msgs.nav_msgs.OccupancyGrid import CostValues, OccupancyGrid +from dimos.mapping.occupancy.operations import _grid_coordinates, _grid_values, _GridT +from dimos.messages.geometry_msgs.msg import PoseStamped as CdrPoseStamped +from dimos.messages.nav_msgs.msg import Path as CdrPath from dimos.msgs.nav_msgs.Path import Path +from dimos.msgs.occupancy import CostValues def make_path_mask( - occupancy_grid: OccupancyGrid, - path: Path, + occupancy_grid: _GridT, + path: Path | CdrPath, robot_width: float, pose_index: int = 0, max_length: float = float("inf"), @@ -44,9 +47,9 @@ def make_path_mask( """ import cv2 - mask = np.zeros((occupancy_grid.height, occupancy_grid.width), dtype=np.uint8) + mask = np.zeros((occupancy_grid.info.height, occupancy_grid.info.width), dtype=np.uint8) - line_width_pixels = max(1, int(robot_width / occupancy_grid.resolution)) + line_width_pixels = max(1, int(robot_width / occupancy_grid.info.resolution)) poses = path.poses if len(poses) < pose_index + 2: @@ -55,8 +58,9 @@ def make_path_mask( # Draw lines between consecutive points cumulative_length = 0.0 for i in range(pose_index, len(poses) - 1): - pos1 = poses[i].position - pos2 = poses[i + 1].position + first, second = poses[i], poses[i + 1] + pos1 = first.pose.position if isinstance(first, CdrPoseStamped) else first.position + pos2 = second.pose.position if isinstance(second, CdrPoseStamped) else second.position segment_length = np.sqrt( (pos2.x - pos1.x) ** 2 + (pos2.y - pos1.y) ** 2 + (pos2.z - pos1.z) ** 2 @@ -67,11 +71,11 @@ def make_path_mask( cumulative_length += segment_length - grid_pt1 = occupancy_grid.world_to_grid(pos1) - grid_pt2 = occupancy_grid.world_to_grid(pos2) + grid_pt1 = _grid_coordinates(occupancy_grid, pos1) + grid_pt2 = _grid_coordinates(occupancy_grid, pos2) - pt1 = (round(grid_pt1.x), round(grid_pt1.y)) - pt2 = (round(grid_pt2.x), round(grid_pt2.y)) + pt1 = (round(grid_pt1[0]), round(grid_pt1[1])) + pt2 = (round(grid_pt2[0]), round(grid_pt2[1])) cv2.line(mask, pt1, pt2, (255.0,), thickness=line_width_pixels) @@ -82,7 +86,7 @@ def make_path_mask( if total_points == 0: return bool_mask - occupied_mask = occupancy_grid.grid >= CostValues.OCCUPIED + occupied_mask = _grid_values(occupancy_grid) >= CostValues.OCCUPIED occupied_in_path = bool_mask & occupied_mask occupied_count = np.sum(occupied_in_path) diff --git a/dimos/mapping/occupancy/test_generated_rendering.py b/dimos/mapping/occupancy/test_generated_rendering.py new file mode 100644 index 00000000000..e2105631c6c --- /dev/null +++ b/dimos/mapping/occupancy/test_generated_rendering.py @@ -0,0 +1,189 @@ +# Copyright 2025-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. + +from copy import deepcopy +import math + +from dimos_cdrgen.registry import decode, encode +import numpy as np +import pytest + +from dimos.mapping.occupancy.path_mask import make_path_mask +from dimos.mapping.occupancy.visualizations import generate_rgba_texture, visualize_occupancy_grid +from dimos.mapping.occupancy.visualize_path import visualize_path +from dimos.messages.geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion +from dimos.messages.nav_msgs.msg import Path +from dimos.messages.sensor_msgs.msg import Image +from dimos.messages.std_msgs.msg import Header +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped as LegacyStamped +from dimos.msgs.image import image_view +from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid as LegacyGrid +from dimos.msgs.nav_msgs.Path import Path as LegacyPath +from dimos.msgs.occupancy import grid_to_world, occupancy_from_array +from dimos.msgs.time import time_from_nanoseconds + + +def make_values(stamp, rotated=False): + cells = np.zeros((9, 9), dtype=np.int8) + cells[0, 0] = -1 + cells[7:9, 7:9] = 100 + cells[5, 5] = 50 + grid = occupancy_from_array( + cells, + header=Header(frame_id="map", stamp=time_from_nanoseconds(stamp)), + resolution=0.25, + origin=Pose( + position=Point(x=0, y=0, z=0), + orientation=Quaternion( + x=0, + y=0, + z=math.sin(math.pi / 4) if rotated else 0, + w=math.cos(math.pi / 4) if rotated else 1, + ), + ), + ) + path = Path( + header=deepcopy(grid.header), + poses=[ + PoseStamped( + header=deepcopy(grid.header), + pose=Pose( + position=grid_to_world(grid, coordinate), + orientation=Quaternion(x=0, y=0, z=0, w=1), + ), + ) + for coordinate in [(1, 1), (4, 1), (4, 4)] + ], + ) + old_grid = LegacyGrid(cells.copy(), resolution=0.25, frame_id="map", ts=0) + old_path = LegacyPath( + frame_id="map", + poses=[ + LegacyStamped( + frame_id="map", position=[x * 0.25, y * 0.25, 0], orientation=[0, 0, 0, 1] + ) + for x, y in [(1, 1), (4, 1), (4, 4)] + ], + ) + return grid, path, old_grid, old_path + + +@pytest.mark.parametrize("palette", ["rainbow", "turbo"]) +@pytest.mark.parametrize("stamp", [0, -1, 1700000000123456789]) +def test_generated_map_pixels_match_existing_palette_and_path_overlay(palette, stamp): + grid, path, old_grid, old_path = make_values(stamp) + before = encode(grid) + + result = visualize_occupancy_grid(grid, palette, path) + expected = visualize_occupancy_grid(old_grid, palette, old_path) + + assert type(result) is Image and result.encoding == "bgr8" + assert result.header == grid.header + np.testing.assert_array_equal(image_view(result), expected.data) + decoded = decode(encode(result), Image) + np.testing.assert_array_equal(image_view(decoded), expected.data) + result.header.frame_id = "changed" + result.data[0] = 255 + assert encode(grid) == before + + +@pytest.mark.parametrize("stamp", [0, -1, 1700000000123456789]) +def test_generated_robot_footprints_match_existing_pixels_and_preserve_header(stamp): + grid, path, old_grid, old_path = make_values(stamp) + result = visualize_path(grid, path, 0.2, 0.4, scale=4) + expected = visualize_path(old_grid, old_path, 0.2, 0.4, scale=4) + assert result.header == grid.header and result.encoding == "bgr8" + assert image_view(result).shape == (36, 36, 3) + np.testing.assert_array_equal(image_view(result), expected.data) + + +@pytest.mark.parametrize( + "pose_index,max_length", [(0, float("inf")), (0, 0.8), (1, float("inf")), (2, 0)] +) +@pytest.mark.parametrize("rotated", [False, True]) +def test_generated_path_mask_keeps_existing_clipping_and_rotated_origin_coordinates( + pose_index, max_length, rotated +): + grid, path, old_grid, old_path = make_values(0, rotated) + result = make_path_mask(grid, path, 0.2, pose_index, max_length) + expected = make_path_mask(old_grid, old_path, 0.2, pose_index, max_length) + np.testing.assert_array_equal(result, expected) + + +@pytest.mark.parametrize("colormap", [None, "viridis"]) +@pytest.mark.parametrize("opacity", [0.0, 0.5, 1.0]) +@pytest.mark.parametrize("cost_range", [None, (0, 50)]) +def test_generated_rgba_texture_matches_existing_colormap_opacity_and_range( + colormap, opacity, cost_range +): + grid, _, old_grid, _ = make_values(-1) + result = generate_rgba_texture(grid, colormap, opacity, cost_range, background="#102030") + expected = generate_rgba_texture(old_grid, colormap, opacity, cost_range, background="#102030") + np.testing.assert_array_equal(result, expected) + np.testing.assert_array_equal(result[0, 0], [0, 0, 0, 0]) + + +def test_generated_default_rgba_values_follow_existing_foxglove_palette(): + grid = occupancy_from_array( + np.array([[-1, 0, 100]], dtype=np.int8), + resolution=1, + header=Header(frame_id="map", stamp=time_from_nanoseconds(0)), + ) + np.testing.assert_array_equal( + generate_rgba_texture(grid), [[[0, 0, 0, 0], [72, 73, 129, 255], [0, 0, 0, 255]]] + ) + + +@pytest.mark.parametrize("occupied_x", [1, 20]) +def test_generated_mask_removes_small_occupied_edge_fraction(occupied_x): + cells = np.zeros((5, 41), dtype=np.int8) + cells[2, occupied_x] = 100 + grid = occupancy_from_array( + cells, resolution=1, header=Header(frame_id="map", stamp=time_from_nanoseconds(0)) + ) + path = Path( + header=deepcopy(grid.header), + poses=[ + PoseStamped( + header=deepcopy(grid.header), + pose=Pose( + position=Point(x=x, y=2, z=0), orientation=Quaternion(x=0, y=0, z=0, w=1) + ), + ) + for x in [0, 40] + ], + ) + expected = np.zeros((5, 41), dtype=bool) + expected[2, :] = True + expected[2, occupied_x] = False + np.testing.assert_array_equal(make_path_mask(grid, path, 1), expected) + + +def test_generated_mask_rejects_unsafe_occupied_fraction(): + grid, path, _, _ = make_values(0) + cells = np.array(grid.data, copy=True).reshape(9, 9) + cells[1, 2] = 100 + grid.data = cells.reshape(-1) + with pytest.raises(ValueError, match="More than 5%"): + make_path_mask(grid, path, 0.2) + + +def test_generated_turbo_handles_unknown_int8_cells_with_exact_existing_colors(): + grid = occupancy_from_array( + np.array([[-1, 0, 100]], dtype=np.int8), + resolution=1, + header=Header(frame_id="map", stamp=time_from_nanoseconds(0)), + ) + image = visualize_occupancy_grid(grid, "turbo") + np.testing.assert_array_equal(image_view(image), [[[28, 24, 34], [27, 23, 35], [0, 0, 0]]]) diff --git a/dimos/mapping/occupancy/visualizations.py b/dimos/mapping/occupancy/visualizations.py index 560707ccae4..2fafc100390 100644 --- a/dimos/mapping/occupancy/visualizations.py +++ b/dimos/mapping/occupancy/visualizations.py @@ -12,13 +12,19 @@ # See the License for the specific language governing permissions and # limitations under the License. +from copy import deepcopy from functools import lru_cache -from typing import Literal, TypeAlias +from typing import Literal, TypeAlias, overload import matplotlib.pyplot as plt import numpy as np from numpy.typing import NDArray +from dimos.mapping.occupancy.operations import _grid_coordinates, _grid_values, _GridT +from dimos.messages.geometry_msgs.msg import PoseStamped as CdrPoseStamped +from dimos.messages.nav_msgs.msg import OccupancyGrid as CdrOccupancyGrid, Path as CdrPath +from dimos.messages.sensor_msgs.msg import Image as CdrImage +from dimos.msgs.image import image_from_array from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid from dimos.msgs.nav_msgs.Path import Path from dimos.msgs.sensor_msgs.Image import Image, ImageFormat @@ -26,38 +32,60 @@ Palette: TypeAlias = Literal["rainbow", "turbo"] +def _map_image( + grid: OccupancyGrid | CdrOccupancyGrid, pixels: NDArray[np.uint8] +) -> Image | CdrImage: + if isinstance(grid, CdrOccupancyGrid): + return image_from_array(pixels, encoding="bgr8", header=deepcopy(grid.header)) + return Image(data=pixels, format=ImageFormat.BGR, frame_id=grid.frame_id, ts=grid.ts) + + +@overload +def visualize_occupancy_grid( + occupancy_grid: CdrOccupancyGrid, palette: Palette, path: Path | CdrPath | None = None +) -> CdrImage: ... + + +@overload def visualize_occupancy_grid( - occupancy_grid: OccupancyGrid, palette: Palette, path: Path | None = None -) -> Image: + occupancy_grid: OccupancyGrid, palette: Palette, path: Path | CdrPath | None = None +) -> Image: ... + + +def visualize_occupancy_grid( + occupancy_grid: OccupancyGrid | CdrOccupancyGrid, + palette: Palette, + path: Path | CdrPath | None = None, +) -> Image | CdrImage: match palette: case "rainbow": - bgr_image = rainbow_image(occupancy_grid.grid) + bgr_image = rainbow_image(_grid_values(occupancy_grid)) case "turbo": - bgr_image = turbo_image(occupancy_grid.grid) + bgr_image = turbo_image(_grid_values(occupancy_grid)) case _: raise NotImplementedError() if path is not None and len(path.poses) > 0: _draw_path(occupancy_grid, bgr_image, path) - return Image( - data=bgr_image, - format=ImageFormat.BGR, - frame_id=occupancy_grid.frame_id, - ts=occupancy_grid.ts, - ) + return _map_image(occupancy_grid, bgr_image) -def _draw_path(occupancy_grid: OccupancyGrid, bgr_image: NDArray[np.uint8], path: Path) -> None: +def _draw_path( + occupancy_grid: OccupancyGrid | CdrOccupancyGrid, + bgr_image: NDArray[np.uint8], + path: Path | CdrPath, +) -> None: import cv2 points = [] for pose in path.poses: - grid_coord = occupancy_grid.world_to_grid([pose.x, pose.y, pose.z]) - pixel_x = int(grid_coord.x) - pixel_y = int(grid_coord.y) + position = pose.pose.position if isinstance(pose, CdrPoseStamped) else pose.position + x, y = _grid_coordinates(occupancy_grid, position) + pixel_x = int(x) + pixel_y = int(y) - if 0 <= pixel_x < occupancy_grid.width and 0 <= pixel_y < occupancy_grid.height: + if 0 <= pixel_x < occupancy_grid.info.width and 0 <= pixel_y < occupancy_grid.info.height: points.append((pixel_x, pixel_y)) if len(points) > 1: @@ -109,7 +137,8 @@ def turbo_image(grid: NDArray[np.int8]) -> NDArray[np.uint8]: # Map grid values to lookup indices # Values: -1 -> 255, 0-100 -> 0-100, clipped to valid range - lookup_indices = np.where(grid == -1, 255, np.clip(grid, 0, 100)).astype(np.uint8) + lookup_indices = np.clip(grid, 0, 100).astype(np.uint8) + lookup_indices[grid == -1] = 255 # Create BGR image using lookup table (vectorized operation) return color_lut[lookup_indices] @@ -140,7 +169,7 @@ def _interpolate_turbo(t: float) -> tuple[int, int, int]: def generate_rgba_texture( - grid: OccupancyGrid, + grid: _GridT, colormap: str | None = None, opacity: float = 1.0, cost_range: tuple[int, int] | None = None, @@ -159,6 +188,7 @@ def generate_rgba_texture( RGBA numpy array of shape (height, width, 4). Note: NOT flipped - caller handles orientation. """ + cells = _grid_values(grid) if background is not None: bg = background.lstrip("#") bg_rgb = np.array([int(bg[i : i + 2], 16) for i in (0, 2, 4)], dtype=np.float32) @@ -166,18 +196,18 @@ def generate_rgba_texture( bg_rgb = np.array([0, 0, 0], dtype=np.float32) if cost_range is not None: - in_range_mask = (grid.grid >= cost_range[0]) & (grid.grid <= cost_range[1]) + in_range_mask = (cells >= cost_range[0]) & (cells <= cost_range[1]) else: in_range_mask = None if colormap is not None: cmap = plt.get_cmap(colormap) - grid_float = grid.grid.astype(np.float32) + grid_float = cells.astype(np.float32) - vis = np.zeros((grid.height, grid.width, 4), dtype=np.uint8) + vis = np.zeros((grid.info.height, grid.info.width, 4), dtype=np.uint8) - free_mask = grid.grid == 0 - occupied_mask = grid.grid > 0 + free_mask = cells == 0 + occupied_mask = cells > 0 if np.any(free_mask): fg = np.array(cmap(0.0)[:3]) * 255 @@ -193,21 +223,21 @@ def generate_rgba_texture( vis[occupied_mask, :3] = blended.astype(np.uint8) vis[occupied_mask, 3] = 255 - unknown_mask = grid.grid == -1 + unknown_mask = cells == -1 vis[unknown_mask] = 0 if in_range_mask is not None: - out_of_range = ~in_range_mask & (grid.grid != -1) + out_of_range = ~in_range_mask & (cells != -1) vis[out_of_range, :3] = bg_rgb.astype(np.uint8) vis[out_of_range, 3] = 255 return vis # Default: Foxglove-style coloring - vis = np.zeros((grid.height, grid.width, 4), dtype=np.uint8) + vis = np.zeros((grid.info.height, grid.info.width, 4), dtype=np.uint8) - free_mask = grid.grid == 0 - occupied_mask = grid.grid > 0 + free_mask = cells == 0 + occupied_mask = cells > 0 fg_free = np.array([72, 73, 129], dtype=np.float32) blended_free = fg_free * opacity + bg_rgb * (1 - opacity) @@ -215,18 +245,18 @@ def generate_rgba_texture( vis[free_mask, 3] = 255 if np.any(occupied_mask): - costs = grid.grid[occupied_mask].astype(np.float32) + costs = cells[occupied_mask].astype(np.float32) factor = (1 - costs / 100).clip(0, 1) fg_occ = np.column_stack([72 * factor, 73 * factor, 129 * factor]) blended_occ = fg_occ * opacity + bg_rgb * (1 - opacity) vis[occupied_mask, :3] = blended_occ.astype(np.uint8) vis[occupied_mask, 3] = 255 - unknown_mask = grid.grid == -1 + unknown_mask = cells == -1 vis[unknown_mask] = 0 if in_range_mask is not None: - out_of_range = ~in_range_mask & (grid.grid != -1) + out_of_range = ~in_range_mask & (cells != -1) vis[out_of_range, :3] = bg_rgb.astype(np.uint8) vis[out_of_range, 3] = 255 diff --git a/dimos/mapping/occupancy/visualize_path.py b/dimos/mapping/occupancy/visualize_path.py index 4d3de0f0a0e..e531361fcd9 100644 --- a/dimos/mapping/occupancy/visualize_path.py +++ b/dimos/mapping/occupancy/visualize_path.py @@ -12,47 +12,78 @@ # See the License for the specific language governing permissions and # limitations under the License. +from typing import overload + import numpy as np -from dimos.mapping.occupancy.visualizations import visualize_occupancy_grid +from dimos.mapping.occupancy.operations import _grid_coordinates +from dimos.mapping.occupancy.visualizations import _map_image, visualize_occupancy_grid +from dimos.messages.geometry_msgs.msg import PoseStamped as CdrPoseStamped +from dimos.messages.nav_msgs.msg import OccupancyGrid as CdrOccupancyGrid, Path as CdrPath +from dimos.messages.sensor_msgs.msg import Image as CdrImage +from dimos.msgs.geometry import yaw +from dimos.msgs.image import image_view from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid from dimos.msgs.nav_msgs.Path import Path -from dimos.msgs.sensor_msgs.Image import Image, ImageFormat +from dimos.msgs.sensor_msgs.Image import Image + + +@overload +def visualize_path( + occupancy_grid: CdrOccupancyGrid, + path: Path | CdrPath, + robot_width: float, + robot_length: float, + thickness: int = 1, + scale: int = 8, +) -> CdrImage: ... +@overload def visualize_path( occupancy_grid: OccupancyGrid, - path: Path, + path: Path | CdrPath, + robot_width: float, + robot_length: float, + thickness: int = 1, + scale: int = 8, +) -> Image: ... + + +def visualize_path( + occupancy_grid: OccupancyGrid | CdrOccupancyGrid, + path: Path | CdrPath, robot_width: float, robot_length: float, thickness: int = 1, scale: int = 8, -) -> Image: +) -> Image | CdrImage: import cv2 image = visualize_occupancy_grid(occupancy_grid, "rainbow") - bgr = image.data + bgr = image_view(image) if isinstance(image, CdrImage) else image.data bgr = cv2.resize( bgr, (bgr.shape[1] * scale, bgr.shape[0] * scale), interpolation=cv2.INTER_NEAREST, - ) + ).astype(np.uint8, copy=False) # Convert robot dimensions from meters to grid cells, then to scaled pixels - resolution = occupancy_grid.resolution + resolution = occupancy_grid.info.resolution robot_width_px = int((robot_width / resolution) * scale) robot_length_px = int((robot_length / resolution) * scale) # Draw robot rectangle at each path point for pose in path.poses: # Convert world coordinates to grid coordinates - grid_coord = occupancy_grid.world_to_grid([pose.x, pose.y, pose.z]) - cx = int(grid_coord.x * scale) - cy = int(grid_coord.y * scale) + position = pose.pose.position if isinstance(pose, CdrPoseStamped) else pose.position + x, y = _grid_coordinates(occupancy_grid, position) + cx = int(x * scale) + cy = int(y * scale) # Get yaw angle from pose orientation - yaw = pose.yaw + angle = yaw(pose.pose.orientation) if isinstance(pose, CdrPoseStamped) else pose.yaw # Define rectangle corners centered at origin (length along x, width along y) half_length = robot_length_px / 2 @@ -68,8 +99,8 @@ def visualize_path( ) # Rotate corners by yaw angle - cos_yaw = np.cos(yaw) - sin_yaw = np.sin(yaw) + cos_yaw = np.cos(angle) + sin_yaw = np.sin(angle) rotation_matrix = np.array([[cos_yaw, -sin_yaw], [sin_yaw, cos_yaw]]) rotated_corners = corners @ rotation_matrix.T @@ -81,9 +112,4 @@ def visualize_path( pts = rotated_corners.astype(np.int32).reshape((-1, 1, 2)) cv2.polylines(bgr, [pts], isClosed=True, color=(0, 0, 0), thickness=thickness) - return Image( - data=bgr, - format=ImageFormat.BGR, - frame_id=occupancy_grid.frame_id, - ts=occupancy_grid.ts, - ) + return _map_image(occupancy_grid, bgr) From 2c16e091962eed27d915fe168d8b3d10c59ad1e7 Mon Sep 17 00:00:00 2001 From: "autofix-ci[bot]" <114827586+autofix-ci[bot]@users.noreply.github.com> Date: Tue, 20 Oct 2026 06:03:54 +0000 Subject: [PATCH 2/2] [autofix.ci] apply automated fixes --- dimos/models/embedding/clip.py | 2 +- dimos/models/embedding/dino.py | 2 +- dimos/models/embedding/siglip.py | 2 +- dimos/models/embedding/siglip2.py | 2 +- .../models/embedding/test_generated_images.py | 37 +++++++++++-------- dimos/models/embedding/treid.py | 2 +- 6 files changed, 26 insertions(+), 21 deletions(-) diff --git a/dimos/models/embedding/clip.py b/dimos/models/embedding/clip.py index ad5e3e0cc6c..f429dc62a93 100644 --- a/dimos/models/embedding/clip.py +++ b/dimos/models/embedding/clip.py @@ -17,12 +17,12 @@ from functools import cached_property from typing import overload -from dimos.messages.sensor_msgs.msg import Image from PIL import Image as PILImage import torch import torch.nn.functional as functional from transformers import CLIPModel as HFCLIPModel, CLIPProcessor +from dimos.messages.sensor_msgs.msg import Image from dimos.models.base import HuggingFaceModel from dimos.models.embedding.base import Embedding, EmbeddingModel, HuggingFaceEmbeddingModelConfig from dimos.msgs.image import image_to_rgb diff --git a/dimos/models/embedding/dino.py b/dimos/models/embedding/dino.py index 5b0d24fd137..022a442ebf7 100644 --- a/dimos/models/embedding/dino.py +++ b/dimos/models/embedding/dino.py @@ -17,12 +17,12 @@ from functools import cached_property from typing import Any, overload -from dimos.messages.sensor_msgs.msg import Image from PIL import Image as PILImage import torch import torch.nn.functional as functional from transformers import AutoImageProcessor, AutoModel +from dimos.messages.sensor_msgs.msg import Image from dimos.models.base import HuggingFaceModel from dimos.models.embedding.base import Embedding, EmbeddingModel, HuggingFaceEmbeddingModelConfig from dimos.msgs.image import image_to_rgb diff --git a/dimos/models/embedding/siglip.py b/dimos/models/embedding/siglip.py index bb0ed762943..8cabb1afdf4 100644 --- a/dimos/models/embedding/siglip.py +++ b/dimos/models/embedding/siglip.py @@ -17,12 +17,12 @@ from functools import cached_property from typing import overload -from dimos.messages.sensor_msgs.msg import Image from PIL import Image as PILImage import torch import torch.nn.functional as functional from transformers import SiglipModel as HFSiglipModel, SiglipProcessor +from dimos.messages.sensor_msgs.msg import Image from dimos.models.base import HuggingFaceModel from dimos.models.embedding.base import Embedding, EmbeddingModel, HuggingFaceEmbeddingModelConfig from dimos.msgs.image import image_to_rgb diff --git a/dimos/models/embedding/siglip2.py b/dimos/models/embedding/siglip2.py index f8f1fe8861b..c85932f9176 100644 --- a/dimos/models/embedding/siglip2.py +++ b/dimos/models/embedding/siglip2.py @@ -17,12 +17,12 @@ from functools import cached_property from typing import overload -from dimos.messages.sensor_msgs.msg import Image from PIL import Image as PILImage import torch import torch.nn.functional as functional from transformers import SiglipModel as HFSiglipModel, SiglipProcessor +from dimos.messages.sensor_msgs.msg import Image from dimos.models.base import HuggingFaceModel from dimos.models.embedding.base import Embedding, EmbeddingModel, HuggingFaceEmbeddingModelConfig from dimos.msgs.image import image_to_rgb diff --git a/dimos/models/embedding/test_generated_images.py b/dimos/models/embedding/test_generated_images.py index e3c1245ab07..a51f9778ba4 100644 --- a/dimos/models/embedding/test_generated_images.py +++ b/dimos/models/embedding/test_generated_images.py @@ -29,20 +29,25 @@ from dimos.msgs.image import image_from_array, image_view -@pytest.mark.parametrize("model_type", [CLIPModel,DINOModel,SigLIPModel,SigLIP2Model]) -def test_generated_embeddings_keep_zero_source_time_and_convert_rgb(mocker,model_type): - processor,network=mocker.Mock(),mocker.Mock() - features=torch.tensor([[3.0,4.0]]) - processor.return_value=BatchFeature(data={"pixel_values":torch.zeros(1,3,4,8)}) - network.get_image_features.return_value=features - network.return_value=BaseModelOutputWithPooling(last_hidden_state=features[:,None,:],pooler_output=features) - mocker.patch.object(model_type,"_processor",new_callable=PropertyMock,return_value=processor) - mocker.patch.object(model_type,"_model",new_callable=PropertyMock,return_value=network) - image=image_from_array(np.full((4,8,3),[0,0,255],dtype=np.uint8),encoding="bgr8", - header=Header(frame_id="camera",stamp=Time(sec=0,nanosec=0))) - model=model_type(device="cpu") - result=model.embed(image) +@pytest.mark.parametrize("model_type", [CLIPModel, DINOModel, SigLIPModel, SigLIP2Model]) +def test_generated_embeddings_keep_zero_source_time_and_convert_rgb(mocker, model_type): + processor, network = mocker.Mock(), mocker.Mock() + features = torch.tensor([[3.0, 4.0]]) + processor.return_value = BatchFeature(data={"pixel_values": torch.zeros(1, 3, 4, 8)}) + network.get_image_features.return_value = features + network.return_value = BaseModelOutputWithPooling( + last_hidden_state=features[:, None, :], pooler_output=features + ) + mocker.patch.object(model_type, "_processor", new_callable=PropertyMock, return_value=processor) + mocker.patch.object(model_type, "_model", new_callable=PropertyMock, return_value=network) + image = image_from_array( + np.full((4, 8, 3), [0, 0, 255], dtype=np.uint8), + encoding="bgr8", + header=Header(frame_id="camera", stamp=Time(sec=0, nanosec=0)), + ) + model = model_type(device="cpu") + result = model.embed(image) assert result.timestamp == 0.0 - torch.testing.assert_close(result.vector,torch.tensor([0.6,0.8])) - assert processor.call_args.kwargs["images"][0].getpixel((0,0)) == (255,0,0) - np.testing.assert_array_equal(image_view(image)[0,0],[0,0,255]) + torch.testing.assert_close(result.vector, torch.tensor([0.6, 0.8])) + assert processor.call_args.kwargs["images"][0].getpixel((0, 0)) == (255, 0, 0) + np.testing.assert_array_equal(image_view(image)[0, 0], [0, 0, 255]) diff --git a/dimos/models/embedding/treid.py b/dimos/models/embedding/treid.py index ce36b5d004b..3ce7331eec3 100644 --- a/dimos/models/embedding/treid.py +++ b/dimos/models/embedding/treid.py @@ -19,11 +19,11 @@ from functools import cached_property -from dimos.messages.sensor_msgs.msg import Image import torch import torch.nn.functional as functional from torchreid import utils as torchreid_utils +from dimos.messages.sensor_msgs.msg import Image from dimos.models.base import LocalModel from dimos.models.embedding.base import Embedding, EmbeddingModel, EmbeddingModelConfig from dimos.msgs.image import image_to_bgr