diff --git a/.gitignore b/.gitignore index 2a85d4557f..de2e2b0e16 100644 --- a/.gitignore +++ b/.gitignore @@ -55,3 +55,5 @@ yolo11n.pt .claude /logs + +*.so diff --git a/data/.lfs/apartment.tar.gz b/data/.lfs/apartment.tar.gz new file mode 100644 index 0000000000..c8e6cf0331 --- /dev/null +++ b/data/.lfs/apartment.tar.gz @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:8d2c44f39573a80a65aeb6ccd3fcb1c8cb0741dbc7286132856409e88e150e77 +size 18141029 diff --git a/data/.lfs/astar_corner_general.png.tar.gz b/data/.lfs/astar_corner_general.png.tar.gz new file mode 100644 index 0000000000..1303bf22d6 --- /dev/null +++ b/data/.lfs/astar_corner_general.png.tar.gz @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:9790641ad053f4566fb867f9d91450306328cf26f76368bf2639d985be3ae27a +size 5696 diff --git a/data/.lfs/astar_corner_min_cost.png.tar.gz b/data/.lfs/astar_corner_min_cost.png.tar.gz new file mode 100644 index 0000000000..35f3ffe0b6 --- /dev/null +++ b/data/.lfs/astar_corner_min_cost.png.tar.gz @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:42517c5f67a9f06949cb2015a345f9d6b43d22cafd50e1fefb9b5d24d8b72509 +size 5671 diff --git a/data/.lfs/astar_general.png.tar.gz b/data/.lfs/astar_general.png.tar.gz new file mode 100644 index 0000000000..86fe9101d6 --- /dev/null +++ b/data/.lfs/astar_general.png.tar.gz @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:4d4d426bda849551a0555b958792d7bc738b71c9573b477555f5d55c425395cb +size 12109 diff --git a/data/.lfs/astar_min_cost.png.tar.gz b/data/.lfs/astar_min_cost.png.tar.gz new file mode 100644 index 0000000000..752a778295 --- /dev/null +++ b/data/.lfs/astar_min_cost.png.tar.gz @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:06b67aa0d18c291c3525e67ca3a2a9ab2530f6fe782a850872ba4c343353a20a +size 12018 diff --git a/data/.lfs/expected_occupancy_scene.xml.tar.gz b/data/.lfs/expected_occupancy_scene.xml.tar.gz new file mode 100644 index 0000000000..efbe7ce49d --- /dev/null +++ b/data/.lfs/expected_occupancy_scene.xml.tar.gz @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:e3eb91f3c7787882bf26a69df21bb1933d2f6cd71132ca5f0521e2808269bfa2 +size 6777 diff --git a/data/.lfs/gradient_simple.png.tar.gz b/data/.lfs/gradient_simple.png.tar.gz new file mode 100644 index 0000000000..7232282ce4 --- /dev/null +++ b/data/.lfs/gradient_simple.png.tar.gz @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:e418f2a6858c757cb72bd25772749a1664c97a407682d88ad2b51c4bbdcb8006 +size 11568 diff --git a/data/.lfs/gradient_voronoi.png.tar.gz b/data/.lfs/gradient_voronoi.png.tar.gz new file mode 100644 index 0000000000..28e7f263c4 --- /dev/null +++ b/data/.lfs/gradient_voronoi.png.tar.gz @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:3867c0fb5b00f8cb5e0876e5120a70d61f7da121c0a3400010743cc858ee2d54 +size 20680 diff --git a/data/.lfs/inflation_simple.png.tar.gz b/data/.lfs/inflation_simple.png.tar.gz new file mode 100644 index 0000000000..ca6586800c --- /dev/null +++ b/data/.lfs/inflation_simple.png.tar.gz @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:658ed8cafc24ac7dc610b7e5ae484f23e1963872ffc2add0632ee61a7c20492d +size 3412 diff --git a/data/.lfs/make_navigation_map_mixed.png.tar.gz b/data/.lfs/make_navigation_map_mixed.png.tar.gz new file mode 100644 index 0000000000..4fcaa8134a --- /dev/null +++ b/data/.lfs/make_navigation_map_mixed.png.tar.gz @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:36ea27a2434836eb309728f35033674736552daeb82f6e41fb7e3eb175d950da +size 13084 diff --git a/data/.lfs/make_navigation_map_simple.png.tar.gz b/data/.lfs/make_navigation_map_simple.png.tar.gz new file mode 100644 index 0000000000..f966b459e2 --- /dev/null +++ b/data/.lfs/make_navigation_map_simple.png.tar.gz @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:a0d211fa1bc517ef78e8dc548ebff09f58ad34c86d28eb3bd48a09a577ee5d1e +size 11767 diff --git a/data/.lfs/make_path_mask_full.png.tar.gz b/data/.lfs/make_path_mask_full.png.tar.gz new file mode 100644 index 0000000000..0e9336aaea --- /dev/null +++ b/data/.lfs/make_path_mask_full.png.tar.gz @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:b772d266dffa82ccf14f13c7d8cc2443210202836883c80f016a56d4cfe2b52a +size 11213 diff --git a/data/.lfs/make_path_mask_two_meters.png.tar.gz b/data/.lfs/make_path_mask_two_meters.png.tar.gz new file mode 100644 index 0000000000..7fa9e767b8 --- /dev/null +++ b/data/.lfs/make_path_mask_two_meters.png.tar.gz @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:da608d410f4a1afee0965abfac814bc05267bdde31b0d3a9622c39515ee4f813 +size 11395 diff --git a/data/.lfs/occupancy_general.png.tar.gz b/data/.lfs/occupancy_general.png.tar.gz new file mode 100644 index 0000000000..b509151e5a --- /dev/null +++ b/data/.lfs/occupancy_general.png.tar.gz @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:b770d950cf7206a67ccdfd8660ee0ab818228faa9ebbf1a37cbf6ee9d1ac7539 +size 2970 diff --git a/data/.lfs/occupancy_simple.npy.tar.gz b/data/.lfs/occupancy_simple.npy.tar.gz new file mode 100644 index 0000000000..cf42cf3667 --- /dev/null +++ b/data/.lfs/occupancy_simple.npy.tar.gz @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:e1cf83464442fb284b6f7ba2752546fc4571a73f3490c24a58fb45987555a66c +size 1954 diff --git a/data/.lfs/occupancy_simple.png.tar.gz b/data/.lfs/occupancy_simple.png.tar.gz new file mode 100644 index 0000000000..f6370052d6 --- /dev/null +++ b/data/.lfs/occupancy_simple.png.tar.gz @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:7569eb465fb29b3a2f76ae289b094d0a5579dce99c9a99741c7e9e4e741a00b6 +size 2832 diff --git a/data/.lfs/overlay_occupied.png.tar.gz b/data/.lfs/overlay_occupied.png.tar.gz new file mode 100644 index 0000000000..158a52c6bd --- /dev/null +++ b/data/.lfs/overlay_occupied.png.tar.gz @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:0b55bcf7a2a7a5cbdfdfe8c6a75c53ffe5707197d991d1e39e9aa9dc22503397 +size 3657 diff --git a/data/.lfs/resample_path_simple.png.tar.gz b/data/.lfs/resample_path_simple.png.tar.gz new file mode 100644 index 0000000000..1a8c1118d6 --- /dev/null +++ b/data/.lfs/resample_path_simple.png.tar.gz @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:0b5c454ed6cc66cf4446ce4a246464aec27368da4902651b4ad9ed29b3ba56ec +size 118319 diff --git a/data/.lfs/resample_path_smooth.png.tar.gz b/data/.lfs/resample_path_smooth.png.tar.gz new file mode 100644 index 0000000000..80af3d3805 --- /dev/null +++ b/data/.lfs/resample_path_smooth.png.tar.gz @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:6cc0dfd80bada94f2ab1bb577e2ec1734dad6894113f2fe77964bd80d886c3d3 +size 109699 diff --git a/data/.lfs/smooth_occupied.png.tar.gz b/data/.lfs/smooth_occupied.png.tar.gz new file mode 100644 index 0000000000..0e09e7d15a --- /dev/null +++ b/data/.lfs/smooth_occupied.png.tar.gz @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:44c8988b8a7d954ee26a0a5f195b961c62bbdb251b540df6b4d67cd85a72e5ac +size 3511 diff --git a/data/.lfs/three_paths.npy.tar.gz b/data/.lfs/three_paths.npy.tar.gz new file mode 100644 index 0000000000..744eb06305 --- /dev/null +++ b/data/.lfs/three_paths.npy.tar.gz @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:ba849a6b648ccc9ed4987bbe985ee164dd9ad0324895076baa9f86196b2a0d5f +size 5180 diff --git a/data/.lfs/three_paths.ply.tar.gz b/data/.lfs/three_paths.ply.tar.gz new file mode 100644 index 0000000000..a5bfc6bac4 --- /dev/null +++ b/data/.lfs/three_paths.ply.tar.gz @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:639093004355c1ba796c668cd43476dfcabff137ca0bb430ace07730cc512f0e +size 307187 diff --git a/data/.lfs/three_paths.png.tar.gz b/data/.lfs/three_paths.png.tar.gz new file mode 100644 index 0000000000..ade2bd3eb7 --- /dev/null +++ b/data/.lfs/three_paths.png.tar.gz @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:2265ddd76bfb70e7ac44f2158dc0d16e0df264095b0f45a77f95eb85c529d935 +size 2559 diff --git a/data/.lfs/visualize_occupancy_rainbow.png.tar.gz b/data/.lfs/visualize_occupancy_rainbow.png.tar.gz new file mode 100644 index 0000000000..9bbd2e6ea1 --- /dev/null +++ b/data/.lfs/visualize_occupancy_rainbow.png.tar.gz @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:3dc1e3b6519f7d7ff25b16c3124ee447f02857eeb3eb20930cdab95464b1f0a3 +size 11582 diff --git a/data/.lfs/visualize_occupancy_turbo.png.tar.gz b/data/.lfs/visualize_occupancy_turbo.png.tar.gz new file mode 100644 index 0000000000..e2863cdae6 --- /dev/null +++ b/data/.lfs/visualize_occupancy_turbo.png.tar.gz @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:c21874bab6ec7cd9692d2b1e67498ddfff3c832ec992e9552fee17093759b270 +size 18593 diff --git a/dimos/agents2/skills/google_maps_skill_container.py b/dimos/agents2/skills/google_maps_skill_container.py index ba1af7831d..a62b4cdd12 100644 --- a/dimos/agents2/skills/google_maps_skill_container.py +++ b/dimos/agents2/skills/google_maps_skill_container.py @@ -15,8 +15,6 @@ import json from typing import Any -from reactivex.disposable import Disposable - from dimos.core.core import rpc from dimos.core.skill_module import SkillModule from dimos.core.stream import In diff --git a/dimos/core/blueprints.py b/dimos/core/blueprints.py index 9de50ac7d2..5431476b80 100644 --- a/dimos/core/blueprints.py +++ b/dimos/core/blueprints.py @@ -29,6 +29,9 @@ from dimos.core.stream import In, Out from dimos.core.transport import LCMTransport, pLCMTransport from dimos.utils.generic import short_id +from dimos.utils.logging_config import setup_logger + +logger = setup_logger() @dataclass(frozen=True) @@ -205,6 +208,15 @@ def _connect_transports(self, module_coordinator: ModuleCoordinator) -> None: for module, original_name in connections[(remapped_name, type)]: instance = module_coordinator.get_instance(module) instance.set_transport(original_name, transport) # type: ignore[union-attr] + logger.info( + "Transport", + name=remapped_name, + original_name=original_name, + topic=str(getattr(transport, "topic", None)), + type=f"{type.__module__}.{type.__qualname__}", + module=module.__name__, + transport=transport.__class__.__name__, + ) def _connect_rpc_methods(self, module_coordinator: ModuleCoordinator) -> None: # Gather all RPC methods. diff --git a/dimos/core/global_config.py b/dimos/core/global_config.py index 9b68a80384..ca6d45cd97 100644 --- a/dimos/core/global_config.py +++ b/dimos/core/global_config.py @@ -13,9 +13,17 @@ # limitations under the License. from functools import cached_property +import re from pydantic_settings import BaseSettings, SettingsConfigDict +from dimos.mapping.occupancy.path_map import NavigationStrategy +from dimos.navigation.global_planner.types import AStarAlgorithm + + +def _get_all_numbers(s: str) -> list[float]: + return [float(x) for x in re.findall(r"-?\d+\.?\d*", s)] + class GlobalConfig(BaseSettings): robot_ip: str | None = None @@ -23,8 +31,18 @@ class GlobalConfig(BaseSettings): replay: bool = False n_dask_workers: int = 2 memory_limit: str = "auto" + mujoco_camera_position: str | None = None mujoco_room: str | None = None + mujoco_room_from_occupancy: str | None = None + mujoco_global_costmap_from_occupancy: str | None = None + mujoco_global_map_from_pointcloud: str | None = None + mujoco_start_pos: str = "-1.0, 1.0" robot_model: str | None = None + robot_width: float = 0.3 + robot_rotation_diameter: float = 0.6 + planner_strategy: NavigationStrategy = "simple" + astar_algorithm: AStarAlgorithm = "min_cost" + planner_robot_speed: float | None = None model_config = SettingsConfigDict( env_file=".env", @@ -40,3 +58,14 @@ def unitree_connection_type(self) -> str: if self.simulation: return "mujoco" return "webrtc" + + @cached_property + def mujoco_start_pos_float(self) -> tuple[float, float]: + x, y = _get_all_numbers(self.mujoco_start_pos) + return (x, y) + + @cached_property + def mujoco_camera_position_float(self) -> tuple[float, ...] | None: + if self.mujoco_camera_position is None: + return None + return tuple(_get_all_numbers(self.mujoco_camera_position)) diff --git a/dimos/core/transport.py b/dimos/core/transport.py index 03bb073327..d5c7a090df 100644 --- a/dimos/core/transport.py +++ b/dimos/core/transport.py @@ -149,7 +149,7 @@ def broadcast(self, _, msg) -> None: # type: ignore[no-untyped-def] self.shm.publish(self.topic, msg) - def subscribe(self, callback: Callable[[T], None], selfstream: In[T] = None) -> None: # type: ignore[assignment, override] + def subscribe(self, callback: Callable[[T], None], selfstream: In[T] | None = None) -> None: # type: ignore[assignment, override] if not self._started: self.shm.start() self._started = True @@ -174,7 +174,7 @@ def broadcast(self, _, msg) -> None: # type: ignore[no-untyped-def] self.shm.publish(self.topic, msg) - def subscribe(self, callback: Callable[[T], None], selfstream: In[T] = None) -> None: # type: ignore[assignment, override] + def subscribe(self, callback: Callable[[T], None], selfstream: In[T] | None = None) -> None: # type: ignore[assignment, override] if not self._started: self.shm.start() self._started = True diff --git a/dimos/mapping/occupancy/conftest.py b/dimos/mapping/occupancy/conftest.py new file mode 100644 index 0000000000..9cc9006c72 --- /dev/null +++ b/dimos/mapping/occupancy/conftest.py @@ -0,0 +1,30 @@ +# Copyright 2025 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 numpy as np +import pytest + +from dimos.mapping.occupancy.gradient import gradient +from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid +from dimos.utils.data import get_data + + +@pytest.fixture +def occupancy() -> OccupancyGrid: + return OccupancyGrid(np.load(get_data("occupancy_simple.npy"))) + + +@pytest.fixture +def occupancy_gradient(occupancy) -> OccupancyGrid: + return gradient(occupancy, max_distance=1.5) diff --git a/dimos/mapping/occupancy/extrude_occupancy.py b/dimos/mapping/occupancy/extrude_occupancy.py new file mode 100644 index 0000000000..71e6879456 --- /dev/null +++ b/dimos/mapping/occupancy/extrude_occupancy.py @@ -0,0 +1,235 @@ +# Copyright 2025 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 numpy as np +from numpy.typing import NDArray + +from dimos.msgs.nav_msgs.OccupancyGrid import CostValues, OccupancyGrid + +# Rectangle type: (x, y, width, height) +Rect = tuple[int, int, int, int] + + +def identify_convex_shapes(occupancy_grid: OccupancyGrid) -> list[Rect]: + """Identify occupied zones and decompose them into convex rectangles. + + This function finds all occupied cells in the occupancy grid and + decomposes them into axis-aligned rectangles suitable for MuJoCo + collision geometry. + + Args: + occupancy_grid: The input occupancy grid. + output_path: Path to save the visualization image. + + Returns: + List of rectangles as (x, y, width, height) tuples in grid coords. + """ + grid = occupancy_grid.grid + + # Create binary mask of occupied cells (treat UNKNOWN as OCCUPIED) + occupied_mask = ((grid == CostValues.OCCUPIED) | (grid == CostValues.UNKNOWN)).astype( + np.uint8 + ) * 255 + + return _decompose_to_rectangles(occupied_mask) + + +def _decompose_to_rectangles(mask: NDArray[np.uint8]) -> list[Rect]: + """Decompose a binary mask into rectangles using greedy maximal rectangles. + + Iteratively finds and removes the largest rectangle until the mask is empty. + + Args: + mask: Binary mask of the shape (255 for occupied, 0 for free). + + Returns: + List of rectangles as (x, y, width, height) tuples. + """ + rectangles: list[Rect] = [] + remaining = mask.copy() + + max_iterations = 10000 # Safety limit + + for _ in range(max_iterations): + # Find the largest rectangle in the remaining mask + rect = _find_largest_rectangle(remaining) + + if rect is None: + break + + x_start, y_start, x_end, y_end = rect + + # Add rectangle to shapes + # Store as (x, y, width, height) + # x_end and y_end are exclusive (like Python slicing) + rectangles.append((x_start, y_start, x_end - x_start, y_end - y_start)) + + # Remove this rectangle from the mask + remaining[y_start:y_end, x_start:x_end] = 0 + + return rectangles + + +def _find_largest_rectangle(mask: NDArray[np.uint8]) -> tuple[int, int, int, int] | None: + """Find the largest rectangle of 1s in a binary mask. + + Uses the histogram method for O(rows * cols) complexity. + + Args: + mask: Binary mask (non-zero = occupied). + + Returns: + (x_start, y_start, x_end, y_end) or None if no rectangle found. + Coordinates are exclusive on the end (like Python slicing). + """ + if not np.any(mask): + return None + + rows, cols = mask.shape + binary = (mask > 0).astype(np.int32) + + # Build histogram of heights for each row + heights = np.zeros((rows, cols), dtype=np.int32) + heights[0] = binary[0] + for i in range(1, rows): + heights[i] = np.where(binary[i] > 0, heights[i - 1] + 1, 0) + + best_area = 0 + best_rect: tuple[int, int, int, int] | None = None + + # For each row, find largest rectangle in histogram + for row_idx in range(rows): + hist = heights[row_idx] + rect = _largest_rect_in_histogram(hist, row_idx) + if rect is not None: + x_start, y_start, x_end, y_end = rect + area = (x_end - x_start) * (y_end - y_start) + if area > best_area: + best_area = area + best_rect = rect + + return best_rect + + +def _largest_rect_in_histogram( + hist: NDArray[np.int32], bottom_row: int +) -> tuple[int, int, int, int] | None: + """Find largest rectangle in a histogram. + + Args: + hist: Array of heights. + bottom_row: The row index this histogram ends at. + + Returns: + (x_start, y_start, x_end, y_end) or None. + """ + n = len(hist) + if n == 0: + return None + + # Stack-based algorithm for largest rectangle in histogram + stack: list[int] = [] # Stack of indices + best_area = 0 + best_rect: tuple[int, int, int, int] | None = None + + for i in range(n + 1): + h = hist[i] if i < n else 0 + + while stack and hist[stack[-1]] > h: + height = hist[stack.pop()] + width_start = stack[-1] + 1 if stack else 0 + width_end = i + area = height * (width_end - width_start) + + if area > best_area: + best_area = area + # Convert to rectangle coordinates + y_start = bottom_row - height + 1 + y_end = bottom_row + 1 + best_rect = (width_start, y_start, width_end, y_end) + + stack.append(i) + + return best_rect + + +def generate_mujoco_scene( + occupancy_grid: OccupancyGrid, +) -> str: + """Generate a MuJoCo scene XML from an occupancy grid. + + Creates a scene with a flat floor and extruded boxes for each occupied + region. All boxes are red and used for collision. + + Args: + occupancy_grid: The input occupancy grid. + + Returns: + Path to the generated XML file. + """ + extrude_height = 0.5 + + # Get rectangles from the occupancy grid + rectangles = identify_convex_shapes(occupancy_grid) + + resolution = occupancy_grid.resolution + origin_x = occupancy_grid.origin.position.x + origin_y = occupancy_grid.origin.position.y + + # Build XML + xml_lines = [ + '', + '', + ' ', + ' ', + " ", + ' ', + ' ', + ' ', + ' ', + " ", + " ", + ' ', + ' ', + ] + + # Add each rectangle as a box geom + for i, (gx, gy, gw, gh) in enumerate(rectangles): + # Convert grid coordinates to world coordinates + # Grid origin is top-left, world origin is at occupancy_grid.origin + # gx, gy are in grid cells, need to convert to meters + world_x = origin_x + (gx + gw / 2) * resolution + world_y = origin_y + (gy + gh / 2) * resolution + world_z = extrude_height / 2 # Center of the box + + # Box half-sizes + half_x = (gw * resolution) / 2 + half_y = (gh * resolution) / 2 + half_z = extrude_height / 2 + + xml_lines.append( + f' ' + ) + + xml_lines.append(" ") + xml_lines.append(' ') + xml_lines.append("\n") + + xml_content = "\n".join(xml_lines) + + return xml_content diff --git a/dimos/mapping/occupancy/gradient.py b/dimos/mapping/occupancy/gradient.py new file mode 100644 index 0000000000..91e9bb83a5 --- /dev/null +++ b/dimos/mapping/occupancy/gradient.py @@ -0,0 +1,202 @@ +# Copyright 2025 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 numpy as np +from scipy import ndimage # type: ignore[import-untyped] + +from dimos.msgs.nav_msgs.OccupancyGrid import CostValues, OccupancyGrid + + +def gradient( + occupancy_grid: OccupancyGrid, obstacle_threshold: int = 50, max_distance: float = 2.0 +) -> OccupancyGrid: + """Create a gradient OccupancyGrid for path planning. + + Creates a gradient where free space has value 0 and values increase near obstacles. + This can be used as a cost map for path planning algorithms like A*. + + Args: + obstacle_threshold: Cell values >= this are considered obstacles (default: 50) + max_distance: Maximum distance to compute gradient in meters (default: 2.0) + + Returns: + New OccupancyGrid with gradient values: + - -1: Unknown cells (preserved as-is) + - 0: Free space far from obstacles + - 1-99: Increasing cost as you approach obstacles + - 100: At obstacles + + Note: Unknown cells remain as unknown (-1) and do not receive gradient values. + """ + + # Remember which cells are unknown + unknown_mask = occupancy_grid.grid == CostValues.UNKNOWN + + # Create binary obstacle map + # Consider cells >= threshold as obstacles (1), everything else as free (0) + # Unknown cells are not considered obstacles for distance calculation + obstacle_map = (occupancy_grid.grid >= obstacle_threshold).astype(np.float32) + + # Compute distance transform (distance to nearest obstacle in cells) + # Unknown cells are treated as if they don't exist for distance calculation + distance_cells = ndimage.distance_transform_edt(1 - obstacle_map) + + # Convert to meters and clip to max distance + distance_meters = np.clip(distance_cells * occupancy_grid.resolution, 0, max_distance) # type: ignore[operator] + + # Invert and scale to 0-100 range + # Far from obstacles (max_distance) -> 0 + # At obstacles (0 distance) -> 100 + gradient_values = (1 - distance_meters / max_distance) * 100 + + # Ensure obstacles are exactly 100 + gradient_values[obstacle_map > 0] = CostValues.OCCUPIED + + # Convert to int8 for OccupancyGrid + gradient_data = gradient_values.astype(np.int8) + + # Preserve unknown cells as unknown (don't apply gradient to them) + gradient_data[unknown_mask] = CostValues.UNKNOWN + + # Create new OccupancyGrid with gradient + gradient_grid = OccupancyGrid( + grid=gradient_data, + resolution=occupancy_grid.resolution, + origin=occupancy_grid.origin, + frame_id=occupancy_grid.frame_id, + ts=occupancy_grid.ts, + ) + + return gradient_grid + + +def voronoi_gradient( + occupancy_grid: OccupancyGrid, obstacle_threshold: int = 50, max_distance: float = 2.0 +) -> OccupancyGrid: + """Create a Voronoi-based gradient OccupancyGrid for path planning. + + Unlike the regular gradient which can result in suboptimal paths in narrow + corridors (where the center still has high cost), this method creates a cost + map based on the Voronoi diagram of obstacles. Cells on Voronoi edges + (equidistant from multiple obstacles) have minimum cost, encouraging paths + that stay maximally far from all obstacles. + + For a corridor of width 10 cells: + - Regular gradient: center cells might be 95 (still high cost) + - Voronoi gradient: center cells are 0 (optimal path) + + The cost is interpolated based on relative position between the nearest + obstacle and the nearest Voronoi edge: + - At obstacle: cost = 100 + - At Voronoi edge: cost = 0 + - In between: cost = 99 * d_voronoi / (d_obstacle + d_voronoi) + + Args: + obstacle_threshold: Cell values >= this are considered obstacles (default: 50) + max_distance: Maximum distance in meters beyond which cost is 0 (default: 2.0) + + Returns: + New OccupancyGrid with gradient values: + - -1: Unknown cells (preserved as-is) + - 0: On Voronoi edges (equidistant from obstacles) or far from obstacles + - 1-99: Increasing cost closer to obstacles + - 100: At obstacles + """ + # Remember which cells are unknown + unknown_mask = occupancy_grid.grid == CostValues.UNKNOWN + + # Create binary obstacle map + obstacle_map = (occupancy_grid.grid >= obstacle_threshold).astype(np.float32) + + # Check if there are any obstacles + if not np.any(obstacle_map): + # No obstacles - everything is free + gradient_data = np.zeros_like(occupancy_grid.grid, dtype=np.int8) + gradient_data[unknown_mask] = CostValues.UNKNOWN + return OccupancyGrid( + grid=gradient_data, + resolution=occupancy_grid.resolution, + origin=occupancy_grid.origin, + frame_id=occupancy_grid.frame_id, + ts=occupancy_grid.ts, + ) + + # Label connected obstacle regions (clusters) + # This groups all cells of the same wall/obstacle together + obstacle_labels, num_obstacles = ndimage.label(obstacle_map) + + # If only one obstacle cluster, Voronoi edges don't make sense + # Fall back to regular gradient behavior + if num_obstacles <= 1: + return gradient(occupancy_grid, obstacle_threshold, max_distance) + + # Compute distance transform with indices to nearest obstacle + # indices[0][i,j], indices[1][i,j] = row,col of nearest obstacle to (i,j) + distance_cells, indices = ndimage.distance_transform_edt(1 - obstacle_map, return_indices=True) + + # For each cell, find which obstacle cluster it belongs to (Voronoi region) + # by looking up the label of its nearest obstacle cell + nearest_obstacle_cluster = obstacle_labels[indices[0], indices[1]] + + # Find Voronoi edges: cells where neighbors belong to different obstacle clusters + # Using max/min filters: an edge exists where max != min in the 3x3 neighborhood + footprint = np.ones((3, 3), dtype=bool) + local_max = ndimage.maximum_filter( + nearest_obstacle_cluster, footprint=footprint, mode="nearest" + ) + local_min = ndimage.minimum_filter( + nearest_obstacle_cluster, footprint=footprint, mode="nearest" + ) + voronoi_edges = local_max != local_min + + # Don't count obstacle cells as Voronoi edges + voronoi_edges &= obstacle_map == 0 + + # Compute distance to nearest Voronoi edge + if not np.any(voronoi_edges): + # No Voronoi edges found - fall back to regular gradient + return gradient(occupancy_grid, obstacle_threshold, max_distance) + + voronoi_distance = ndimage.distance_transform_edt(~voronoi_edges) + + # Calculate cost based on position between obstacle and Voronoi edge + # cost = 99 * d_voronoi / (d_obstacle + d_voronoi) + # At Voronoi edge: d_voronoi = 0, cost = 0 + # Near obstacle: d_obstacle small, d_voronoi large, cost high + total_distance = distance_cells + voronoi_distance + with np.errstate(divide="ignore", invalid="ignore"): + cost_ratio = np.where(total_distance > 0, voronoi_distance / total_distance, 0) + + gradient_values = cost_ratio * 99 + + # Ensure obstacles are exactly 100 + gradient_values[obstacle_map > 0] = CostValues.OCCUPIED + + # Apply max_distance clipping - cells beyond max_distance from obstacles get cost 0 + max_distance_cells = max_distance / occupancy_grid.resolution + gradient_values[distance_cells > max_distance_cells] = 0 + + # Convert to int8 + gradient_data = gradient_values.astype(np.int8) + + # Preserve unknown cells + gradient_data[unknown_mask] = CostValues.UNKNOWN + + return OccupancyGrid( + grid=gradient_data, + resolution=occupancy_grid.resolution, + origin=occupancy_grid.origin, + frame_id=occupancy_grid.frame_id, + ts=occupancy_grid.ts, + ) diff --git a/dimos/mapping/occupancy/inflation.py b/dimos/mapping/occupancy/inflation.py new file mode 100644 index 0000000000..fdade7de63 --- /dev/null +++ b/dimos/mapping/occupancy/inflation.py @@ -0,0 +1,53 @@ +# Copyright 2025 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 numpy as np +from scipy import ndimage # type: ignore[import-untyped] + +from dimos.msgs.nav_msgs.OccupancyGrid import CostValues, OccupancyGrid + + +def simple_inflate(occupancy_grid: OccupancyGrid, radius: float) -> OccupancyGrid: + """Inflate obstacles by a given radius (binary inflation). + Args: + radius: Inflation radius in meters + Returns: + New OccupancyGrid with inflated obstacles + """ + # Convert radius to grid cells + cell_radius = int(np.ceil(radius / occupancy_grid.resolution)) + + # Get grid as numpy array + grid_array = occupancy_grid.grid + + # Create circular kernel for binary inflation + y, x = np.ogrid[-cell_radius : cell_radius + 1, -cell_radius : cell_radius + 1] + kernel = (x**2 + y**2 <= cell_radius**2).astype(np.uint8) + + # Find occupied cells + occupied_mask = grid_array >= CostValues.OCCUPIED + + # Binary inflation + inflated = ndimage.binary_dilation(occupied_mask, structure=kernel) + result_grid = grid_array.copy() + result_grid[inflated] = CostValues.OCCUPIED + + # Create new OccupancyGrid with inflated data using numpy constructor + return OccupancyGrid( + grid=result_grid, + resolution=occupancy_grid.resolution, + origin=occupancy_grid.origin, + frame_id=occupancy_grid.frame_id, + ts=occupancy_grid.ts, + ) diff --git a/dimos/mapping/occupancy/operations.py b/dimos/mapping/occupancy/operations.py new file mode 100644 index 0000000000..bc22a2afa4 --- /dev/null +++ b/dimos/mapping/occupancy/operations.py @@ -0,0 +1,88 @@ +# Copyright 2025 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 numpy as np +from scipy import ndimage # type: ignore[import-untyped] + +from dimos.msgs.nav_msgs.OccupancyGrid import CostValues, OccupancyGrid + + +def smooth_occupied( + occupancy_grid: OccupancyGrid, min_neighbor_fraction: float = 0.4 +) -> OccupancyGrid: + """Smooth occupied zones by removing unsupported protrusions. + + Removes occupied cells that don't have sufficient neighboring occupied + cells. + + Args: + occupancy_grid: Input occupancy grid + min_neighbor_fraction: Minimum fraction of 8-connected neighbors + that must be occupied for a cell to remain occupied. + Returns: + New OccupancyGrid with smoothed occupied zones + """ + grid_array = occupancy_grid.grid + occupied_mask = grid_array >= CostValues.OCCUPIED + + # Count occupied neighbors for each cell (8-connectivity). + kernel = np.array([[1, 1, 1], [1, 0, 1], [1, 1, 1]], dtype=np.uint8) + neighbor_count = ndimage.convolve( + occupied_mask.astype(np.uint8), kernel, mode="constant", cval=0 + ) + + # Remove cells with too few occupied neighbors. + min_neighbors = int(np.ceil(8 * min_neighbor_fraction)) + unsupported = occupied_mask & (neighbor_count < min_neighbors) + + result_grid = grid_array.copy() + result_grid[unsupported] = CostValues.FREE + + return OccupancyGrid( + grid=result_grid, + resolution=occupancy_grid.resolution, + origin=occupancy_grid.origin, + frame_id=occupancy_grid.frame_id, + ts=occupancy_grid.ts, + ) + + +def overlay_occupied(base: OccupancyGrid, overlay: OccupancyGrid) -> OccupancyGrid: + """Overlay occupied zones from one grid onto another. + + Marks cells as occupied in the base grid wherever they are occupied + in the overlay grid. + + Args: + base: The base occupancy grid + overlay: The grid whose occupied zones will be overlaid onto base + Returns: + New OccupancyGrid with combined occupied zones + """ + if base.grid.shape != overlay.grid.shape: + raise ValueError( + f"Grid shapes must match: base {base.grid.shape} vs overlay {overlay.grid.shape}" + ) + + result_grid = base.grid.copy() + overlay_occupied_mask = overlay.grid >= CostValues.OCCUPIED + result_grid[overlay_occupied_mask] = CostValues.OCCUPIED + + return OccupancyGrid( + grid=result_grid, + resolution=base.resolution, + origin=base.origin, + frame_id=base.frame_id, + ts=base.ts, + ) diff --git a/dimos/mapping/occupancy/path_map.py b/dimos/mapping/occupancy/path_map.py new file mode 100644 index 0000000000..aa0a5599f5 --- /dev/null +++ b/dimos/mapping/occupancy/path_map.py @@ -0,0 +1,40 @@ +# Copyright 2025 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 typing import Literal, TypeAlias + +from dimos.mapping.occupancy.gradient import voronoi_gradient +from dimos.mapping.occupancy.inflation import simple_inflate +from dimos.mapping.occupancy.operations import overlay_occupied, smooth_occupied +from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid + +NavigationStrategy: TypeAlias = Literal["simple", "mixed"] + + +def make_navigation_map( + occupancy_grid: OccupancyGrid, robot_width: float, strategy: NavigationStrategy +) -> OccupancyGrid: + half_width = robot_width / 2 + gradient_distance = 1.5 + + if strategy == "simple": + costmap = simple_inflate(occupancy_grid, half_width) + elif strategy == "mixed": + costmap = smooth_occupied(occupancy_grid) + costmap = simple_inflate(costmap, half_width) + costmap = overlay_occupied(costmap, occupancy_grid) + else: + raise ValueError(f"Unknown strategy: {strategy}") + + return voronoi_gradient(costmap, max_distance=gradient_distance) diff --git a/dimos/mapping/occupancy/path_mask.py b/dimos/mapping/occupancy/path_mask.py new file mode 100644 index 0000000000..8dfbd94cec --- /dev/null +++ b/dimos/mapping/occupancy/path_mask.py @@ -0,0 +1,98 @@ +# Copyright 2025 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 cv2 +import numpy as np +from numpy.typing import NDArray + +from dimos.msgs.nav_msgs import Path +from dimos.msgs.nav_msgs.OccupancyGrid import CostValues, OccupancyGrid + + +def make_path_mask( + occupancy_grid: OccupancyGrid, + path: Path, + robot_width: float, + pose_index: int = 0, + max_length: float = float("inf"), +) -> NDArray[np.bool_]: + """Generate a numpy mask of path cells the robot will travel through. + + Creates a boolean mask where True indicates cells that the robot will + occupy while following the path, accounting for the robot's width. + + Args: + occupancy_grid: The occupancy grid providing dimensions and resolution. + path: The path containing poses the robot will follow. + robot_width: The width of the robot in meters. + pose_index: The index in path.poses to start drawing from. Defaults to 0. + max_length: Maximum cumulative length to draw. Defaults to infinity. + + Returns: + A 2D boolean numpy array (height x width) where True indicates + cells the robot will pass through. + """ + mask = np.zeros((occupancy_grid.height, occupancy_grid.width), dtype=np.uint8) + + line_width_pixels = max(1, int(robot_width / occupancy_grid.resolution)) + + poses = path.poses + if len(poses) < pose_index + 2: + return mask.astype(np.bool_) + + # 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 + + segment_length = np.sqrt( + (pos2.x - pos1.x) ** 2 + (pos2.y - pos1.y) ** 2 + (pos2.z - pos1.z) ** 2 + ) + + if cumulative_length + segment_length > max_length: + break + + cumulative_length += segment_length + + grid_pt1 = occupancy_grid.world_to_grid(pos1) + grid_pt2 = occupancy_grid.world_to_grid(pos2) + + pt1 = (round(grid_pt1.x), round(grid_pt1.y)) + pt2 = (round(grid_pt2.x), round(grid_pt2.y)) + + cv2.line(mask, pt1, pt2, (255.0,), thickness=line_width_pixels) + + bool_mask = mask.astype(np.bool_) + + total_points = np.sum(bool_mask) + + if total_points == 0: + return bool_mask + + occupied_mask = occupancy_grid.grid >= CostValues.OCCUPIED + occupied_in_path = bool_mask & occupied_mask + occupied_count = np.sum(occupied_in_path) + + if occupied_count / total_points > 0.05: + raise ValueError( + f"More than 5% of path points are occupied: " + f"{occupied_count}/{total_points} ({100 * occupied_count / total_points:.1f}%)" + ) + + # Some of the points on the edge of the path may be occupied due to + # rounding. Remove them. + bool_mask = bool_mask & ~occupied_mask + + return bool_mask diff --git a/dimos/mapping/occupancy/path_resampling.py b/dimos/mapping/occupancy/path_resampling.py new file mode 100644 index 0000000000..f33d6c7cb9 --- /dev/null +++ b/dimos/mapping/occupancy/path_resampling.py @@ -0,0 +1,245 @@ +# Copyright 2025 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 + +import numpy as np +from scipy.ndimage import uniform_filter1d # type: ignore[import-untyped] + +from dimos.msgs.geometry_msgs import Pose, PoseStamped, Quaternion, Vector3 +from dimos.msgs.nav_msgs import Path +from dimos.utils.logging_config import setup_logger +from dimos.utils.transform_utils import euler_to_quaternion + +logger = setup_logger() + + +def _add_orientations_to_path(path: Path, goal_orientation: Quaternion) -> None: + """Add orientations to path poses based on direction of movement. + + Args: + path: Path with poses to add orientations to + goal_orientation: Desired orientation for the final pose + + Returns: + Path with orientations added to all poses + """ + if not path.poses or len(path.poses) < 2: + return + + # Calculate orientations for all poses except the last one + for i in range(len(path.poses) - 1): + current_pose = path.poses[i] + next_pose = path.poses[i + 1] + + # Calculate direction to next point + dx = next_pose.position.x - current_pose.position.x + dy = next_pose.position.y - current_pose.position.y + + # Calculate yaw angle + yaw = math.atan2(dy, dx) + + # Convert to quaternion (roll=0, pitch=0, yaw) + orientation = euler_to_quaternion(Vector3(0, 0, yaw)) + current_pose.orientation = orientation + + # Set last pose orientation + identity_quat = Quaternion(0, 0, 0, 1) + if goal_orientation != identity_quat: + # Use the provided goal orientation if it's not the identity + path.poses[-1].orientation = goal_orientation + elif len(path.poses) > 1: + # Use the previous pose's orientation + path.poses[-1].orientation = path.poses[-2].orientation + else: + # Single pose with identity goal orientation + path.poses[-1].orientation = identity_quat + + +# TODO: replace goal_pose with just goal_orientation +def simple_resample_path(path: Path, goal_pose: Pose, spacing: float) -> Path: + """Resample a path to have approximately uniform spacing between poses. + + Args: + path: The original Path + spacing: Desired distance between consecutive poses + + Returns: + A new Path with resampled poses + """ + if len(path) < 2 or spacing <= 0: + return path + + resampled = [] + resampled.append(path.poses[0]) + + accumulated_distance = 0.0 + + for i in range(1, len(path.poses)): + current = path.poses[i] + prev = path.poses[i - 1] + + # Calculate segment distance + dx = current.x - prev.x + dy = current.y - prev.y + segment_length = (dx**2 + dy**2) ** 0.5 + + if segment_length < 1e-10: + continue + + # Direction vector + dir_x = dx / segment_length + dir_y = dy / segment_length + + # Add points along this segment + while accumulated_distance + segment_length >= spacing: + # Distance along segment for next point + dist_along = spacing - accumulated_distance + if dist_along < 0: + break + + # Create new pose + new_x = prev.x + dir_x * dist_along + new_y = prev.y + dir_y * dist_along + new_pose = PoseStamped( + frame_id=path.frame_id, + position=[new_x, new_y, 0.0], + orientation=prev.orientation, # Keep same orientation + ) + resampled.append(new_pose) + + # Update for next iteration + accumulated_distance = 0 + segment_length -= dist_along + prev = new_pose + + accumulated_distance += segment_length + + # Add last pose if not already there + if len(path.poses) > 1: + last = path.poses[-1] + if not resampled or (resampled[-1].x != last.x or resampled[-1].y != last.y): + resampled.append(last) + + ret = Path(frame_id=path.frame_id, poses=resampled) + + _add_orientations_to_path(ret, goal_pose.orientation) + + return ret + + +def smooth_resample_path( + path: Path, goal_pose: Pose, spacing: float, smoothing_window: int = 100 +) -> Path: + """Resample a path with smoothing to reduce jagged corners and abrupt turns. + + This produces smoother paths than simple_resample_path by: + - First upsampling the path to have many points + - Applying a moving average filter to smooth the coordinates + - Resampling at the desired spacing + - Keeping start and end points fixed + + Args: + path: The original Path + goal_pose: Goal pose with desired final orientation + spacing: Desired approximate distance between consecutive poses + smoothing_window: Size of the smoothing window (larger = smoother) + + Returns: + A new Path with smoothly resampled poses + """ + if len(path) < 2 or spacing <= 0: + return path + + # Extract x, y coordinates from path + xs = np.array([p.x for p in path.poses]) + ys = np.array([p.y for p in path.poses]) + + # Remove duplicate consecutive points + diffs = np.sqrt(np.diff(xs) ** 2 + np.diff(ys) ** 2) + valid_mask = np.concatenate([[True], diffs > 1e-10]) + xs = xs[valid_mask] + ys = ys[valid_mask] + + if len(xs) < 2: + return path + + # Calculate total path length + dx = np.diff(xs) + dy = np.diff(ys) + segment_lengths = np.sqrt(dx**2 + dy**2) + total_length = np.sum(segment_lengths) + + if total_length < spacing: + return path + + # Upsample: create many points along the original path using linear interpolation + # This gives us enough points for effective smoothing + upsample_factor = 10 + num_upsampled = max(len(xs) * upsample_factor, 100) + + arc_length = np.concatenate([[0], np.cumsum(segment_lengths)]) + upsample_distances = np.linspace(0, total_length, num_upsampled) + + # Linear interpolation along arc length + xs_upsampled = np.interp(upsample_distances, arc_length, xs) + ys_upsampled = np.interp(upsample_distances, arc_length, ys) + + # Apply moving average smoothing + # Use 'nearest' mode to avoid shrinking at boundaries + window = min(smoothing_window, len(xs_upsampled) // 3) + if window >= 3: + xs_smooth = uniform_filter1d(xs_upsampled, size=window, mode="nearest") + ys_smooth = uniform_filter1d(ys_upsampled, size=window, mode="nearest") + else: + xs_smooth = xs_upsampled + ys_smooth = ys_upsampled + + # Keep start and end points exactly as original + xs_smooth[0] = xs[0] + ys_smooth[0] = ys[0] + xs_smooth[-1] = xs[-1] + ys_smooth[-1] = ys[-1] + + # Recalculate arc length on smoothed path + dx_smooth = np.diff(xs_smooth) + dy_smooth = np.diff(ys_smooth) + segment_lengths_smooth = np.sqrt(dx_smooth**2 + dy_smooth**2) + arc_length_smooth = np.concatenate([[0], np.cumsum(segment_lengths_smooth)]) + total_length_smooth = arc_length_smooth[-1] + + # Resample at desired spacing + num_samples = max(2, int(np.ceil(total_length_smooth / spacing)) + 1) + sample_distances = np.linspace(0, total_length_smooth, num_samples) + + # Interpolate to get final points + sampled_x = np.interp(sample_distances, arc_length_smooth, xs_smooth) + sampled_y = np.interp(sample_distances, arc_length_smooth, ys_smooth) + + # Create resampled poses + resampled = [] + for i in range(len(sampled_x)): + new_pose = PoseStamped( + frame_id=path.frame_id, + position=[float(sampled_x[i]), float(sampled_y[i]), 0.0], + orientation=Quaternion(0, 0, 0, 1), + ) + resampled.append(new_pose) + + ret = Path(frame_id=path.frame_id, poses=resampled) + + _add_orientations_to_path(ret, goal_pose.orientation) + + return ret diff --git a/dimos/mapping/occupancy/test_extrude_occupancy.py b/dimos/mapping/occupancy/test_extrude_occupancy.py new file mode 100644 index 0000000000..6051241998 --- /dev/null +++ b/dimos/mapping/occupancy/test_extrude_occupancy.py @@ -0,0 +1,25 @@ +# Copyright 2025 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 dimos.mapping.occupancy.extrude_occupancy import generate_mujoco_scene +from dimos.utils.data import get_data + + +def test_generate_mujoco_scene(occupancy) -> None: + with open(get_data("expected_occupancy_scene.xml")) as f: + expected = f.read() + + actual = generate_mujoco_scene(occupancy) + + assert actual == expected diff --git a/dimos/mapping/occupancy/test_gradient.py b/dimos/mapping/occupancy/test_gradient.py new file mode 100644 index 0000000000..8e05b6525f --- /dev/null +++ b/dimos/mapping/occupancy/test_gradient.py @@ -0,0 +1,37 @@ +# Copyright 2025 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 numpy as np +import pytest + +from dimos.mapping.occupancy.gradient import gradient, voronoi_gradient +from dimos.mapping.occupancy.visualizations import visualize_occupancy_grid +from dimos.msgs.sensor_msgs.Image import Image +from dimos.utils.data import get_data + + +@pytest.mark.parametrize("method", ["simple", "voronoi"]) +def test_gradient(occupancy, method) -> None: + expected = Image.from_file(get_data(f"gradient_{method}.png")) + + match method: + case "simple": + og = gradient(occupancy, max_distance=1.5) + case "voronoi": + og = voronoi_gradient(occupancy, max_distance=1.5) + case _: + raise ValueError(f"Unknown resampling method: {method}") + + actual = visualize_occupancy_grid(og, "rainbow") + np.testing.assert_array_equal(actual.data, expected.data) diff --git a/dimos/mapping/occupancy/test_inflation.py b/dimos/mapping/occupancy/test_inflation.py new file mode 100644 index 0000000000..04763777b8 --- /dev/null +++ b/dimos/mapping/occupancy/test_inflation.py @@ -0,0 +1,31 @@ +#!/usr/bin/env python3 +# Copyright 2025 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 cv2 +import numpy as np + +from dimos.mapping.occupancy.inflation import simple_inflate +from dimos.mapping.occupancy.visualizations import visualize_occupancy_grid +from dimos.utils.data import get_data + + +def test_inflation(occupancy) -> None: + expected = cv2.imread(get_data("inflation_simple.png"), cv2.IMREAD_COLOR) + + og = simple_inflate(occupancy, 0.2) + + result = visualize_occupancy_grid(og, "rainbow") + np.testing.assert_array_equal(result.data, expected) diff --git a/dimos/mapping/occupancy/test_operations.py b/dimos/mapping/occupancy/test_operations.py new file mode 100644 index 0000000000..6dc05bc91a --- /dev/null +++ b/dimos/mapping/occupancy/test_operations.py @@ -0,0 +1,40 @@ +#!/usr/bin/env python3 +# Copyright 2025 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 cv2 +import numpy as np + +from dimos.mapping.occupancy.operations import overlay_occupied, smooth_occupied +from dimos.mapping.occupancy.visualizations import visualize_occupancy_grid +from dimos.utils.data import get_data + + +def test_smooth_occupied(occupancy) -> None: + expected = cv2.imread(get_data("smooth_occupied.png"), cv2.IMREAD_COLOR) + + result = visualize_occupancy_grid(smooth_occupied(occupancy), "rainbow") + + np.testing.assert_array_equal(result.data, expected) + + +def test_overlay_occupied(occupancy) -> None: + expected = cv2.imread(get_data("overlay_occupied.png"), cv2.IMREAD_COLOR) + overlay = occupancy.copy() + overlay.grid[50:100, 50:100] = 100 + + result = visualize_occupancy_grid(overlay_occupied(occupancy, overlay), "rainbow") + + np.testing.assert_array_equal(result.data, expected) diff --git a/dimos/mapping/occupancy/test_path_map.py b/dimos/mapping/occupancy/test_path_map.py new file mode 100644 index 0000000000..603336e5a2 --- /dev/null +++ b/dimos/mapping/occupancy/test_path_map.py @@ -0,0 +1,34 @@ +#!/usr/bin/env python3 +# Copyright 2025 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 cv2 +import numpy as np +import pytest + +from dimos.mapping.occupancy.path_map import make_navigation_map +from dimos.mapping.occupancy.visualizations import visualize_occupancy_grid +from dimos.utils.data import get_data + + +@pytest.mark.parametrize("strategy", ["simple", "mixed"]) +def test_make_navigation_map(occupancy, strategy) -> None: + expected = cv2.imread(get_data(f"make_navigation_map_{strategy}.png"), cv2.IMREAD_COLOR) + robot_width = 0.4 + + og = make_navigation_map(occupancy, robot_width, strategy=strategy) + + result = visualize_occupancy_grid(og, "rainbow") + np.testing.assert_array_equal(result.data, expected) diff --git a/dimos/mapping/occupancy/test_path_mask.py b/dimos/mapping/occupancy/test_path_mask.py new file mode 100644 index 0000000000..16a519755b --- /dev/null +++ b/dimos/mapping/occupancy/test_path_mask.py @@ -0,0 +1,48 @@ +# Copyright 2025 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 numpy as np +import pytest + +from dimos.mapping.occupancy.path_mask import make_path_mask +from dimos.mapping.occupancy.path_resampling import smooth_resample_path +from dimos.mapping.occupancy.visualizations import visualize_occupancy_grid +from dimos.msgs.geometry_msgs import Pose +from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.msgs.sensor_msgs import Image +from dimos.navigation.global_planner.astar import astar +from dimos.utils.data import get_data + + +@pytest.mark.parametrize( + "pose_index,max_length,expected_image", + [ + (0, float("inf"), "make_path_mask_full.png"), + (50, 2, "make_path_mask_two_meters.png"), + ], +) +def test_make_path_mask(occupancy_gradient, pose_index, max_length, expected_image) -> None: + start = Vector3(4.0, 2.0, 0) + goal_pose = Pose(6.15, 10.0, 0, 0, 0, 0, 1) + expected = Image.from_file(get_data(expected_image)) + path = astar("min_cost", occupancy_gradient, goal_pose.position, start, use_cpp=False) + path = smooth_resample_path(path, goal_pose, 0.1) + robot_width = 0.4 + path_mask = make_path_mask(occupancy_gradient, path, robot_width, pose_index, max_length) + actual = visualize_occupancy_grid(occupancy_gradient, "rainbow") + + actual.data[path_mask] = [0, 100, 0] + + np.testing.assert_array_equal(actual.data, expected.data) diff --git a/dimos/mapping/occupancy/test_path_resampling.py b/dimos/mapping/occupancy/test_path_resampling.py new file mode 100644 index 0000000000..8f3408ef33 --- /dev/null +++ b/dimos/mapping/occupancy/test_path_resampling.py @@ -0,0 +1,50 @@ +# Copyright 2025 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 numpy as np +import pytest + +from dimos.mapping.occupancy.gradient import gradient +from dimos.mapping.occupancy.path_resampling import simple_resample_path, smooth_resample_path +from dimos.mapping.occupancy.visualize_path import visualize_path +from dimos.msgs.geometry_msgs import Pose +from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid +from dimos.msgs.sensor_msgs.Image import Image +from dimos.navigation.global_planner.astar import astar +from dimos.utils.data import get_data + + +@pytest.fixture +def costmap() -> OccupancyGrid: + return gradient(OccupancyGrid(np.load(get_data("occupancy_simple.npy"))), max_distance=1.5) + + +@pytest.mark.parametrize("method", ["simple", "smooth"]) +def test_resample_path(costmap, method) -> None: + start = Vector3(4.0, 2.0, 0) + goal_pose = Pose(6.15, 10.0, 0, 0, 0, 0, 1) + expected = Image.from_file(get_data(f"resample_path_{method}.png")) + path = astar("min_cost", costmap, goal_pose.position, start, use_cpp=False) + + match method: + case "simple": + resampled = simple_resample_path(path, goal_pose, 0.1) + case "smooth": + resampled = smooth_resample_path(path, goal_pose, 0.1) + case _: + raise ValueError(f"Unknown resampling method: {method}") + + actual = visualize_path(costmap, resampled, 0.2, 0.4) + np.testing.assert_array_equal(actual.data, expected.data) diff --git a/dimos/mapping/occupancy/test_visualizations.py b/dimos/mapping/occupancy/test_visualizations.py new file mode 100644 index 0000000000..9823fa9855 --- /dev/null +++ b/dimos/mapping/occupancy/test_visualizations.py @@ -0,0 +1,31 @@ +#!/usr/bin/env python3 +# Copyright 2025 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 cv2 +import numpy as np +import pytest + +from dimos.mapping.occupancy.visualizations import visualize_occupancy_grid +from dimos.utils.data import get_data + + +@pytest.mark.parametrize("palette", ["rainbow", "turbo"]) +def test_visualize_occupancy_grid(occupancy_gradient, palette) -> None: + expected = cv2.imread(get_data(f"visualize_occupancy_{palette}.png"), cv2.IMREAD_COLOR) + + result = visualize_occupancy_grid(occupancy_gradient, palette) + + np.testing.assert_array_equal(result.data, expected) diff --git a/dimos/mapping/occupancy/visualizations.py b/dimos/mapping/occupancy/visualizations.py new file mode 100644 index 0000000000..26ec29e95f --- /dev/null +++ b/dimos/mapping/occupancy/visualizations.py @@ -0,0 +1,160 @@ +# Copyright 2025 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 functools import lru_cache +from typing import Literal, TypeAlias + +import cv2 +import numpy as np +from numpy.typing import NDArray + +from dimos.msgs.nav_msgs import Path +from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid +from dimos.msgs.sensor_msgs import Image +from dimos.msgs.sensor_msgs.image_impls.AbstractImage import ImageFormat + +Palette: TypeAlias = Literal["rainbow", "turbo"] + + +def visualize_occupancy_grid( + occupancy_grid: OccupancyGrid, palette: Palette, path: Path | None = None +) -> Image: + match palette: + case "rainbow": + bgr_image = rainbow_image(occupancy_grid.grid) + case "turbo": + bgr_image = turbo_image(occupancy_grid.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, + ) + + +def _draw_path(occupancy_grid: OccupancyGrid, bgr_image: NDArray[np.uint8], path: Path) -> None: + 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) + + if 0 <= pixel_x < occupancy_grid.width and 0 <= pixel_y < occupancy_grid.height: + points.append((pixel_x, pixel_y)) + + if len(points) > 1: + points_array = np.array(points, dtype=np.int32) + cv2.polylines(bgr_image, [points_array], isClosed=False, color=(0, 0, 0), thickness=1) + + +def rainbow_image(grid: NDArray[np.int8]) -> NDArray[np.uint8]: + """Convert the occupancy grid to a rainbow-colored Image. + + Color scheme: + - -1 (unknown): black + - 100 (occupied): magenta + - 0-99: rainbow from blue (0) to red (99) + + Returns: + Image with rainbow visualization of the occupancy grid + """ + + # Create a copy of the grid for visualization + # Map values to 0-255 range for colormap + height, width = grid.shape + vis_grid = np.zeros((height, width), dtype=np.uint8) + + # Handle 0-99: map to colormap range + gradient_mask = (grid >= 0) & (grid < 100) + vis_grid[gradient_mask] = ((grid[gradient_mask] / 99.0) * 255).astype(np.uint8) + + # Apply JET colormap (blue to red) - returns BGR + bgr_image = cv2.applyColorMap(vis_grid, cv2.COLORMAP_JET) + + unknown_mask = grid == -1 + bgr_image[unknown_mask] = [0, 0, 0] + + occupied_mask = grid == 100 + bgr_image[occupied_mask] = [255, 0, 255] + + return bgr_image.astype(np.uint8) + + +def turbo_image(grid: NDArray[np.int8]) -> NDArray[np.uint8]: + """Convert the occupancy grid to a turbo-colored Image. + + Returns: + Image with turbo visualization of the occupancy grid + """ + color_lut = _turbo_lut() + + # 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) + + # Create BGR image using lookup table (vectorized operation) + return color_lut[lookup_indices] + + +def _interpolate_turbo(t: float) -> tuple[int, int, int]: + """D3's interpolateTurbo colormap implementation. + + Based on Anton Mikhailov's Turbo colormap using polynomial approximations. + + Args: + t: Value in [0, 1] + + Returns: + RGB tuple (0-255 range) + """ + t = max(0.0, min(1.0, t)) + + r = 34.61 + t * (1172.33 - t * (10793.56 - t * (33300.12 - t * (38394.49 - t * 14825.05)))) + g = 23.31 + t * (557.33 + t * (1225.33 - t * (3574.96 - t * (1073.77 + t * 707.56)))) + b = 27.2 + t * (3211.1 - t * (15327.97 - t * (27814.0 - t * (22569.18 - t * 6838.66)))) + + return ( + max(0, min(255, round(r))), + max(0, min(255, round(g))), + max(0, min(255, round(b))), + ) + + +@lru_cache(maxsize=1) +def _turbo_lut() -> NDArray[np.uint8]: + # Pre-compute lookup table for all possible values (-1 to 100) + color_lut = np.zeros((256, 3), dtype=np.uint8) + + for value in range(-1, 101): + # Normalize to [0, 1] range based on domain [-1, 100] + t = (value + 1) / 101.0 + + if value == -1: + rgb = (34, 24, 28) + elif value == 100: + rgb = (0, 0, 0) + else: + rgb = _interpolate_turbo(t * 2 - 1) + + # Map -1 to index 255, 0-100 to indices 0-100 + idx = 255 if value == -1 else value + color_lut[idx] = [rgb[2], rgb[1], rgb[0]] + + return color_lut diff --git a/dimos/mapping/occupancy/visualize_path.py b/dimos/mapping/occupancy/visualize_path.py new file mode 100644 index 0000000000..ec67eebf7c --- /dev/null +++ b/dimos/mapping/occupancy/visualize_path.py @@ -0,0 +1,89 @@ +# Copyright 2025 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 cv2 +import numpy as np + +from dimos.mapping.occupancy.visualizations import visualize_occupancy_grid +from dimos.msgs.nav_msgs import Path +from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid +from dimos.msgs.sensor_msgs.Image import Image +from dimos.msgs.sensor_msgs.image_impls.AbstractImage import ImageFormat + + +def visualize_path( + occupancy_grid: OccupancyGrid, + path: Path, + robot_width: float, + robot_length: float, + thickness: int = 1, + scale: int = 8, +) -> Image: + image = visualize_occupancy_grid(occupancy_grid, "rainbow") + bgr = image.data + + bgr = cv2.resize( + bgr, + (bgr.shape[1] * scale, bgr.shape[0] * scale), + interpolation=cv2.INTER_NEAREST, + ) + + # Convert robot dimensions from meters to grid cells, then to scaled pixels + resolution = occupancy_grid.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) + + # Get yaw angle from pose orientation + yaw = pose.yaw + + # Define rectangle corners centered at origin (length along x, width along y) + half_length = robot_length_px / 2 + half_width = robot_width_px / 2 + corners = np.array( + [ + [-half_length, -half_width], + [half_length, -half_width], + [half_length, half_width], + [-half_length, half_width], + ], + dtype=np.float32, + ) + + # Rotate corners by yaw angle + cos_yaw = np.cos(yaw) + sin_yaw = np.sin(yaw) + rotation_matrix = np.array([[cos_yaw, -sin_yaw], [sin_yaw, cos_yaw]]) + rotated_corners = corners @ rotation_matrix.T + + # Translate to center position + rotated_corners[:, 0] += cx + rotated_corners[:, 1] += cy + + # Draw the rotated rectangle + 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, + ) diff --git a/dimos/mapping/pointclouds/accumulators/general.py b/dimos/mapping/pointclouds/accumulators/general.py new file mode 100644 index 0000000000..21fe1f7b08 --- /dev/null +++ b/dimos/mapping/pointclouds/accumulators/general.py @@ -0,0 +1,77 @@ +# Copyright 2025 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 numpy as np +from open3d.geometry import PointCloud # type: ignore[import-untyped] +from open3d.io import read_point_cloud # type: ignore[import-untyped] + +from dimos.core.global_config import GlobalConfig # type: ignore[import-untyped] + + +class GeneralPointCloudAccumulator: + _point_cloud: PointCloud + _voxel_size: float + + def __init__(self, voxel_size: float, global_config: GlobalConfig) -> None: + self._point_cloud = PointCloud() + self._voxel_size = voxel_size + + if global_config.mujoco_global_map_from_pointcloud: + path = global_config.mujoco_global_map_from_pointcloud + self._point_cloud = read_point_cloud(path) + + def get_point_cloud(self) -> PointCloud: + return self._point_cloud + + def add(self, point_cloud: PointCloud) -> None: + """Voxelise *frame* and splice it into the running map.""" + new_pct = point_cloud.voxel_down_sample(voxel_size=self._voxel_size) + + # Skip for empty pointclouds. + if len(new_pct.points) == 0: + return + + self._point_cloud = _splice_cylinder(self._point_cloud, new_pct, shrink=0.5) + + +def _splice_cylinder( + map_pcd: PointCloud, + patch_pcd: PointCloud, + axis: int = 2, + shrink: float = 0.95, +) -> PointCloud: + center = patch_pcd.get_center() + patch_pts = np.asarray(patch_pcd.points) + + # Axes perpendicular to cylinder + axes = [0, 1, 2] + axes.remove(axis) + + planar_dists = np.linalg.norm(patch_pts[:, axes] - center[axes], axis=1) + radius = planar_dists.max() * shrink + + axis_min = (patch_pts[:, axis].min() - center[axis]) * shrink + center[axis] + axis_max = (patch_pts[:, axis].max() - center[axis]) * shrink + center[axis] + + map_pts = np.asarray(map_pcd.points) + planar_dists_map = np.linalg.norm(map_pts[:, axes] - center[axes], axis=1) + + victims = np.nonzero( + (planar_dists_map < radius) + & (map_pts[:, axis] >= axis_min) + & (map_pts[:, axis] <= axis_max) + )[0] + + survivors = map_pcd.select_by_index(victims, invert=True) + return survivors + patch_pcd diff --git a/dimos/mapping/pointclouds/accumulators/protocol.py b/dimos/mapping/pointclouds/accumulators/protocol.py new file mode 100644 index 0000000000..42b0b3ff39 --- /dev/null +++ b/dimos/mapping/pointclouds/accumulators/protocol.py @@ -0,0 +1,28 @@ +#!/usr/bin/env python3 +# Copyright 2025 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 typing import Protocol + +from open3d.geometry import PointCloud # type: ignore[import-untyped] + + +class PointCloudAccumulator(Protocol): + def get_point_cloud(self) -> PointCloud: + """Get the accumulated pointcloud.""" + ... + + def add(self, point_cloud: PointCloud) -> None: + """Add a pointcloud to the accumulator.""" + ... diff --git a/dimos/mapping/pointclouds/demo.py b/dimos/mapping/pointclouds/demo.py new file mode 100644 index 0000000000..abf52edd13 --- /dev/null +++ b/dimos/mapping/pointclouds/demo.py @@ -0,0 +1,86 @@ +#!/usr/bin/env python3 +# Copyright 2025 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 cv2 +from open3d.geometry import PointCloud # type: ignore[import-untyped] +import typer + +from dimos.mapping.occupancy.gradient import gradient +from dimos.mapping.occupancy.visualizations import visualize_occupancy_grid +from dimos.mapping.pointclouds.occupancy import simple_occupancy +from dimos.mapping.pointclouds.util import ( + height_colorize, + read_pointcloud, + visualize, +) +from dimos.msgs.nav_msgs import OccupancyGrid +from dimos.msgs.sensor_msgs import PointCloud2 +from dimos.utils.data import get_data + +app = typer.Typer() + + +def _get_sum_map() -> PointCloud: + return read_pointcloud(get_data("apartment") / "sum.ply") + + +def _get_occupancy_grid() -> OccupancyGrid: + resolution = 0.05 + min_height = 0.15 + max_height = 0.6 + occupancygrid = simple_occupancy( + PointCloud2(_get_sum_map()), + resolution=resolution, + min_height=min_height, + max_height=max_height, + ) + return occupancygrid + + +def _show_occupancy_grid(og: OccupancyGrid) -> None: + cost_map = visualize_occupancy_grid(og, "turbo").to_opencv() + cost_map = cv2.flip(cost_map, 0) + + # Resize to make the image larger (scale by 4x) + height, width = cost_map.shape[:2] + cost_map = cv2.resize(cost_map, (width * 4, height * 4), interpolation=cv2.INTER_NEAREST) + + cv2.namedWindow("Occupancy Grid", cv2.WINDOW_NORMAL) + cv2.imshow("Occupancy Grid", cost_map) + cv2.waitKey(0) + cv2.destroyAllWindows() + + +@app.command() +def view_sum() -> None: + pointcloud = _get_sum_map() + height_colorize(pointcloud) + visualize(pointcloud) + + +@app.command() +def view_map() -> None: + og = _get_occupancy_grid() + _show_occupancy_grid(og) + + +@app.command() +def view_map_inflated() -> None: + og = gradient(_get_occupancy_grid(), max_distance=1.5) + _show_occupancy_grid(og) + + +if __name__ == "__main__": + app() diff --git a/dimos/mapping/pointclouds/occupancy.py b/dimos/mapping/pointclouds/occupancy.py new file mode 100644 index 0000000000..849be722bc --- /dev/null +++ b/dimos/mapping/pointclouds/occupancy.py @@ -0,0 +1,268 @@ +# Copyright 2025 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 __future__ import annotations + +from typing import TYPE_CHECKING + +import numpy as np +from scipy import ndimage # type: ignore[import-untyped] + +from dimos.msgs.geometry_msgs import Pose +from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid + +if TYPE_CHECKING: + from dimos.msgs.sensor_msgs import PointCloud2 + + +def general_occupancy( + cloud: PointCloud2, + resolution: float = 0.05, + min_height: float = 0.1, + max_height: float = 2.0, + frame_id: str | None = None, + mark_free_radius: float = 0.4, +) -> OccupancyGrid: + """Create an OccupancyGrid from a PointCloud2 message. + + Args: + cloud: PointCloud2 message containing 3D points + resolution: Grid resolution in meters/cell (default: 0.05) + min_height: Minimum height threshold for including points (default: 0.1) + max_height: Maximum height threshold for including points (default: 2.0) + frame_id: Reference frame for the grid (default: uses cloud's frame_id) + mark_free_radius: Radius in meters around obstacles to mark as free space (default: 0.0) + If 0, only immediate neighbors are marked free. + Set to preserve unknown areas for exploration. + + Returns: + OccupancyGrid with occupied cells where points were projected + """ + + # Get points as numpy array + points = cloud.as_numpy() + + if len(points) == 0: + # Return empty grid + return OccupancyGrid( + width=1, height=1, resolution=resolution, frame_id=frame_id or cloud.frame_id + ) + + # Filter points by height for obstacles + obstacle_mask = (points[:, 2] >= min_height) & (points[:, 2] <= max_height) + obstacle_points = points[obstacle_mask] + + # Get points below min_height for marking as free space + ground_mask = points[:, 2] < min_height + ground_points = points[ground_mask] + + # Find bounds of the point cloud in X-Y plane (use all points) + if len(points) > 0: + min_x = np.min(points[:, 0]) + max_x = np.max(points[:, 0]) + min_y = np.min(points[:, 1]) + max_y = np.max(points[:, 1]) + else: + # Return empty grid if no points at all + return OccupancyGrid( + width=1, height=1, resolution=resolution, frame_id=frame_id or cloud.frame_id + ) + + # Add some padding around the bounds + padding = 1.0 # 1 meter padding + min_x -= padding + max_x += padding + min_y -= padding + max_y += padding + + # Calculate grid dimensions + width = int(np.ceil((max_x - min_x) / resolution)) + height = int(np.ceil((max_y - min_y) / resolution)) + + # Create origin pose (bottom-left corner of the grid) + origin = Pose() + origin.position.x = min_x + origin.position.y = min_y + origin.position.z = 0.0 + origin.orientation.w = 1.0 # No rotation + + # Initialize grid (all unknown) + grid = np.full((height, width), -1, dtype=np.int8) + + # First, mark ground points as free space + if len(ground_points) > 0: + ground_x = ((ground_points[:, 0] - min_x) / resolution).astype(np.int32) + ground_y = ((ground_points[:, 1] - min_y) / resolution).astype(np.int32) + + # Clip indices to grid bounds + ground_x = np.clip(ground_x, 0, width - 1) + ground_y = np.clip(ground_y, 0, height - 1) + + # Mark ground cells as free + grid[ground_y, ground_x] = 0 # Free space + + # Then mark obstacle points (will override ground if at same location) + if len(obstacle_points) > 0: + obs_x = ((obstacle_points[:, 0] - min_x) / resolution).astype(np.int32) + obs_y = ((obstacle_points[:, 1] - min_y) / resolution).astype(np.int32) + + # Clip indices to grid bounds + obs_x = np.clip(obs_x, 0, width - 1) + obs_y = np.clip(obs_y, 0, height - 1) + + # Mark cells as occupied + grid[obs_y, obs_x] = 100 # Lethal obstacle + + # Apply mark_free_radius to expand free space areas + if mark_free_radius > 0: + # Expand existing free space areas by the specified radius + # This will NOT expand from obstacles, only from free space + + free_mask = grid == 0 # Current free space + free_radius_cells = int(np.ceil(mark_free_radius / resolution)) + + # Create circular kernel + y, x = np.ogrid[ + -free_radius_cells : free_radius_cells + 1, + -free_radius_cells : free_radius_cells + 1, + ] + kernel = x**2 + y**2 <= free_radius_cells**2 + + # Dilate free space areas + expanded_free = ndimage.binary_dilation(free_mask, structure=kernel, iterations=1) + + # Mark expanded areas as free, but don't override obstacles + grid[expanded_free & (grid != 100)] = 0 + + # Create and return OccupancyGrid + # Get timestamp from cloud if available + ts = cloud.ts if hasattr(cloud, "ts") and cloud.ts is not None else 0.0 + + occupancy_grid = OccupancyGrid( + grid=grid, + resolution=resolution, + origin=origin, + frame_id=frame_id or cloud.frame_id, + ts=ts, + ) + + return occupancy_grid + + +def simple_occupancy( + cloud: PointCloud2, + resolution: float = 0.05, + min_height: float = 0.1, + max_height: float = 2.0, + frame_id: str | None = None, + closing_iterations: int = 1, + closing_connectivity: int = 2, +) -> OccupancyGrid: + points = cloud.as_numpy() + + if len(points) == 0: + return OccupancyGrid( + width=1, height=1, resolution=resolution, frame_id=frame_id or cloud.frame_id + ) + + # Filter points by height for obstacles + obstacle_mask = (points[:, 2] >= min_height) & (points[:, 2] <= max_height) + obstacle_points = points[obstacle_mask] + + # Get points below min_height for marking as free space + ground_mask = points[:, 2] < min_height + ground_points = points[ground_mask] + + # Find bounds of the point cloud in X-Y plane (use all points) + if len(points) > 0: + min_x = np.min(points[:, 0]) + max_x = np.max(points[:, 0]) + min_y = np.min(points[:, 1]) + max_y = np.max(points[:, 1]) + else: + # Return empty grid if no points at all + return OccupancyGrid( + width=1, height=1, resolution=resolution, frame_id=frame_id or cloud.frame_id + ) + + # Add some padding around the bounds + padding = 1.0 # 1 meter padding + min_x -= padding + max_x += padding + min_y -= padding + max_y += padding + + # Calculate grid dimensions + width = int(np.ceil((max_x - min_x) / resolution)) + height = int(np.ceil((max_y - min_y) / resolution)) + + # Create origin pose (bottom-left corner of the grid) + origin = Pose() + origin.position.x = min_x + origin.position.y = min_y + origin.position.z = 0.0 + origin.orientation.w = 1.0 # No rotation + + # Initialize grid (all unknown) + grid = np.full((height, width), -1, dtype=np.int8) + + # First, mark ground points as free space + if len(ground_points) > 0: + ground_x = np.round((ground_points[:, 0] - min_x) / resolution).astype(np.int32) + ground_y = np.round((ground_points[:, 1] - min_y) / resolution).astype(np.int32) + + # Clip indices to grid bounds + ground_x = np.clip(ground_x, 0, width - 1) + ground_y = np.clip(ground_y, 0, height - 1) + + # Mark ground cells as free + grid[ground_y, ground_x] = 0 # Free space + + # Then mark obstacle points (will override ground if at same location) + if len(obstacle_points) > 0: + obs_x = np.round((obstacle_points[:, 0] - min_x) / resolution).astype(np.int32) + obs_y = np.round((obstacle_points[:, 1] - min_y) / resolution).astype(np.int32) + + # Clip indices to grid bounds + obs_x = np.clip(obs_x, 0, width - 1) + obs_y = np.clip(obs_y, 0, height - 1) + + # Mark cells as occupied + grid[obs_y, obs_x] = 100 # Lethal obstacle + + # Fill small gaps in occupied regions using morphological closing + occupied_mask = grid == 100 + if np.any(occupied_mask) and closing_iterations > 0: + # connectivity=1 gives 4-connectivity, connectivity=2 gives 8-connectivity + structure = ndimage.generate_binary_structure(2, closing_connectivity) + # Closing = dilation then erosion - fills small holes + closed_mask = ndimage.binary_closing( + occupied_mask, structure=structure, iterations=closing_iterations + ) + # Fill gaps (both unknown and free space) + grid[closed_mask] = 100 + + # Create and return OccupancyGrid + # Get timestamp from cloud if available + ts = cloud.ts if hasattr(cloud, "ts") and cloud.ts is not None else 0.0 + + occupancy_grid = OccupancyGrid( + grid=grid, + resolution=resolution, + origin=origin, + frame_id=frame_id or cloud.frame_id, + ts=ts, + ) + + return occupancy_grid diff --git a/dimos/mapping/pointclouds/test_occupancy.py b/dimos/mapping/pointclouds/test_occupancy.py new file mode 100644 index 0000000000..6c057527d9 --- /dev/null +++ b/dimos/mapping/pointclouds/test_occupancy.py @@ -0,0 +1,48 @@ +#!/usr/bin/env python3 +# Copyright 2025 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 cv2 +import numpy as np +from open3d.geometry import PointCloud # type: ignore[import-untyped] +import pytest + +from dimos.mapping.pointclouds.occupancy import general_occupancy, simple_occupancy +from dimos.mapping.pointclouds.util import read_pointcloud +from dimos.msgs.sensor_msgs import PointCloud2 +from dimos.utils.data import get_data + + +@pytest.fixture +def apartment() -> PointCloud: + return read_pointcloud(get_data("apartment") / "sum.ply") + + +@pytest.mark.parametrize( + "occupancy_fn,output_name", + [ + (general_occupancy, "occupancy_general.png"), + (simple_occupancy, "occupancy_simple.png"), + ], +) +def test_occupancy(apartment: PointCloud, occupancy_fn, output_name: str) -> None: + expected_image = cv2.imread(str(get_data(output_name)), cv2.IMREAD_GRAYSCALE) + cloud = PointCloud2.from_numpy(np.asarray(apartment.points), frame_id="map") + + occupancy_grid = occupancy_fn(cloud) + + # Convert grid from -1..100 to 0..101 for PNG + computed_image = (occupancy_grid.grid + 1).astype(np.uint8) + + np.testing.assert_array_equal(computed_image, expected_image) diff --git a/dimos/mapping/pointclouds/util.py b/dimos/mapping/pointclouds/util.py new file mode 100644 index 0000000000..e401b4c532 --- /dev/null +++ b/dimos/mapping/pointclouds/util.py @@ -0,0 +1,58 @@ +# Copyright 2025 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 collections.abc import Iterable +import colorsys +from pathlib import Path + +import numpy as np +import open3d as o3d # type: ignore[import-untyped] +from open3d.geometry import PointCloud # type: ignore[import-untyped] + + +def read_pointcloud(path: Path) -> PointCloud: + return o3d.io.read_point_cloud(path) + + +def sum_pointclouds(pointclouds: Iterable[PointCloud]) -> PointCloud: + it = iter(pointclouds) + ret = next(it) + for x in it: + ret += x + return ret.remove_duplicated_points() + + +def height_colorize(pointcloud: PointCloud) -> None: + points = np.asarray(pointcloud.points) + z_values = points[:, 2] + z_min = z_values.min() + z_max = z_values.max() + + z_normalized = (z_values - z_min) / (z_max - z_min) + + # Create rainbow color map. + colors = np.array([colorsys.hsv_to_rgb(0.7 * (1 - h), 1.0, 1.0) for h in z_normalized]) + + pointcloud.colors = o3d.utility.Vector3dVector(colors) + + +def visualize(pointcloud: PointCloud) -> None: + voxel_size = 0.05 # 0.05m voxels + voxel_grid = o3d.geometry.VoxelGrid.create_from_point_cloud(pointcloud, voxel_size=voxel_size) + o3d.visualization.draw_geometries( + [voxel_grid], + window_name="Combined Point Clouds (Voxelized)", + width=1024, + height=768, + ) diff --git a/dimos/models/__init__.py b/dimos/models/__init__.py index 422ec8eb23..d8e2e14341 100644 --- a/dimos/models/__init__.py +++ b/dimos/models/__init__.py @@ -1,3 +1,3 @@ from dimos.models.base import HuggingFaceModel, LocalModel -__all__ = ["LocalModel", "HuggingFaceModel"] +__all__ = ["HuggingFaceModel", "LocalModel"] diff --git a/dimos/models/vl/__init__.py b/dimos/models/vl/__init__.py index 52467b61a5..6f120f9141 100644 --- a/dimos/models/vl/__init__.py +++ b/dimos/models/vl/__init__.py @@ -7,8 +7,8 @@ __all__ = [ "Captioner", "Florence2Model", - "VlModel", - "MoondreamVlModel", "MoondreamHostedVlModel", + "MoondreamVlModel", "QwenVlModel", + "VlModel", ] diff --git a/dimos/models/vl/test_vlm.py b/dimos/models/vl/test_vlm.py index e44d71e2f9..c03d001284 100644 --- a/dimos/models/vl/test_vlm.py +++ b/dimos/models/vl/test_vlm.py @@ -11,7 +11,7 @@ from dimos.models.vl.qwen import QwenVlModel from dimos.msgs.sensor_msgs import Image from dimos.perception.detection.type import ImageDetections2D -from dimos.utils.cli.plot import Plot, bar +from dimos.utils.cli.plot import bar from dimos.utils.data import get_data if TYPE_CHECKING: diff --git a/dimos/msgs/nav_msgs/OccupancyGrid.py b/dimos/msgs/nav_msgs/OccupancyGrid.py index e66ec4fb1d..6f91b2f9f0 100644 --- a/dimos/msgs/nav_msgs/OccupancyGrid.py +++ b/dimos/msgs/nav_msgs/OccupancyGrid.py @@ -24,13 +24,15 @@ ) from dimos_lcm.std_msgs import Time as LCMTime # type: ignore[import-untyped] import numpy as np -from scipy import ndimage # type: ignore[import-untyped] +from PIL import Image from dimos.msgs.geometry_msgs import Pose, Vector3, VectorLike from dimos.types.timestamped import Timestamped if TYPE_CHECKING: - from dimos.msgs.sensor_msgs import PointCloud2 + from pathlib import Path + + from numpy.typing import NDArray class CostValues(IntEnum): @@ -59,11 +61,11 @@ class OccupancyGrid(Timestamped): ts: float frame_id: str info: MapMetaData - grid: np.ndarray # type: ignore[type-arg] + grid: NDArray[np.int8] def __init__( self, - grid: np.ndarray | None = None, # type: ignore[type-arg] + grid: NDArray[np.int8] | None = None, width: int | None = None, height: int | None = None, resolution: float = 0.05, @@ -175,40 +177,16 @@ def unknown_percent(self) -> float: """Percentage of cells that are unknown.""" return (self.unknown_cells / self.total_cells * 100) if self.total_cells > 0 else 0.0 - def inflate(self, radius: float) -> OccupancyGrid: - """Inflate obstacles by a given radius (binary inflation). - Args: - radius: Inflation radius in meters - Returns: - New OccupancyGrid with inflated obstacles - """ - # Convert radius to grid cells - cell_radius = int(np.ceil(radius / self.resolution)) - - # Get grid as numpy array - grid_array = self.grid - - # Create circular kernel for binary inflation - 2 * cell_radius + 1 - y, x = np.ogrid[-cell_radius : cell_radius + 1, -cell_radius : cell_radius + 1] - kernel = (x**2 + y**2 <= cell_radius**2).astype(np.uint8) - - # Find occupied cells - occupied_mask = grid_array >= CostValues.OCCUPIED - - # Binary inflation - inflated = ndimage.binary_dilation(occupied_mask, structure=kernel) - result_grid = grid_array.copy() - result_grid[inflated] = CostValues.OCCUPIED - - # Create new OccupancyGrid with inflated data using numpy constructor - return OccupancyGrid( - grid=result_grid, - resolution=self.resolution, - origin=self.origin, - frame_id=self.frame_id, - ts=self.ts, - ) + @classmethod + def from_path(cls, path: Path) -> OccupancyGrid: + match path.suffix.lower(): + case ".npy": + return cls(grid=np.load(path)) + case ".png": + img = Image.open(path).convert("L") + return cls(grid=np.array(img).astype(np.int8)) + case _: + raise NotImplementedError(f"Unsupported file format: {path.suffix}") def world_to_grid(self, point: VectorLike) -> Vector3: """Convert world coordinates to grid coordinates. @@ -329,201 +307,6 @@ def lcm_decode(cls, data: bytes | BinaryIO) -> OccupancyGrid: instance.info = lcm_msg.info return instance - @classmethod - def from_pointcloud( - cls, - cloud: PointCloud2, - resolution: float = 0.05, - min_height: float = 0.1, - max_height: float = 2.0, - frame_id: str | None = None, - mark_free_radius: float = 0.4, - ) -> OccupancyGrid: - """Create an OccupancyGrid from a PointCloud2 message. - - Args: - cloud: PointCloud2 message containing 3D points - resolution: Grid resolution in meters/cell (default: 0.05) - min_height: Minimum height threshold for including points (default: 0.1) - max_height: Maximum height threshold for including points (default: 2.0) - frame_id: Reference frame for the grid (default: uses cloud's frame_id) - mark_free_radius: Radius in meters around obstacles to mark as free space (default: 0.0) - If 0, only immediate neighbors are marked free. - Set to preserve unknown areas for exploration. - - Returns: - OccupancyGrid with occupied cells where points were projected - """ - - # Get points as numpy array - points = cloud.as_numpy() - - if len(points) == 0: - # Return empty grid - return cls( - width=1, height=1, resolution=resolution, frame_id=frame_id or cloud.frame_id - ) - - # Filter points by height for obstacles - obstacle_mask = (points[:, 2] >= min_height) & (points[:, 2] <= max_height) - obstacle_points = points[obstacle_mask] - - # Get points below min_height for marking as free space - ground_mask = points[:, 2] < min_height - ground_points = points[ground_mask] - - # Find bounds of the point cloud in X-Y plane (use all points) - if len(points) > 0: - min_x = np.min(points[:, 0]) - max_x = np.max(points[:, 0]) - min_y = np.min(points[:, 1]) - max_y = np.max(points[:, 1]) - else: - # Return empty grid if no points at all - return cls( - width=1, height=1, resolution=resolution, frame_id=frame_id or cloud.frame_id - ) - - # Add some padding around the bounds - padding = 1.0 # 1 meter padding - min_x -= padding - max_x += padding - min_y -= padding - max_y += padding - - # Calculate grid dimensions - width = int(np.ceil((max_x - min_x) / resolution)) - height = int(np.ceil((max_y - min_y) / resolution)) - - # Create origin pose (bottom-left corner of the grid) - origin = Pose() - origin.position.x = min_x - origin.position.y = min_y - origin.position.z = 0.0 - origin.orientation.w = 1.0 # No rotation - - # Initialize grid (all unknown) - grid = np.full((height, width), -1, dtype=np.int8) - - # First, mark ground points as free space - if len(ground_points) > 0: - ground_x = ((ground_points[:, 0] - min_x) / resolution).astype(np.int32) - ground_y = ((ground_points[:, 1] - min_y) / resolution).astype(np.int32) - - # Clip indices to grid bounds - ground_x = np.clip(ground_x, 0, width - 1) - ground_y = np.clip(ground_y, 0, height - 1) - - # Mark ground cells as free - grid[ground_y, ground_x] = 0 # Free space - - # Then mark obstacle points (will override ground if at same location) - if len(obstacle_points) > 0: - obs_x = ((obstacle_points[:, 0] - min_x) / resolution).astype(np.int32) - obs_y = ((obstacle_points[:, 1] - min_y) / resolution).astype(np.int32) - - # Clip indices to grid bounds - obs_x = np.clip(obs_x, 0, width - 1) - obs_y = np.clip(obs_y, 0, height - 1) - - # Mark cells as occupied - grid[obs_y, obs_x] = 100 # Lethal obstacle - - # Apply mark_free_radius to expand free space areas - if mark_free_radius > 0: - # Expand existing free space areas by the specified radius - # This will NOT expand from obstacles, only from free space - - free_mask = grid == 0 # Current free space - free_radius_cells = int(np.ceil(mark_free_radius / resolution)) - - # Create circular kernel - y, x = np.ogrid[ - -free_radius_cells : free_radius_cells + 1, - -free_radius_cells : free_radius_cells + 1, - ] - kernel = x**2 + y**2 <= free_radius_cells**2 - - # Dilate free space areas - expanded_free = ndimage.binary_dilation(free_mask, structure=kernel, iterations=1) - - # Mark expanded areas as free, but don't override obstacles - grid[expanded_free & (grid != 100)] = 0 - - # Create and return OccupancyGrid - # Get timestamp from cloud if available - ts = cloud.ts if hasattr(cloud, "ts") and cloud.ts is not None else 0.0 - - occupancy_grid = cls( - grid=grid, - resolution=resolution, - origin=origin, - frame_id=frame_id or cloud.frame_id, - ts=ts, - ) - - return occupancy_grid - - def gradient(self, obstacle_threshold: int = 50, max_distance: float = 2.0) -> OccupancyGrid: - """Create a gradient OccupancyGrid for path planning. - - Creates a gradient where free space has value 0 and values increase near obstacles. - This can be used as a cost map for path planning algorithms like A*. - - Args: - obstacle_threshold: Cell values >= this are considered obstacles (default: 50) - max_distance: Maximum distance to compute gradient in meters (default: 2.0) - - Returns: - New OccupancyGrid with gradient values: - - -1: Unknown cells (preserved as-is) - - 0: Free space far from obstacles - - 1-99: Increasing cost as you approach obstacles - - 100: At obstacles - - Note: Unknown cells remain as unknown (-1) and do not receive gradient values. - """ - - # Remember which cells are unknown - unknown_mask = self.grid == CostValues.UNKNOWN - - # Create binary obstacle map - # Consider cells >= threshold as obstacles (1), everything else as free (0) - # Unknown cells are not considered obstacles for distance calculation - obstacle_map = (self.grid >= obstacle_threshold).astype(np.float32) - - # Compute distance transform (distance to nearest obstacle in cells) - # Unknown cells are treated as if they don't exist for distance calculation - distance_cells = ndimage.distance_transform_edt(1 - obstacle_map) - - # Convert to meters and clip to max distance - distance_meters = np.clip(distance_cells * self.resolution, 0, max_distance) # type: ignore[operator] - - # Invert and scale to 0-100 range - # Far from obstacles (max_distance) -> 0 - # At obstacles (0 distance) -> 100 - gradient_values = (1 - distance_meters / max_distance) * 100 - - # Ensure obstacles are exactly 100 - gradient_values[obstacle_map > 0] = CostValues.OCCUPIED - - # Convert to int8 for OccupancyGrid - gradient_data = gradient_values.astype(np.int8) - - # Preserve unknown cells as unknown (don't apply gradient to them) - gradient_data[unknown_mask] = CostValues.UNKNOWN - - # Create new OccupancyGrid with gradient - gradient_grid = OccupancyGrid( - grid=gradient_data, - resolution=self.resolution, - origin=self.origin, - frame_id=self.frame_id, - ts=self.ts, - ) - - return gradient_grid - def filter_above(self, threshold: int) -> OccupancyGrid: """Create a new OccupancyGrid with only values above threshold. @@ -609,3 +392,17 @@ def max(self) -> OccupancyGrid: ) return maxed + + def copy(self) -> OccupancyGrid: + """Create a deep copy of the OccupancyGrid. + + Returns: + A new OccupancyGrid instance with copied data. + """ + return OccupancyGrid( + grid=self.grid.copy(), + resolution=self.resolution, + origin=self.origin, + frame_id=self.frame_id, + ts=self.ts, + ) diff --git a/dimos/msgs/nav_msgs/test_OccupancyGrid.py b/dimos/msgs/nav_msgs/test_OccupancyGrid.py index a4cd36f9c0..247aa9cc7f 100644 --- a/dimos/msgs/nav_msgs/test_OccupancyGrid.py +++ b/dimos/msgs/nav_msgs/test_OccupancyGrid.py @@ -20,6 +20,9 @@ import numpy as np import pytest +from dimos.mapping.occupancy.gradient import gradient +from dimos.mapping.occupancy.inflation import simple_inflate +from dimos.mapping.pointclouds.occupancy import general_occupancy from dimos.msgs.geometry_msgs import Pose from dimos.msgs.nav_msgs import OccupancyGrid from dimos.msgs.sensor_msgs import PointCloud2 @@ -177,11 +180,9 @@ def test_from_pointcloud() -> None: pointcloud = PointCloud2.lcm_decode(lcm_msg) # Convert pointcloud to occupancy grid - occupancygrid = OccupancyGrid.from_pointcloud( - pointcloud, resolution=0.05, min_height=0.1, max_height=2.0 - ) + occupancygrid = general_occupancy(pointcloud, resolution=0.05, min_height=0.1, max_height=2.0) # Apply inflation separately if needed - occupancygrid = occupancygrid.inflate(0.1) + occupancygrid = simple_inflate(occupancygrid, 0.1) # Check that grid was created with reasonable properties assert occupancygrid.width > 0 @@ -200,7 +201,7 @@ def test_gradient() -> None: grid = OccupancyGrid(grid=data, resolution=0.1) # 0.1m per cell # Convert to gradient - gradient_grid = grid.gradient(obstacle_threshold=50, max_distance=1.0) + gradient_grid = gradient(grid, obstacle_threshold=50, max_distance=1.0) # Check that we get an OccupancyGrid back assert isinstance(gradient_grid, OccupancyGrid) @@ -229,7 +230,7 @@ def test_gradient() -> None: data_with_unknown[8:10, 8:10] = -1 # Add unknown area (far from obstacle) grid_with_unknown = OccupancyGrid(data_with_unknown, resolution=0.1) - gradient_with_unknown = grid_with_unknown.gradient(max_distance=1.0) # 1m max distance + gradient_with_unknown = gradient(grid_with_unknown, max_distance=1.0) # 1m max distance # Unknown cells should remain unknown (new behavior - unknowns are preserved) assert gradient_with_unknown.grid[0, 0] == -1 # Should remain unknown @@ -375,14 +376,12 @@ def test_lcm_broadcast() -> None: pointcloud = PointCloud2.lcm_decode(lcm_msg) # Create occupancy grid from pointcloud - occupancygrid = OccupancyGrid.from_pointcloud( - pointcloud, resolution=0.05, min_height=0.1, max_height=2.0 - ) + occupancygrid = general_occupancy(pointcloud, resolution=0.05, min_height=0.1, max_height=2.0) # Apply inflation separately if needed - occupancygrid = occupancygrid.inflate(0.1) + occupancygrid = simple_inflate(occupancygrid, 0.1) # Create gradient field with larger max_distance for better visualization - gradient_grid = occupancygrid.gradient(obstacle_threshold=70, max_distance=2.0) + gradient_grid = gradient(occupancygrid, obstacle_threshold=70, max_distance=2.0) # Debug: Print actual values to see the difference print("\n=== DEBUG: Comparing grids ===") diff --git a/dimos/navigation/bt_navigator/goal_validator.py b/dimos/navigation/bt_navigator/goal_validator.py index cdf86ccac5..055ece9fdc 100644 --- a/dimos/navigation/bt_navigator/goal_validator.py +++ b/dimos/navigation/bt_navigator/goal_validator.py @@ -16,6 +16,7 @@ import numpy as np +from dimos.mapping.occupancy.gradient import gradient from dimos.msgs.geometry_msgs import Vector3, VectorLike from dimos.msgs.nav_msgs import CostValues, OccupancyGrid @@ -54,6 +55,15 @@ def find_safe_goal( max_search_distance, connectivity_check_radius, ) + elif algorithm == "bfs_contiguous": + return _find_safe_goal_bfs_contiguous( + costmap, + goal, + cost_threshold, + min_clearance, + max_search_distance, + connectivity_check_radius, + ) elif algorithm == "spiral": return _find_safe_goal_spiral( costmap, @@ -144,6 +154,73 @@ def _find_safe_goal_bfs( return None +def _find_safe_goal_bfs_contiguous( + costmap: OccupancyGrid, + goal: VectorLike, + cost_threshold: int, + min_clearance: float, + max_search_distance: float, + connectivity_check_radius: int, +) -> Vector3 | None: + """ + BFS-based search for nearest safe goal position, only following passable cells. + Unlike regular BFS, this only expands through cells with occupancy < 100, + ensuring the path doesn't cross through impassable obstacles. + + Pros: + - Guarantees finding the closest safe position reachable without crossing obstacles + - Ensures connectivity to the goal through passable space + - Good for finding safe positions in the same "room" or connected area + + Cons: + - May not find nearby safe spots if they're on the other side of a wall + - Slightly slower than regular BFS due to additional checks + """ + + # Convert goal to grid coordinates + goal_grid = costmap.world_to_grid(goal) + gx, gy = int(goal_grid.x), int(goal_grid.y) + + # Convert distances to grid cells + clearance_cells = int(np.ceil(min_clearance / costmap.resolution)) + max_search_cells = int(np.ceil(max_search_distance / costmap.resolution)) + + # BFS queue and visited set + queue = deque([(gx, gy, 0)]) + visited = set([(gx, gy)]) + + # 8-connected neighbors + neighbors = [(0, 1), (1, 0), (0, -1), (-1, 0), (1, 1), (1, -1), (-1, 1), (-1, -1)] + + while queue: + x, y, dist = queue.popleft() + + # Check if we've exceeded max search distance + if dist > max_search_cells: + break + + # Check if position is valid + if _is_position_safe( + costmap, x, y, cost_threshold, clearance_cells, connectivity_check_radius + ): + # Convert back to world coordinates + return costmap.grid_to_world((x, y)) + + # Add neighbors to queue + for dx, dy in neighbors: + nx, ny = x + dx, y + dy + + # Check bounds + if 0 <= nx < costmap.width and 0 <= ny < costmap.height: + if (nx, ny) not in visited: + # Only expand through passable cells (occupancy < 100) + if costmap.grid[ny, nx] < 100: + visited.add((nx, ny)) + queue.append((nx, ny, dist + 1)) + + return None + + def _find_safe_goal_spiral( costmap: OccupancyGrid, goal: VectorLike, @@ -310,8 +387,8 @@ def _find_safe_goal_gradient( # Create gradient if needed (assuming costmap might already be a gradient) if np.all((costmap.grid == 0) | (costmap.grid == 100) | (costmap.grid == -1)): # Binary map, create gradient - gradient_map = costmap.gradient( - obstacle_threshold=cost_threshold, max_distance=min_clearance * 2 + gradient_map = gradient( + costmap, obstacle_threshold=cost_threshold, max_distance=min_clearance * 2 ) grid = gradient_map.grid else: diff --git a/dimos/navigation/bt_navigator/navigator.py b/dimos/navigation/bt_navigator/navigator.py index afcc86b96a..192b91c9e1 100644 --- a/dimos/navigation/bt_navigator/navigator.py +++ b/dimos/navigation/bt_navigator/navigator.py @@ -27,6 +27,8 @@ from dimos.core import In, Module, Out, rpc from dimos.core.rpc_client import RpcCall +from dimos.mapping.occupancy.gradient import gradient +from dimos.mapping.occupancy.inflation import simple_inflate from dimos.msgs.geometry_msgs import PoseStamped from dimos.msgs.nav_msgs import OccupancyGrid from dimos.navigation.base import NavigationInterface, NavigationState @@ -285,7 +287,7 @@ def _control_loop(self) -> None: self.cancel_goal() continue - costmap = self.latest_costmap.inflate(0.1).gradient(max_distance=1.0) + costmap = gradient(simple_inflate(self.latest_costmap, 0.1), max_distance=1.0) # Find safe goal position safe_goal_pos = find_safe_goal( diff --git a/dimos/navigation/bt_navigator/test_goal_validator.py b/dimos/navigation/bt_navigator/test_goal_validator.py new file mode 100644 index 0000000000..e81cf15b93 --- /dev/null +++ b/dimos/navigation/bt_navigator/test_goal_validator.py @@ -0,0 +1,53 @@ +# Copyright 2025 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 numpy as np +import pytest + +from dimos.msgs.geometry_msgs import Vector3 +from dimos.msgs.nav_msgs.OccupancyGrid import CostValues, OccupancyGrid +from dimos.navigation.bt_navigator.goal_validator import find_safe_goal +from dimos.utils.data import get_data + + +@pytest.fixture +def costmap() -> OccupancyGrid: + return OccupancyGrid(np.load(get_data("occupancy_simple.npy"))) + + +@pytest.mark.parametrize( + "input_pos,expected_pos", + [ + # Identical. + ((6.15, 10.0), (6.15, 10.0)), + # Very slightly off. + ((6.0, 10.0), (6.05, 10.0)), + # Don't pick a spot that's the closest, but is actually on the other side of the wall. + ((5.0, 9.0), (5.85, 9.6)), + ], +) +def test_find_safe_goal(costmap, input_pos, expected_pos) -> None: + goal = Vector3(input_pos[0], input_pos[1], 0.0) + + safe_goal = find_safe_goal( + costmap, + goal, + algorithm="bfs_contiguous", + cost_threshold=CostValues.OCCUPIED, + min_clearance=0.3, + max_search_distance=5.0, + connectivity_check_radius=0, + ) + + assert safe_goal == Vector3(expected_pos[0], expected_pos[1], 0.0) diff --git a/dimos/navigation/frontier_exploration/wavefront_frontier_goal_selector.py b/dimos/navigation/frontier_exploration/wavefront_frontier_goal_selector.py index e1701a66cd..282182dc1e 100644 --- a/dimos/navigation/frontier_exploration/wavefront_frontier_goal_selector.py +++ b/dimos/navigation/frontier_exploration/wavefront_frontier_goal_selector.py @@ -29,6 +29,7 @@ from reactivex.disposable import Disposable from dimos.core import In, Module, Out, rpc +from dimos.mapping.occupancy.inflation import simple_inflate from dimos.msgs.geometry_msgs import PoseStamped, Vector3 from dimos.msgs.nav_msgs import CostValues, OccupancyGrid from dimos.utils.logging_config import setup_logger @@ -762,7 +763,7 @@ def _exploration_loop(self) -> None: ) # Get exploration goal - costmap = self.latest_costmap.inflate(0.25) + costmap = simple_inflate(self.latest_costmap, 0.25) goal = self.get_exploration_goal(robot_pose, costmap) if goal: diff --git a/dimos/navigation/global_planner/__init__.py b/dimos/navigation/global_planner/__init__.py deleted file mode 100644 index 275619659b..0000000000 --- a/dimos/navigation/global_planner/__init__.py +++ /dev/null @@ -1,4 +0,0 @@ -from dimos.navigation.global_planner.algo import astar -from dimos.navigation.global_planner.planner import AstarPlanner, astar_planner - -__all__ = ["AstarPlanner", "astar", "astar_planner"] diff --git a/dimos/navigation/global_planner/astar.py b/dimos/navigation/global_planner/astar.py new file mode 100644 index 0000000000..1593004ab2 --- /dev/null +++ b/dimos/navigation/global_planner/astar.py @@ -0,0 +1,52 @@ +# Copyright 2025 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 dimos.msgs.geometry_msgs import VectorLike +from dimos.msgs.nav_msgs import OccupancyGrid, Path +from dimos.navigation.global_planner.general_astar import general_astar +from dimos.navigation.global_planner.min_cost_astar import min_cost_astar +from dimos.navigation.global_planner.types import AStarAlgorithm +from dimos.utils.logging_config import setup_logger + +logger = setup_logger() + + +def astar( + algorithm: AStarAlgorithm, + costmap: OccupancyGrid, + goal: VectorLike, + start: VectorLike, + use_cpp: bool = True, +) -> Path | None: + """ + A* path planning algorithm from start to goal position. + + Args: + algorithm: The A* algorithm variant to use ("general" or "min_cost") + costmap: Costmap object containing the environment + goal: Goal position as any vector-like object + start: Start position as any vector-like object (default: origin [0,0]) + use_cpp: Use C++ implementation for min_cost algorithm if available (default: True) + + Returns: + Path object containing waypoints, or None if no path found + """ + + match algorithm: + case "general": + return general_astar(costmap, goal, start) + case "min_cost": + return min_cost_astar(costmap, goal, start, use_cpp=use_cpp) + case _: + raise NotImplementedError() diff --git a/dimos/navigation/global_planner/algo.py b/dimos/navigation/global_planner/general_astar.py similarity index 99% rename from dimos/navigation/global_planner/algo.py rename to dimos/navigation/global_planner/general_astar.py index aecfcfba83..245786f655 100644 --- a/dimos/navigation/global_planner/algo.py +++ b/dimos/navigation/global_planner/general_astar.py @@ -21,7 +21,7 @@ logger = setup_logger() -def astar( +def general_astar( costmap: OccupancyGrid, goal: VectorLike, start: VectorLike = (0.0, 0.0), diff --git a/dimos/navigation/global_planner/min_cost_astar.py b/dimos/navigation/global_planner/min_cost_astar.py new file mode 100644 index 0000000000..1300a58fd2 --- /dev/null +++ b/dimos/navigation/global_planner/min_cost_astar.py @@ -0,0 +1,227 @@ +# Copyright 2025 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 heapq + +from dimos.msgs.geometry_msgs import PoseStamped, Quaternion, VectorLike +from dimos.msgs.nav_msgs import CostValues, OccupancyGrid, Path +from dimos.utils.logging_config import setup_logger + +# Try to import C++ extension for faster pathfinding +try: + from dimos.navigation.global_planner.min_cost_astar_ext import ( + min_cost_astar_cpp as _astar_cpp, + ) + + _USE_CPP = True +except ImportError: + _USE_CPP = False + +logger = setup_logger() + +# Define possible movements (8-connected grid with diagonal movements) +_directions = [ + (0, 1), + (1, 0), + (0, -1), + (-1, 0), + (1, 1), + (1, -1), + (-1, 1), + (-1, -1), +] + +# Cost for each movement (straight vs diagonal) +_sc = 1.0 # Straight cost +_dc = 1.42 # Diagonal cost (approximately sqrt(2)) +_movement_costs = [_sc, _sc, _sc, _sc, _dc, _dc, _dc, _dc] + + +# Heuristic function (Octile distance for 8-connected grid) +def _heuristic(x1: int, y1: int, x2: int, y2: int) -> float: + dx = abs(x2 - x1) + dy = abs(y2 - y1) + # Octile distance: optimal for 8-connected grids with diagonal movement + return (dx + dy) + (_dc - 2 * _sc) * min(dx, dy) + + +def _reconstruct_path( + parents: dict[tuple[int, int], tuple[int, int]], + current: tuple[int, int], + costmap: OccupancyGrid, + start_tuple: tuple[int, int], + goal_tuple: tuple[int, int], +) -> Path: + waypoints: list[PoseStamped] = [] + while current in parents: + world_point = costmap.grid_to_world(current) + pose = PoseStamped( + frame_id="world", + position=[world_point.x, world_point.y, 0.0], + orientation=Quaternion(0, 0, 0, 1), # Identity quaternion + ) + waypoints.append(pose) + current = parents[current] + + start_world_point = costmap.grid_to_world(start_tuple) + start_pose = PoseStamped( + frame_id="world", + position=[start_world_point.x, start_world_point.y, 0.0], + orientation=Quaternion(0, 0, 0, 1), + ) + waypoints.append(start_pose) + + waypoints.reverse() + + # Add the goal position if it's not already included + goal_point = costmap.grid_to_world(goal_tuple) + + if ( + not waypoints + or (waypoints[-1].x - goal_point.x) ** 2 + (waypoints[-1].y - goal_point.y) ** 2 > 1e-10 + ): + goal_pose = PoseStamped( + frame_id="world", + position=[goal_point.x, goal_point.y, 0.0], + orientation=Quaternion(0, 0, 0, 1), + ) + waypoints.append(goal_pose) + + return Path(frame_id="world", poses=waypoints) + + +def _reconstruct_path_from_coords( + path_coords: list[tuple[int, int]], + costmap: OccupancyGrid, +) -> Path: + waypoints: list[PoseStamped] = [] + + for gx, gy in path_coords: + world_point = costmap.grid_to_world((gx, gy)) + pose = PoseStamped( + frame_id="world", + position=[world_point.x, world_point.y, 0.0], + orientation=Quaternion(0, 0, 0, 1), + ) + waypoints.append(pose) + + return Path(frame_id="world", poses=waypoints) + + +def min_cost_astar( + costmap: OccupancyGrid, + goal: VectorLike, + start: VectorLike = (0.0, 0.0), + cost_threshold: int = 100, + unknown_penalty: float = 0.8, + use_cpp: bool = True, +) -> Path | None: + start_vector = costmap.world_to_grid(start) + goal_vector = costmap.world_to_grid(goal) + + start_tuple = (int(start_vector.x), int(start_vector.y)) + goal_tuple = (int(goal_vector.x), int(goal_vector.y)) + + if not (0 <= goal_tuple[0] < costmap.width and 0 <= goal_tuple[1] < costmap.height): + return None + + if use_cpp: + if _USE_CPP: + path_coords = _astar_cpp( + costmap.grid, + start_tuple[0], + start_tuple[1], + goal_tuple[0], + goal_tuple[1], + cost_threshold, + unknown_penalty, + ) + if not path_coords: + return None + return _reconstruct_path_from_coords(path_coords, costmap) + else: + logger.warning("C++ A* module could not be imported. Using Python.") + + open_set: list[tuple[float, float, tuple[int, int]]] = [] # Priority queue for nodes to explore + closed_set: set[tuple[int, int]] = set() # Set of explored nodes + + # Dictionary to store cost and distance from start, and parents for each node + # Track cumulative cell cost and path length separately + cost_score: dict[tuple[int, int], float] = {start_tuple: 0.0} # Cumulative cell cost + dist_score: dict[tuple[int, int], float] = {start_tuple: 0.0} # Cumulative path length + parents: dict[tuple[int, int], tuple[int, int]] = {} + + # Start with the starting node + # Priority: (total_cost + heuristic_cost, total_distance + heuristic_distance, node) + h_dist = _heuristic(start_tuple[0], start_tuple[1], goal_tuple[0], goal_tuple[1]) + heapq.heappush(open_set, (0.0, h_dist, start_tuple)) + + while open_set: + _, _, current = heapq.heappop(open_set) + current_x, current_y = current + + if current in closed_set: + continue + + if current == goal_tuple: + return _reconstruct_path(parents, current, costmap, start_tuple, goal_tuple) + + closed_set.add(current) + + for i, (dx, dy) in enumerate(_directions): + neighbor_x, neighbor_y = current_x + dx, current_y + dy + neighbor = (neighbor_x, neighbor_y) + + if not (0 <= neighbor_x < costmap.width and 0 <= neighbor_y < costmap.height): + continue + + if neighbor in closed_set: + continue + + neighbor_val = costmap.grid[neighbor_y, neighbor_x] + + if neighbor_val >= cost_threshold: + continue + + if neighbor_val == CostValues.UNKNOWN: + # Unknown cells have a moderate traversal cost + cell_cost = cost_threshold * unknown_penalty + elif neighbor_val == CostValues.FREE: + cell_cost = 0.0 + else: + cell_cost = neighbor_val + + tentative_cost = cost_score[current] + cell_cost + tentative_dist = dist_score[current] + _movement_costs[i] + + # Get the current scores for the neighbor or set to infinity if not yet explored + neighbor_cost = cost_score.get(neighbor, float("inf")) + neighbor_dist = dist_score.get(neighbor, float("inf")) + + # If this path to the neighbor is better (prioritize cost, then distance) + if (tentative_cost, tentative_dist) < (neighbor_cost, neighbor_dist): + # Update the neighbor's scores and parent + parents[neighbor] = current + cost_score[neighbor] = tentative_cost + dist_score[neighbor] = tentative_dist + + # Calculate priority: cost first, then distance (both with heuristic) + h_dist = _heuristic(neighbor_x, neighbor_y, goal_tuple[0], goal_tuple[1]) + priority_cost = tentative_cost + priority_dist = tentative_dist + h_dist + + # Add the neighbor to the open set with its priority + heapq.heappush(open_set, (priority_cost, priority_dist, neighbor)) + + return None diff --git a/dimos/navigation/global_planner/min_cost_astar_cpp.cpp b/dimos/navigation/global_planner/min_cost_astar_cpp.cpp new file mode 100644 index 0000000000..f19b3bf826 --- /dev/null +++ b/dimos/navigation/global_planner/min_cost_astar_cpp.cpp @@ -0,0 +1,265 @@ +// Copyright 2025 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. + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace py = pybind11; + +// Movement directions (8-connected grid) +// Order: right, down, left, up, down-right, down-left, up-right, up-left +constexpr int DX[8] = {0, 1, 0, -1, 1, 1, -1, -1}; +constexpr int DY[8] = {1, 0, -1, 0, 1, -1, 1, -1}; + +// Movement costs: straight = 1.0, diagonal = sqrt(2) ≈ 1.42 +constexpr double STRAIGHT_COST = 1.0; +constexpr double DIAGONAL_COST = 1.42; +constexpr double MOVE_COSTS[8] = { + STRAIGHT_COST, STRAIGHT_COST, STRAIGHT_COST, STRAIGHT_COST, + DIAGONAL_COST, DIAGONAL_COST, DIAGONAL_COST, DIAGONAL_COST +}; + +constexpr int8_t COST_UNKNOWN = -1; +constexpr int8_t COST_FREE = 0; + +// Pack coordinates into a single 64-bit key for fast hashing +inline uint64_t pack_coords(int x, int y) { + return (static_cast(static_cast(x)) << 32) | + static_cast(static_cast(y)); +} + +// Unpack coordinates from 64-bit key +inline std::pair unpack_coords(uint64_t key) { + return {static_cast(key >> 32), static_cast(key & 0xFFFFFFFF)}; +} + +// Octile distance heuristic - optimal for 8-connected grids with diagonal movement +inline double heuristic(int x1, int y1, int x2, int y2) { + int dx = std::abs(x2 - x1); + int dy = std::abs(y2 - y1); + // Octile distance: straight moves + diagonal adjustment + return (dx + dy) + (DIAGONAL_COST - 2 * STRAIGHT_COST) * std::min(dx, dy); +} + +// Reconstruct path from goal to start using parent map +inline std::vector> reconstruct_path( + const std::unordered_map& parents, + uint64_t goal_key, + int start_x, + int start_y +) { + std::vector> path; + uint64_t node = goal_key; + + while (parents.count(node)) { + auto [x, y] = unpack_coords(node); + path.emplace_back(x, y); + node = parents.at(node); + } + + path.emplace_back(start_x, start_y); + std::reverse(path.begin(), path.end()); + return path; +} + +// Priority queue node: (priority_cost, priority_dist, x, y) +struct Node { + double cost; + double dist; + int x; + int y; + + // Min-heap comparison: lower values have higher priority + bool operator>(const Node& other) const { + if (cost != other.cost) return cost > other.cost; + return dist > other.dist; + } +}; + +/** + * A* pathfinding algorithm optimized for costmap grids. + * + * @param grid 2D numpy array of int8 values (height x width) + * @param start_x Starting X coordinate in grid cells + * @param start_y Starting Y coordinate in grid cells + * @param goal_x Goal X coordinate in grid cells + * @param goal_y Goal Y coordinate in grid cells + * @param cost_threshold Cells with value >= this are obstacles (default: 100) + * @param unknown_penalty Cost multiplier for unknown cells (default: 0.8) + * @return Vector of (x, y) grid coordinates from start to goal, empty if no path + */ +std::vector> min_cost_astar_cpp( + py::array_t grid, + int start_x, + int start_y, + int goal_x, + int goal_y, + int cost_threshold = 100, + double unknown_penalty = 0.8 +) { + // Get buffer info for direct array access + auto buf = grid.unchecked<2>(); + const int height = static_cast(buf.shape(0)); + const int width = static_cast(buf.shape(1)); + + // Bounds check for goal + if (goal_x < 0 || goal_x >= width || goal_y < 0 || goal_y >= height) { + return {}; + } + + // Bounds check for start + if (start_x < 0 || start_x >= width || start_y < 0 || start_y >= height) { + return {}; + } + + const uint64_t start_key = pack_coords(start_x, start_y); + const uint64_t goal_key = pack_coords(goal_x, goal_y); + + std::priority_queue, std::greater> open_set; + + std::unordered_set closed_set; + closed_set.reserve(width * height / 4); // Pre-allocate + + // Parent tracking for path reconstruction + std::unordered_map parents; + parents.reserve(width * height / 4); + + // Score tracking (cost and distance) + std::unordered_map cost_score; + std::unordered_map dist_score; + cost_score.reserve(width * height / 4); + dist_score.reserve(width * height / 4); + + // Initialize start node + cost_score[start_key] = 0.0; + dist_score[start_key] = 0.0; + double h = heuristic(start_x, start_y, goal_x, goal_y); + open_set.push({0.0, h, start_x, start_y}); + + while (!open_set.empty()) { + Node current = open_set.top(); + open_set.pop(); + + const int cx = current.x; + const int cy = current.y; + const uint64_t current_key = pack_coords(cx, cy); + + if (closed_set.count(current_key)) { + continue; + } + + if (current_key == goal_key) { + return reconstruct_path(parents, current_key, start_x, start_y); + } + + closed_set.insert(current_key); + + const double current_cost = cost_score[current_key]; + const double current_dist = dist_score[current_key]; + + // Explore all 8 neighbors + for (int i = 0; i < 8; ++i) { + const int nx = cx + DX[i]; + const int ny = cy + DY[i]; + + if (nx < 0 || nx >= width || ny < 0 || ny >= height) { + continue; + } + + const uint64_t neighbor_key = pack_coords(nx, ny); + + if (closed_set.count(neighbor_key)) { + continue; + } + + // Get cell value (note: grid is [y, x] in row-major order) + const int8_t val = buf(ny, nx); + + if (val >= cost_threshold) { + continue; + } + + double cell_cost; + if (val == COST_UNKNOWN) { + // Unknown cells have a moderate traversal cost + cell_cost = cost_threshold * unknown_penalty; + } else if (val == COST_FREE) { + cell_cost = 0.0; + } else { + cell_cost = static_cast(val); + } + + const double tentative_cost = current_cost + cell_cost; + const double tentative_dist = current_dist + MOVE_COSTS[i]; + + // Get existing scores (infinity if not yet visited) + auto cost_it = cost_score.find(neighbor_key); + auto dist_it = dist_score.find(neighbor_key); + const double n_cost = (cost_it != cost_score.end()) ? cost_it->second : INFINITY; + const double n_dist = (dist_it != dist_score.end()) ? dist_it->second : INFINITY; + + // Check if this path is better (prioritize cost, then distance) + if (tentative_cost < n_cost || + (tentative_cost == n_cost && tentative_dist < n_dist)) { + + // Update parent and scores + parents[neighbor_key] = current_key; + cost_score[neighbor_key] = tentative_cost; + dist_score[neighbor_key] = tentative_dist; + + // Calculate priority with heuristic + const double h_dist = heuristic(nx, ny, goal_x, goal_y); + const double priority_cost = tentative_cost; + const double priority_dist = tentative_dist + h_dist; + + open_set.push({priority_cost, priority_dist, nx, ny}); + } + } + } + + return {}; +} + +PYBIND11_MODULE(min_cost_astar_ext, m) { + m.doc() = "C++ implementation of A* pathfinding for costmap grids"; + + m.def("min_cost_astar_cpp", &min_cost_astar_cpp, + "A* pathfinding on a costmap grid.\n\n" + "Args:\n" + " grid: 2D numpy array of int8 values (height x width)\n" + " start_x: Starting X coordinate in grid cells\n" + " start_y: Starting Y coordinate in grid cells\n" + " goal_x: Goal X coordinate in grid cells\n" + " goal_y: Goal Y coordinate in grid cells\n" + " cost_threshold: Cells >= this value are obstacles (default: 100)\n" + " unknown_penalty: Cost multiplier for unknown cells (default: 0.8)\n\n" + "Returns:\n" + " List of (x, y) grid coordinates from start to goal, or empty list if no path", + py::arg("grid"), + py::arg("start_x"), + py::arg("start_y"), + py::arg("goal_x"), + py::arg("goal_y"), + py::arg("cost_threshold") = 100, + py::arg("unknown_penalty") = 0.8); +} diff --git a/dimos/navigation/global_planner/min_cost_astar_ext.pyi b/dimos/navigation/global_planner/min_cost_astar_ext.pyi new file mode 100644 index 0000000000..2ebaadc3cd --- /dev/null +++ b/dimos/navigation/global_planner/min_cost_astar_ext.pyi @@ -0,0 +1,26 @@ +# Copyright 2025 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 numpy as np +from numpy.typing import NDArray + +def min_cost_astar_cpp( + grid: NDArray[np.int8], + start_x: int, + start_y: int, + goal_x: int, + goal_y: int, + cost_threshold: int, + unknown_penalty: float, +) -> list[tuple[int, int]]: ... diff --git a/dimos/navigation/global_planner/planner.py b/dimos/navigation/global_planner/planner.py index 553a5f6c63..40020d41a3 100644 --- a/dimos/navigation/global_planner/planner.py +++ b/dimos/navigation/global_planner/planner.py @@ -16,129 +16,16 @@ from reactivex.disposable import Disposable from dimos.core import In, Module, Out, rpc +from dimos.core.global_config import GlobalConfig +from dimos.mapping.occupancy.path_map import make_navigation_map +from dimos.mapping.occupancy.path_resampling import simple_resample_path from dimos.msgs.geometry_msgs import Pose, PoseStamped from dimos.msgs.nav_msgs import OccupancyGrid, Path -from dimos.navigation.global_planner.algo import astar +from dimos.navigation.global_planner.astar import astar from dimos.utils.logging_config import setup_logger -from dimos.utils.transform_utils import euler_to_quaternion logger = setup_logger() -import math - -from dimos.msgs.geometry_msgs import Quaternion, Vector3 - - -def add_orientations_to_path(path: Path, goal_orientation: Quaternion = None) -> Path: # type: ignore[assignment] - """Add orientations to path poses based on direction of movement. - - Args: - path: Path with poses to add orientations to - goal_orientation: Desired orientation for the final pose - - Returns: - Path with orientations added to all poses - """ - if not path.poses or len(path.poses) < 2: - return path - - # Calculate orientations for all poses except the last one - for i in range(len(path.poses) - 1): - current_pose = path.poses[i] - next_pose = path.poses[i + 1] - - # Calculate direction to next point - dx = next_pose.position.x - current_pose.position.x - dy = next_pose.position.y - current_pose.position.y - - # Calculate yaw angle - yaw = math.atan2(dy, dx) - - # Convert to quaternion (roll=0, pitch=0, yaw) - orientation = euler_to_quaternion(Vector3(0, 0, yaw)) - current_pose.orientation = orientation - - # Set last pose orientation - identity_quat = Quaternion(0, 0, 0, 1) - if goal_orientation is not None and goal_orientation != identity_quat: - # Use the provided goal orientation if it's not the identity - path.poses[-1].orientation = goal_orientation - elif len(path.poses) > 1: - # Use the previous pose's orientation - path.poses[-1].orientation = path.poses[-2].orientation - else: - # Single pose with identity goal orientation - path.poses[-1].orientation = identity_quat - - return path - - -def resample_path(path: Path, spacing: float) -> Path: - """Resample a path to have approximately uniform spacing between poses. - - Args: - path: The original Path - spacing: Desired distance between consecutive poses - - Returns: - A new Path with resampled poses - """ - if len(path) < 2 or spacing <= 0: - return path - - resampled = [] - resampled.append(path.poses[0]) - - accumulated_distance = 0.0 - - for i in range(1, len(path.poses)): - current = path.poses[i] - prev = path.poses[i - 1] - - # Calculate segment distance - dx = current.x - prev.x - dy = current.y - prev.y - segment_length = (dx**2 + dy**2) ** 0.5 - - if segment_length < 1e-10: - continue - - # Direction vector - dir_x = dx / segment_length - dir_y = dy / segment_length - - # Add points along this segment - while accumulated_distance + segment_length >= spacing: - # Distance along segment for next point - dist_along = spacing - accumulated_distance - if dist_along < 0: - break - - # Create new pose - new_x = prev.x + dir_x * dist_along - new_y = prev.y + dir_y * dist_along - new_pose = PoseStamped( - frame_id=path.frame_id, - position=[new_x, new_y, 0.0], - orientation=prev.orientation, # Keep same orientation - ) - resampled.append(new_pose) - - # Update for next iteration - accumulated_distance = 0 - segment_length -= dist_along - prev = new_pose - - accumulated_distance += segment_length - - # Add last pose if not already there - if len(path.poses) > 1: - last = path.poses[-1] - if not resampled or (resampled[-1].x != last.x or resampled[-1].y != last.y): - resampled.append(last) - - return Path(frame_id=path.frame_id, poses=resampled) - class AstarPlanner(Module): # LCM inputs @@ -149,74 +36,70 @@ class AstarPlanner(Module): # LCM outputs path: Out[Path] - def __init__(self) -> None: + _global_config: GlobalConfig + + def __init__( + self, + global_config: GlobalConfig | None = None, + ) -> None: super().__init__() - # Latest data self.latest_costmap: OccupancyGrid | None = None self.latest_odom: PoseStamped | None = None + self._global_config = global_config or GlobalConfig() + @rpc def start(self) -> None: super().start() - unsub = self.target.subscribe(self._on_target) - self._disposables.add(Disposable(unsub)) - - unsub = self.global_costmap.subscribe(self._on_costmap) - self._disposables.add(Disposable(unsub)) - - unsub = self.odom.subscribe(self._on_odom) - self._disposables.add(Disposable(unsub)) - - logger.info("A* planner started") + self._disposables.add(Disposable(self.target.subscribe(self._on_target))) + self._disposables.add(Disposable(self.global_costmap.subscribe(self._on_costmap))) + self._disposables.add(Disposable(self.odom.subscribe(self._on_odom))) @rpc def stop(self) -> None: super().stop() def _on_costmap(self, msg: OccupancyGrid) -> None: - """Handle incoming costmap messages.""" self.latest_costmap = msg def _on_odom(self, msg: PoseStamped) -> None: - """Handle incoming odometry messages.""" self.latest_odom = msg - def _on_target(self, msg: PoseStamped) -> None: - """Handle incoming target messages and trigger planning.""" + def _on_target(self, goal_pose: PoseStamped) -> None: if self.latest_costmap is None or self.latest_odom is None: logger.warning("Cannot plan: missing costmap or odometry data") return - path = self.plan(msg) + path = self.plan(goal_pose) if path: - # Add orientations to the path, using the goal's orientation for the final pose - path = add_orientations_to_path(path, msg.orientation) self.path.publish(path) - def plan(self, goal: Pose) -> Path | None: + def plan(self, goal_pose: Pose) -> Path | None: """Plan a path from current position to goal.""" + if self.latest_costmap is None or self.latest_odom is None: logger.warning("Cannot plan: missing costmap or odometry data") return None - logger.debug(f"Planning path to goal {goal}") + logger.debug(f"Planning path to goal {goal_pose}") - # Get current position from odometry robot_pos = self.latest_odom.position - costmap = self.latest_costmap.inflate(0.2).gradient(max_distance=1.5) - # Run A* planning - path = astar(costmap, goal.position, robot_pos) + costmap = make_navigation_map( + self.latest_costmap, + self._global_config.robot_width, + strategy=self._global_config.planner_strategy, + ) - if path: - path = resample_path(path, 0.1) - logger.debug(f"Path found with {len(path.poses)} waypoints") - return path + path = astar(self._global_config.astar_algorithm, costmap, goal_pose.position, robot_pos) + + if not path: + logger.warning("No path found to the goal.") + return None - logger.warning("No path found to the goal.") - return None + return simple_resample_path(path, goal_pose, 0.1) astar_planner = AstarPlanner.blueprint diff --git a/dimos/navigation/global_planner/test_astar.py b/dimos/navigation/global_planner/test_astar.py new file mode 100644 index 0000000000..6fec9220b9 --- /dev/null +++ b/dimos/navigation/global_planner/test_astar.py @@ -0,0 +1,102 @@ +# Copyright 2025 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 time + +import numpy as np +from open3d.geometry import PointCloud # type: ignore[import-untyped] +import pytest + +from dimos.mapping.occupancy.gradient import gradient, voronoi_gradient +from dimos.mapping.occupancy.visualizations import visualize_occupancy_grid +from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid +from dimos.msgs.sensor_msgs.Image import Image +from dimos.navigation.global_planner.astar import astar +from dimos.utils.data import get_data + + +@pytest.fixture +def costmap() -> PointCloud: + return gradient(OccupancyGrid(np.load(get_data("occupancy_simple.npy"))), max_distance=1.5) + + +@pytest.fixture +def costmap_three_paths() -> PointCloud: + return voronoi_gradient(OccupancyGrid(np.load(get_data("three_paths.npy"))), max_distance=1.5) + + +@pytest.mark.parametrize( + "mode,expected_image", + [ + ("general", "astar_general.png"), + ("min_cost", "astar_min_cost.png"), + ], +) +def test_astar(costmap, mode, expected_image) -> None: + start = Vector3(4.0, 2.0) + goal = Vector3(6.15, 10.0) + expected = Image.from_file(get_data(expected_image)) + + path = astar(mode, costmap, goal, start, use_cpp=False) + actual = visualize_occupancy_grid(costmap, "rainbow", path) + + np.testing.assert_array_equal(actual.data, expected.data) + + +@pytest.mark.parametrize( + "mode,expected_image", + [ + ("general", "astar_corner_general.png"), + ("min_cost", "astar_corner_min_cost.png"), + ], +) +def test_astar_corner(costmap_three_paths, mode, expected_image) -> None: + start = Vector3(2.8, 3.35) + goal = Vector3(6.35, 4.25) + expected = Image.from_file(get_data(expected_image)) + + path = astar(mode, costmap_three_paths, goal, start, use_cpp=False) + actual = visualize_occupancy_grid(costmap_three_paths, "rainbow", path) + + np.testing.assert_array_equal(actual.data, expected.data) + + +def test_astar_python_and_cpp(costmap) -> None: + start = Vector3(4.0, 2.0, 0) + goal = Vector3(6.15, 10.0) + + start_time = time.perf_counter() + path_python = astar("min_cost", costmap, goal, start, use_cpp=False) + elapsed_time_python = time.perf_counter() - start_time + print(f"\nastar Python took {elapsed_time_python:.6f} seconds") + assert path_python is not None + assert len(path_python.poses) > 0 + + start_time = time.perf_counter() + path_cpp = astar("min_cost", costmap, goal, start, use_cpp=True) + elapsed_time_cpp = time.perf_counter() - start_time + print(f"astar C++ took {elapsed_time_cpp:.6f} seconds") + assert path_cpp is not None + assert len(path_cpp.poses) > 0 + + times_better = elapsed_time_python / elapsed_time_cpp + print(f"astar C++ is {times_better:.2f} times faster than Python") + + # Assert that both implementations return almost identical points. + np.testing.assert_allclose( + [(p.position.x, p.position.y) for p in path_python.poses], + [(p.position.x, p.position.y) for p in path_cpp.poses], + atol=0.05001, + ) diff --git a/dimos/navigation/global_planner/types.py b/dimos/navigation/global_planner/types.py new file mode 100644 index 0000000000..40dceaac6a --- /dev/null +++ b/dimos/navigation/global_planner/types.py @@ -0,0 +1,17 @@ +# Copyright 2025 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 typing import Literal, TypeAlias + +AStarAlgorithm: TypeAlias = Literal["general", "min_cost"] diff --git a/dimos/navigation/local_planner/__init__.py b/dimos/navigation/local_planner/__init__.py index 9e0f62931a..b08e97b1aa 100644 --- a/dimos/navigation/local_planner/__init__.py +++ b/dimos/navigation/local_planner/__init__.py @@ -1,2 +1,4 @@ from dimos.navigation.local_planner.holonomic_local_planner import HolonomicLocalPlanner from dimos.navigation.local_planner.local_planner import BaseLocalPlanner + +__all__ = ["BaseLocalPlanner", "HolonomicLocalPlanner"] diff --git a/dimos/navigation/local_planner/local_planner.py b/dimos/navigation/local_planner/local_planner.py index 8e3e161e1e..06b20545d8 100644 --- a/dimos/navigation/local_planner/local_planner.py +++ b/dimos/navigation/local_planner/local_planner.py @@ -26,8 +26,11 @@ from reactivex.disposable import Disposable from dimos.core import In, Module, Out, rpc +from dimos.mapping.occupancy.gradient import gradient +from dimos.mapping.pointclouds.occupancy import general_occupancy from dimos.msgs.geometry_msgs import PoseStamped, Twist from dimos.msgs.nav_msgs import OccupancyGrid, Path +from dimos.robot.unitree_webrtc.type.lidar import LidarMessage from dimos.utils.logging_config import setup_logger from dimos.utils.transform_utils import get_distance, normalize_angle, quaternion_to_euler @@ -39,7 +42,7 @@ class BaseLocalPlanner(Module): local planner module for obstacle avoidance and path following. Subscribes to: - - /local_costmap: Local occupancy grid for obstacle detection + - /lidar: Local lidar for obstacle detection - /odom: Robot odometry for current pose - /path: Path to follow (continuously updated at ~1Hz) @@ -48,7 +51,7 @@ class BaseLocalPlanner(Module): """ # LCM inputs - local_costmap: In[OccupancyGrid] + lidar: In[LidarMessage] odom: In[PoseStamped] path: In[Path] @@ -92,22 +95,25 @@ def __init__( # type: ignore[no-untyped-def] def start(self) -> None: super().start() - unsub = self.local_costmap.subscribe(self._on_costmap) - self._disposables.add(Disposable(unsub)) - - unsub = self.odom.subscribe(self._on_odom) - self._disposables.add(Disposable(unsub)) - - unsub = self.path.subscribe(self._on_path) - self._disposables.add(Disposable(unsub)) + self._disposables.add(Disposable(self.lidar.subscribe(self._on_lidar))) + self._disposables.add(Disposable(self.odom.subscribe(self._on_odom))) + self._disposables.add(Disposable(self.path.subscribe(self._on_path))) @rpc def stop(self) -> None: self.cancel_planning() super().stop() - def _on_costmap(self, msg: OccupancyGrid) -> None: - self.latest_costmap = msg + def _on_lidar(self, msg: LidarMessage) -> None: + self.latest_costmap = gradient( + general_occupancy( + msg, + resolution=0.05, + min_height=0.15, + max_height=0.6, + ), + max_distance=0.25, + ) def _on_odom(self, msg: PoseStamped) -> None: self.latest_odom = msg @@ -120,14 +126,11 @@ def _on_path(self, msg: Path) -> None: self._start_planning_thread() def _start_planning_thread(self) -> None: - """Start the planning thread.""" self.stop_planning.clear() self.planning_thread = threading.Thread(target=self._follow_path_loop, daemon=True) self.planning_thread.start() - logger.debug("Started follow path thread") def _follow_path_loop(self) -> None: - """Main planning loop that runs in a separate thread.""" while not self.stop_planning.is_set(): if self.is_goal_reached(): self.stop_planning.set() diff --git a/dimos/navigation/replanning_a_star/global_planner.py b/dimos/navigation/replanning_a_star/global_planner.py new file mode 100644 index 0000000000..efe4f814b3 --- /dev/null +++ b/dimos/navigation/replanning_a_star/global_planner.py @@ -0,0 +1,323 @@ +# Copyright 2025 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 threading import Event, RLock, Thread, current_thread +import time + +from dimos_lcm.std_msgs import Bool # type: ignore[import-untyped] +from reactivex import Subject +from reactivex.disposable import CompositeDisposable + +from dimos.core.global_config import GlobalConfig +from dimos.core.resource import Resource +from dimos.mapping.occupancy.path_resampling import smooth_resample_path +from dimos.msgs.geometry_msgs import Twist +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.msgs.nav_msgs.OccupancyGrid import CostValues, OccupancyGrid +from dimos.msgs.nav_msgs.Path import Path +from dimos.msgs.sensor_msgs import Image +from dimos.navigation.base import NavigationState +from dimos.navigation.bt_navigator.goal_validator import find_safe_goal +from dimos.navigation.global_planner.astar import astar +from dimos.navigation.replanning_a_star.local_planner import LocalPlanner, StopMessage +from dimos.navigation.replanning_a_star.navigation_map import NavigationMap +from dimos.navigation.replanning_a_star.position_tracker import PositionTracker +from dimos.utils.logging_config import setup_logger + +logger = setup_logger() + + +class GlobalPlanner(Resource): + path: Subject[Path] + goal_reached: Subject[Bool] + + _current_odom: PoseStamped | None = None + _current_goal: PoseStamped | None = None + _goal_reached: bool = False + _thread: Thread | None = None + + _global_config: GlobalConfig + _navigation_map: NavigationMap + _local_planner: LocalPlanner + _position_tracker: PositionTracker + _disposables: CompositeDisposable + _stop_planner: Event + _replan_event: Event + _replan_reason: StopMessage | None + _lock: RLock + _replan_attempt: int + + _safe_goal_tolerance: float = 4.0 + _replan_goal_tolerance: float = 0.5 + _max_replan_attempts: int = 10 + _stuck_time_window: float = 8.0 + _max_path_deviation: float = 0.9 + + def __init__(self, global_config: GlobalConfig) -> None: + self.path = Subject() + self.goal_reached = Subject() + + self._global_config = global_config + self._navigation_map = NavigationMap(self._global_config) + self._local_planner = LocalPlanner(self._global_config, self._navigation_map) + self._position_tracker = PositionTracker(self._stuck_time_window) + self._disposables = CompositeDisposable() + self._stop_planner = Event() + self._replan_event = Event() + self._replan_reason = None + self._lock = RLock() + self._replan_attempt = 0 + + def start(self) -> None: + self._local_planner.start() + self._disposables.add( + self._local_planner.stopped_navigating.subscribe(self._on_stopped_navigating) + ) + self._stop_planner.clear() + self._thread = Thread(target=self._thread_entrypoint, daemon=True) + self._thread.start() + + def stop(self) -> None: + self.cancel_goal() + self._local_planner.stop() + self._disposables.dispose() + self._stop_planner.set() + self._replan_event.set() + + if self._thread is not None and self._thread is not current_thread(): + self._thread.join(2) + if self._thread.is_alive(): + logger.error("GlobalPlanner thread did not stop in time.") + self._thread = None + + def handle_odom(self, msg: PoseStamped) -> None: + with self._lock: + self._current_odom = msg + + self._local_planner.handle_odom(msg) + self._position_tracker.add_position(msg) + + def handle_global_costmap(self, msg: OccupancyGrid) -> None: + self._navigation_map.update(msg) + + def handle_goal_request(self, goal: PoseStamped) -> None: + with self._lock: + self._current_goal = goal + self._goal_reached = False + self._replan_attempt = 0 + self._plan_path() + + def cancel_goal(self, *, but_will_try_again: bool = False, arrived: bool = False) -> None: + logger.info("Cancelling goal.", but_will_try_again=but_will_try_again, arrived=arrived) + + with self._lock: + self._position_tracker.reset_data() + + if not but_will_try_again: + self._current_goal = None + self._goal_reached = arrived + self._replan_attempt = 0 + + self.path.on_next(Path()) + self._local_planner.stop_planning() + + if not but_will_try_again: + self.goal_reached.on_next(Bool(arrived)) + + def get_state(self) -> NavigationState: + return self._local_planner.get_state() + + def is_goal_reached(self) -> bool: + with self._lock: + return self._goal_reached + + @property + def cmd_vel(self) -> Subject[Twist]: + return self._local_planner.cmd_vel + + @property + def debug_navigation(self) -> Subject[Image]: + return self._local_planner.debug_navigation + + def _thread_entrypoint(self) -> None: + """Monitor if the robot is stuck, veers off track, or stopped navigating.""" + + last_id = -1 + last_stuck_check = time.perf_counter() + + while not self._stop_planner.is_set(): + # Wait for either timeout or replan signal from local planner. + replanning_wanted = self._replan_event.wait(timeout=0.1) + + if self._stop_planner.is_set(): + break + + # Handle stop message from local planner (priority) + if replanning_wanted: + self._replan_event.clear() + with self._lock: + reason = self._replan_reason + self._replan_reason = None + + if reason is not None: + self._handle_stop_message(reason) + last_stuck_check = time.perf_counter() + continue + + with self._lock: + current_goal = self._current_goal + current_odom = self._current_odom + + if not current_goal or not current_odom: + continue + + if current_goal.position.distance(current_odom.position) < self._replan_goal_tolerance: + logger.info("Close enough to goal. Accepting as arrived.") + self.cancel_goal(arrived=True) + continue + + # Check if robot has veered too far off the path + deviation = self._local_planner.get_distance_to_path() + if deviation is not None and deviation > self._max_path_deviation: + logger.info( + "Robot veered off track. Replanning.", + deviation=round(deviation, 2), + threshold=self._max_path_deviation, + ) + self._replan_path() + last_stuck_check = time.perf_counter() + continue + + _, new_id = self._local_planner.get_unique_state() + + if new_id != last_id: + last_id = new_id + last_stuck_check = time.perf_counter() + continue + + if ( + time.perf_counter() - last_stuck_check > self._stuck_time_window + and self._position_tracker.is_stuck() + ): + logger.info("Robot is stuck. Replanning.") + self._replan_path() + last_stuck_check = time.perf_counter() + + def _on_stopped_navigating(self, stop_message: StopMessage) -> None: + with self._lock: + self._replan_reason = stop_message + # Signal the monitoring thread to do the replanning. This is so we don't have two + # threads which could be replanning at the same time. + self._replan_event.set() + + def _handle_stop_message(self, stop_message: StopMessage) -> None: + # Note, this runs in the monitoring thread. + + self.path.on_next(Path()) + + if stop_message == "arrived": + logger.info("Arrived at goal.") + self.cancel_goal(arrived=True) + elif stop_message == "obstacle_found": + logger.info("Replanning path due to obstacle found.") + self._replan_path() + elif stop_message == "error": + logger.info("Failure in navigation.") + self._replan_path() + else: + logger.error(f"No code to handle '{stop_message}'.") + self.cancel_goal() + + def _replan_path(self) -> None: + with self._lock: + current_odom = self._current_odom + current_goal = self._current_goal + replan_attempt = self._replan_attempt + + logger.info("Replanning.", attempt=replan_attempt) + + assert current_odom is not None + assert current_goal is not None + + if current_goal.position.distance(current_odom.position) < self._replan_goal_tolerance: + self.cancel_goal(arrived=True) + return + + if replan_attempt + 1 > self._max_replan_attempts: + self.cancel_goal() + return + + with self._lock: + self._replan_attempt += 1 + + self._plan_path() + + def _plan_path(self) -> None: + self.cancel_goal(but_will_try_again=True) + + with self._lock: + current_odom = self._current_odom + current_goal = self._current_goal + + assert current_goal is not None + + if current_odom is None: + logger.warning("Cannot handle goal request: missing odometry.") + return + + safe_goal = find_safe_goal( + self._navigation_map.binary_costmap, + current_goal.position, + algorithm="bfs_contiguous", + cost_threshold=CostValues.OCCUPIED, + min_clearance=self._global_config.robot_rotation_diameter / 2, + max_search_distance=self._safe_goal_tolerance, + ) + + if safe_goal is None: + logger.warning("No safe goal found near requested target.") + return + + goals_distance = safe_goal.distance(current_goal.position) + if goals_distance > 0.2: + logger.warning(f"Travelling to goal {goals_distance}m away from requested goal.") + + logger.info("Found safe goal.", x=round(safe_goal.x, 2), y=round(safe_goal.y, 2)) + + path = self._find_wide_path(safe_goal, current_odom.position) + + if not path: + logger.warning( + "No path found to the goal.", x=round(safe_goal.x, 3), y=round(safe_goal.y, 3) + ) + return + + resampled_path = smooth_resample_path(path, current_goal, 0.1) + + self.path.on_next(resampled_path) + + self._local_planner.start_planning(resampled_path) + + def _find_wide_path(self, goal: Vector3, robot_pos: Vector3) -> Path | None: + sizes_to_try: list[float] = [2.2, 1.7, 1.3, 1] + + for size in sizes_to_try: + costmap = self._navigation_map.make_gradient_costmap(size) + path = astar(self._global_config.astar_algorithm, costmap, goal, robot_pos) + if path and path.poses: + logger.info(f"Found path {size}x robot width.") + return path + + return None diff --git a/dimos/navigation/replanning_a_star/local_planner.py b/dimos/navigation/replanning_a_star/local_planner.py new file mode 100644 index 0000000000..f31c5e2e4a --- /dev/null +++ b/dimos/navigation/replanning_a_star/local_planner.py @@ -0,0 +1,421 @@ +# Copyright 2025 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 +import os +from threading import Event, RLock, Thread +import time +import traceback +from typing import Literal, TypeAlias + +import numpy as np +from reactivex import Subject + +from dimos.core.global_config import GlobalConfig +from dimos.core.resource import Resource +from dimos.mapping.occupancy.visualize_path import visualize_path +from dimos.msgs.geometry_msgs import Twist, Vector3 +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.nav_msgs import Path +from dimos.msgs.sensor_msgs import Image +from dimos.navigation.base import NavigationState +from dimos.navigation.replanning_a_star.navigation_map import NavigationMap +from dimos.navigation.replanning_a_star.path_clearance import PathClearance +from dimos.navigation.replanning_a_star.path_distancer import PathDistancer +from dimos.utils.logging_config import setup_logger +from dimos.utils.transform_utils import normalize_angle, quaternion_to_euler + +PlannerState: TypeAlias = Literal[ + "idle", "initial_rotation", "path_following", "final_rotation", "arrived" +] +StopMessage: TypeAlias = Literal["arrived", "obstacle_found", "error"] + +logger = setup_logger() + + +class LocalPlanner(Resource): + cmd_vel: Subject[Twist] + stopped_navigating: Subject[StopMessage] + debug_navigation: Subject[Image] + + _thread: Thread | None = None + _path: Path | None = None + _path_clearance: PathClearance | None = None + _path_distancer: PathDistancer | None = None + _current_odom: PoseStamped | None = None + + _pose_index: int + _lock: RLock + _stop_planning_event: Event + _state: PlannerState + _state_unique_id: int + _global_config: GlobalConfig + _navigation_map: NavigationMap + + _speed: float = 0.55 + _min_linear_velocity: float = 0.2 + _min_angular_velocity: float = 0.2 + _k_angular: float = 0.5 + _k_derivative: float = 0.15 + _max_angular_accel: float = 2.0 + _goal_tolerance: float = 0.3 + _orientation_tolerance: float = 0.35 + _control_frequency: float = 10.0 + _rotation_threshold: float = 90 + + _prev_yaw_error: float + _prev_angular_velocity: float + + def __init__(self, global_config: GlobalConfig, navigation_map: NavigationMap) -> None: + self.cmd_vel = Subject() + self.stopped_navigating = Subject() + self.debug_navigation = Subject() + + self._pose_index = 0 + self._lock = RLock() + self._stop_planning_event = Event() + self._state = "idle" + self._state_unique_id = 0 + self._global_config = global_config + self._navigation_map = navigation_map + + self._prev_yaw_error = 0.0 + self._prev_angular_velocity = 0.0 + + def start(self) -> None: + pass + + def stop(self) -> None: + self.stop_planning() + + def handle_odom(self, msg: PoseStamped) -> None: + with self._lock: + self._current_odom = msg + + def start_planning(self, path: Path) -> None: + self.stop_planning() + + self._stop_planning_event = Event() + + with self._lock: + self._path = path + self._path_clearance = PathClearance(self._global_config, self._path) + self._path_distancer = PathDistancer(self._path) + self._pose_index = 0 + self._thread = Thread(target=self._thread_entrypoint, daemon=True) + self._thread.start() + + def stop_planning(self) -> None: + self.cmd_vel.on_next(Twist()) + self._stop_planning_event.set() + + with self._lock: + self._thread = None + + self._reset_state() + + def get_state(self) -> NavigationState: + with self._lock: + state = self._state + + match state: + case "idle" | "arrived": + return NavigationState.IDLE + case "initial_rotation" | "path_following" | "final_rotation": + return NavigationState.FOLLOWING_PATH + case _: + raise ValueError(f"Unknown planner state: {state}") + + def get_unique_state(self) -> tuple[PlannerState, int]: + with self._lock: + return (self._state, self._state_unique_id) + + def _thread_entrypoint(self) -> None: + try: + self._loop() + except Exception as e: + traceback.print_exc() + logger.exception("Error in local planning", exc_info=e) + self.stopped_navigating.on_next("error") + finally: + self._reset_state() + self.cmd_vel.on_next(Twist()) + + def _change_state(self, new_state: PlannerState) -> None: + self._state = new_state + self._state_unique_id += 1 + logger.info("changed state", state=new_state) + + def _loop(self) -> None: + stop_event = self._stop_planning_event + + with self._lock: + path = self._path + path_clearance = self._path_clearance + current_odom = self._current_odom + + if path is None or path_clearance is None: + raise RuntimeError("No path set for local planner.") + + # Determine initial state: skip initial_rotation if already aligned. + new_state: PlannerState = "initial_rotation" + if current_odom is not None and len(path.poses) > 0: + first_yaw = quaternion_to_euler(path.poses[0].orientation).z + robot_yaw = current_odom.orientation.euler[2] + initial_yaw_error = normalize_angle(first_yaw - robot_yaw) + self._prev_yaw_error = initial_yaw_error + if abs(initial_yaw_error) < self._orientation_tolerance: + new_state = "path_following" + + with self._lock: + self._change_state(new_state) + + while not stop_event.is_set(): + start_time = time.perf_counter() + + with self._lock: + path_clearance.update_costmap(self._navigation_map.binary_costmap) + path_clearance.update_pose_index(self._pose_index) + + if "DEBUG_NAVIGATION" in os.environ: + self.debug_navigation.on_next( + self._make_debug_navigation_image(path, path_clearance) + ) + + if path_clearance.is_obstacle_ahead(): + logger.info("Obstacle detected ahead, stopping local planner.") + self.stopped_navigating.on_next("obstacle_found") + break + + with self._lock: + state: PlannerState = self._state + + if state == "initial_rotation": + cmd_vel = self._compute_initial_rotation() + elif state == "path_following": + cmd_vel = self._compute_path_following() + elif state == "final_rotation": + cmd_vel = self._compute_final_rotation() + elif state == "arrived": + self.stopped_navigating.on_next("arrived") + break + elif state == "idle": + cmd_vel = None + + if cmd_vel is not None: + self.cmd_vel.on_next(cmd_vel) + + elapsed = time.perf_counter() - start_time + sleep_time = max(0.0, (1.0 / self._control_frequency) - elapsed) + stop_event.wait(sleep_time) + + if stop_event.is_set(): + logger.info("Local planner loop exited due to stop event.") + + def _apply_min_velocity(self, velocity: float, min_velocity: float) -> float: + """Apply minimum velocity threshold, preserving sign. Returns 0 if velocity is 0.""" + if velocity == 0.0: + return 0.0 + if abs(velocity) < min_velocity: + return min_velocity if velocity > 0 else -min_velocity + return velocity + + def _compute_angular_velocity(self, yaw_error: float, max_speed: float) -> float: + dt = 1.0 / self._control_frequency + + # PD control: proportional + derivative damping + yaw_error_derivative = (yaw_error - self._prev_yaw_error) / dt + angular_velocity = self._k_angular * yaw_error - self._k_derivative * yaw_error_derivative + + # Rate limiting: limit angular acceleration to prevent jerky corrections + max_delta = self._max_angular_accel * dt + angular_velocity = np.clip( + angular_velocity, + self._prev_angular_velocity - max_delta, + self._prev_angular_velocity + max_delta, + ) + + # Clamp to max speed + angular_velocity = np.clip(angular_velocity, -max_speed, max_speed) + + # Apply minimum velocity threshold + angular_velocity = self._apply_min_velocity(angular_velocity, self._min_angular_velocity) + + # Update state for next iteration + self._prev_yaw_error = yaw_error + self._prev_angular_velocity = angular_velocity + + return float(angular_velocity) + + def _compute_initial_rotation(self) -> Twist: + with self._lock: + path = self._path + current_odom = self._current_odom + + assert path is not None + assert current_odom is not None + + first_pose = path.poses[0] + first_yaw = quaternion_to_euler(first_pose.orientation).z + robot_yaw = current_odom.orientation.euler[2] + yaw_error = normalize_angle(first_yaw - robot_yaw) + + if abs(yaw_error) < self._orientation_tolerance: + with self._lock: + self._change_state("path_following") + return self._compute_path_following() + + angular_velocity = self._compute_angular_velocity(yaw_error, self._speed) + + return Twist( + linear=Vector3(0.0, 0.0, 0.0), + angular=Vector3(0.0, 0.0, angular_velocity), + ) + + def get_distance_to_path(self) -> float | None: + with self._lock: + path_distancer = self._path_distancer + current_odom = self._current_odom + + if path_distancer is None or current_odom is None: + return None + + current_pos = np.array([current_odom.position.x, current_odom.position.y]) + + return path_distancer.get_distance_to_path(current_pos) + + def _compute_path_following(self) -> Twist: + with self._lock: + path_distancer = self._path_distancer + current_odom = self._current_odom + + assert path_distancer is not None + assert current_odom is not None + + current_pos = np.array([current_odom.position.x, current_odom.position.y]) + + if path_distancer.distance_to_goal(current_pos) < self._goal_tolerance: + logger.info("Reached goal position, starting final rotation") + with self._lock: + self._change_state("final_rotation") + return self._compute_final_rotation() + + closest_index = path_distancer.find_closest_point_index(current_pos) + + with self._lock: + self._pose_index = closest_index + + lookahead_point = path_distancer.find_lookahead_point(closest_index) + + direction = lookahead_point - current_pos + distance = np.linalg.norm(direction) + + if distance < 1e-6: + # Robot is coincidentally at the lookahead point; skip this cycle. + return Twist() + + robot_yaw = current_odom.orientation.euler[2] + desired_yaw = np.arctan2(direction[1], direction[0]) + yaw_error = normalize_angle(desired_yaw - robot_yaw) + + # Rotate-then-drive: if heading error is large, rotate in place first + rotation_threshold = self._rotation_threshold * math.pi / 180 + if abs(yaw_error) > rotation_threshold: + angular_velocity = self._compute_angular_velocity(yaw_error, self._speed) + return Twist( + linear=Vector3(0.0, 0.0, 0.0), + angular=Vector3(0.0, 0.0, angular_velocity), + ) + + # When aligned, drive forward with proportional angular correction + angular_velocity = self._compute_angular_velocity(yaw_error, self._speed) + linear_velocity = self._speed * (1.0 - abs(yaw_error) / rotation_threshold) + linear_velocity = self._apply_min_velocity(linear_velocity, self._min_linear_velocity) + + return Twist( + linear=Vector3(linear_velocity, 0.0, 0.0), + angular=Vector3(0.0, 0.0, angular_velocity), + ) + + def _compute_final_rotation(self) -> Twist: + with self._lock: + path = self._path + current_odom = self._current_odom + + assert path is not None + assert current_odom is not None + + goal_yaw = quaternion_to_euler(path.poses[-1].orientation).z + robot_yaw = current_odom.orientation.euler[2] + yaw_error = normalize_angle(goal_yaw - robot_yaw) + + if abs(yaw_error) < self._orientation_tolerance: + logger.info("Final rotation complete, goal reached") + with self._lock: + self._change_state("arrived") + return Twist() + + angular_velocity = self._compute_angular_velocity(yaw_error, self._speed) + + return Twist( + linear=Vector3(0.0, 0.0, 0.0), + angular=Vector3(0.0, 0.0, angular_velocity), + ) + + def _reset_state(self) -> None: + with self._lock: + self._change_state("idle") + self._path = None + self._path_clearance = None + self._path_distancer = None + self._pose_index = 0 + self._prev_yaw_error = 0.0 + self._prev_angular_velocity = 0.0 + + def _make_debug_navigation_image(self, path: Path, path_clearance: PathClearance) -> Image: + scale = 8 + image = visualize_path( + self._navigation_map.gradient_costmap, + path, + self._global_config.robot_width, + self._global_config.robot_rotation_diameter, + 2, + scale, + ) + image.data = np.flipud(image.data) + + # Add path mask. + mask = path_clearance.mask + scaled_mask = np.repeat(np.repeat(mask, scale, axis=0), scale, axis=1) + scaled_mask = np.flipud(scaled_mask) + white = np.array([255, 255, 255], dtype=np.int16) + image.data[scaled_mask] = (image.data[scaled_mask].astype(np.int16) * 3 + white * 7) // 10 + + with self._lock: + current_odom = self._current_odom + + # Draw robot position. + if current_odom is not None: + grid_pos = self._navigation_map.gradient_costmap.world_to_grid(current_odom.position) + x = int(grid_pos.x * scale) + y = image.data.shape[0] - 1 - int(grid_pos.y * scale) + radius = 8 + for dy in range(-radius, radius + 1): + for dx in range(-radius, radius + 1): + if dx * dx + dy * dy <= radius * radius: + py, px = y + dy, x + dx + if 0 <= py < image.data.shape[0] and 0 <= px < image.data.shape[1]: + image.data[py, px] = [255, 255, 255] + + return image diff --git a/dimos/navigation/replanning_a_star/module.py b/dimos/navigation/replanning_a_star/module.py new file mode 100644 index 0000000000..7ffd5d4fb1 --- /dev/null +++ b/dimos/navigation/replanning_a_star/module.py @@ -0,0 +1,106 @@ +# Copyright 2025 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 os + +from dimos_lcm.std_msgs import Bool, String # type: ignore[import-untyped] +from reactivex.disposable import Disposable + +from dimos.core import In, Module, Out, rpc +from dimos.core.global_config import GlobalConfig +from dimos.msgs.geometry_msgs import PoseStamped, Twist +from dimos.msgs.nav_msgs import OccupancyGrid, Path +from dimos.msgs.sensor_msgs import Image +from dimos.navigation.base import NavigationInterface, NavigationState +from dimos.navigation.replanning_a_star.global_planner import GlobalPlanner + + +class ReplanningAStarPlanner(Module, NavigationInterface): + odom: In[PoseStamped] # TODO: Use TF. + global_costmap: In[OccupancyGrid] + goal_request: In[PoseStamped] + target: In[PoseStamped] + + goal_reached: Out[Bool] + navigation_state: Out[String] # TODO: set it + cmd_vel: Out[Twist] + path: Out[Path] + debug_navigation: Out[Image] + + _planner: GlobalPlanner + _global_config: GlobalConfig + + def __init__(self, global_config: GlobalConfig | None = None) -> None: + super().__init__() + self._global_config = global_config or GlobalConfig() + self._planner = GlobalPlanner(self._global_config) + + @rpc + def start(self) -> None: + super().start() + + unsub = self.odom.subscribe(self._planner.handle_odom) + self._disposables.add(Disposable(unsub)) + + unsub = self.global_costmap.subscribe(self._planner.handle_global_costmap) + self._disposables.add(Disposable(unsub)) + + unsub = self.goal_request.subscribe(self._planner.handle_goal_request) + self._disposables.add(Disposable(unsub)) + + unsub = self.target.subscribe(self._planner.handle_goal_request) + self._disposables.add(Disposable(unsub)) + + self._disposables.add(self._planner.path.subscribe(self.path.publish)) + + self._disposables.add(self._planner.cmd_vel.subscribe(self.cmd_vel.publish)) + + self._disposables.add(self._planner.goal_reached.subscribe(self.goal_reached.publish)) + + if "DEBUG_NAVIGATION" in os.environ: + self._disposables.add( + self._planner.debug_navigation.subscribe(self.debug_navigation.publish) + ) + + self._planner.start() + + @rpc + def stop(self) -> None: + self.cancel_goal() + self._planner.stop() + + super().stop() + + @rpc + def set_goal(self, goal: PoseStamped) -> bool: + self._planner.handle_goal_request(goal) + return True + + @rpc + def get_state(self) -> NavigationState: + return self._planner.get_state() + + @rpc + def is_goal_reached(self) -> bool: + return self._planner.is_goal_reached() + + @rpc + def cancel_goal(self) -> bool: + self._planner.cancel_goal() + return True + + +replanning_a_star_planner = ReplanningAStarPlanner.blueprint + +__all__ = ["ReplanningAStarPlanner", "replanning_a_star_planner"] diff --git a/dimos/navigation/replanning_a_star/navigation_map.py b/dimos/navigation/replanning_a_star/navigation_map.py new file mode 100644 index 0000000000..e29122784f --- /dev/null +++ b/dimos/navigation/replanning_a_star/navigation_map.py @@ -0,0 +1,66 @@ +# Copyright 2025 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 threading import RLock + +from dimos.core.global_config import GlobalConfig +from dimos.mapping.occupancy.path_map import make_navigation_map +from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid + + +class NavigationMap: + _global_config: GlobalConfig + _binary: OccupancyGrid | None = None + _lock: RLock + + def __init__(self, global_config: GlobalConfig) -> None: + self._global_config = global_config + self._lock = RLock() + + def update(self, occupancy_grid: OccupancyGrid) -> None: + with self._lock: + self._binary = occupancy_grid + + @property + def binary_costmap(self) -> OccupancyGrid: + """ + Get the latest binary costmap received from the global costmap source. + """ + + with self._lock: + if self._binary is None: + raise ValueError("No current global costmap available") + + return self._binary + + @property + def gradient_costmap(self) -> OccupancyGrid: + return self.make_gradient_costmap() + + def make_gradient_costmap(self, robot_increase: float = 1.0) -> OccupancyGrid: + """ + Get the latest navigation map created from inflating and applying a + gradient to the binary costmap. + """ + + with self._lock: + binary = self._binary + if binary is None: + raise ValueError("No current global costmap available") + + return make_navigation_map( + binary, + self._global_config.robot_width * robot_increase, + strategy=self._global_config.planner_strategy, + ) diff --git a/dimos/navigation/replanning_a_star/path_clearance.py b/dimos/navigation/replanning_a_star/path_clearance.py new file mode 100644 index 0000000000..52897de7fc --- /dev/null +++ b/dimos/navigation/replanning_a_star/path_clearance.py @@ -0,0 +1,94 @@ +# Copyright 2025 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 threading import RLock + +import numpy as np +from numpy.typing import NDArray + +from dimos.core.global_config import GlobalConfig +from dimos.mapping.occupancy.path_mask import make_path_mask +from dimos.msgs.nav_msgs import Path +from dimos.msgs.nav_msgs.OccupancyGrid import CostValues, OccupancyGrid + + +class PathClearance: + _costmap: OccupancyGrid | None = None + _last_costmap: OccupancyGrid | None = None + _path_lookup_distance: float = 3.0 + _max_distance_cache: float = 1.0 + _last_used_shape: tuple[int, ...] | None = None + _last_mask: NDArray[np.bool_] | None = None + _last_used_pose: int | None = None + _global_config: GlobalConfig + _lock: RLock + _path: Path + _pose_index: int + + def __init__(self, global_config: GlobalConfig, path: Path) -> None: + self._global_config = global_config + self._path = path + self._pose_index = 0 + self._lock = RLock() + + def update_costmap(self, costmap: OccupancyGrid) -> None: + with self._lock: + self._costmap = costmap + + def update_pose_index(self, index: int) -> None: + with self._lock: + self._pose_index = index + + @property + def mask(self) -> NDArray[np.bool_]: + with self._lock: + costmap = self._costmap + pose_index = self._pose_index + + assert costmap is not None + + if ( + self._last_mask is not None + and self._last_used_pose is not None + and costmap.grid.shape == self._last_used_shape + and self._pose_distance(self._last_used_pose, pose_index) < self._max_distance_cache + ): + return self._last_mask + + self._last_mask = make_path_mask( + occupancy_grid=costmap, + path=self._path, + robot_width=self._global_config.robot_width, + pose_index=pose_index, + max_length=self._path_lookup_distance, + ) + + self._last_used_shape = costmap.grid.shape + self._last_used_pose = pose_index + + return self._last_mask + + def is_obstacle_ahead(self) -> bool: + with self._lock: + costmap = self._costmap + + if costmap is None: + return True + + return bool(np.any(costmap.grid[self.mask] == CostValues.OCCUPIED)) + + def _pose_distance(self, index1: int, index2: int) -> float: + p1 = self._path.poses[index1].position + p2 = self._path.poses[index2].position + return p1.distance(p2) diff --git a/dimos/navigation/replanning_a_star/path_distancer.py b/dimos/navigation/replanning_a_star/path_distancer.py new file mode 100644 index 0000000000..9f0e623c64 --- /dev/null +++ b/dimos/navigation/replanning_a_star/path_distancer.py @@ -0,0 +1,89 @@ +# Copyright 2025 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 typing import cast + +import numpy as np +from numpy.typing import NDArray + +from dimos.msgs.nav_msgs import Path + + +class PathDistancer: + _lookahead_dist: float = 0.5 + _path: NDArray[np.float64] + _cumulative_dists: NDArray[np.float64] + + def __init__(self, path: Path) -> None: + self._path = np.array([[p.position.x, p.position.y] for p in path.poses]) + self._cumulative_dists = _make_cumulative_distance_array(self._path) + + def find_lookahead_point(self, start_idx: int) -> NDArray[np.float64]: + """ + Given a path, and a precomputed array of cumulative distances, find the + point which is `lookahead_dist` ahead of the current point. + """ + + if start_idx >= len(self._path) - 1: + return cast("NDArray[np.float64]", self._path[-1]) + + # Distance from path[0] to path[start_idx]. + base_dist = self._cumulative_dists[start_idx - 1] if start_idx > 0 else 0.0 + target_dist = base_dist + self._lookahead_dist + + # Binary search: cumulative_dists[i] = distance from path[0] to path[i+1] + idx = int(np.searchsorted(self._cumulative_dists, target_dist)) + + if idx >= len(self._cumulative_dists): + return cast("NDArray[np.float64]", self._path[-1]) + + # Interpolate within segment from path[idx] to path[idx+1]. + prev_cum_dist = self._cumulative_dists[idx - 1] if idx > 0 else 0.0 + segment_dist = self._cumulative_dists[idx] - prev_cum_dist + remaining_dist = target_dist - prev_cum_dist + + if segment_dist > 0: + t = remaining_dist / segment_dist + return cast( + "NDArray[np.float64]", + self._path[idx] + t * (self._path[idx + 1] - self._path[idx]), + ) + + return cast("NDArray[np.float64]", self._path[idx]) + + def distance_to_goal(self, current_pos: NDArray[np.float64]) -> float: + return float(np.linalg.norm(self._path[-1] - current_pos)) + + def get_distance_to_path(self, pos: NDArray[np.float64]) -> float: + index = self.find_closest_point_index(pos) + return float(np.linalg.norm(self._path[index] - pos)) + + def find_closest_point_index(self, pos: NDArray[np.float64]) -> int: + """Find the index of the closest point on the path.""" + distances = np.linalg.norm(self._path - pos, axis=1) + return int(np.argmin(distances)) + + +def _make_cumulative_distance_array(array: NDArray[np.float64]) -> NDArray[np.float64]: + """ + For an array representing 2D points, create an array of all the distances + between the points. + """ + + if len(array) < 2: + return np.array([0.0]) + + segments = array[1:] - array[:-1] + segment_dists = np.linalg.norm(segments, axis=1) + return np.cumsum(segment_dists) diff --git a/dimos/navigation/replanning_a_star/position_tracker.py b/dimos/navigation/replanning_a_star/position_tracker.py new file mode 100644 index 0000000000..66b069684f --- /dev/null +++ b/dimos/navigation/replanning_a_star/position_tracker.py @@ -0,0 +1,83 @@ +# Copyright 2025 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 threading import RLock +import time +from typing import cast + +import numpy as np +from numpy.typing import NDArray + +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped + +_max_points_per_second = 1000 + + +class PositionTracker: + _lock: RLock + _time_window: float + _max_points: int + _threshold: float + _timestamps: NDArray[np.float32] + _positions: NDArray[np.float32] + _index: int + _size: int + + def __init__(self, time_window: float) -> None: + self._lock = RLock() + self._time_window = time_window + self._threshold = 0.4 + self._max_points = int(_max_points_per_second * self._time_window) + self.reset_data() + + def reset_data(self) -> None: + with self._lock: + self._timestamps = np.zeros(self._max_points, dtype=np.float32) + self._positions = np.zeros((self._max_points, 2), dtype=np.float32) + self._index = 0 + self._size = 0 + + def add_position(self, pose: PoseStamped) -> None: + with self._lock: + self._timestamps[self._index] = time.time() + self._positions[self._index] = (pose.position.x, pose.position.y) + self._index = (self._index + 1) % self._max_points + self._size = min(self._size + 1, self._max_points) + + def _get_recent_positions(self) -> NDArray[np.float32]: + cutoff = time.time() - self._time_window + + if self._size == 0: + return np.empty((0, 2), dtype=np.float32) + + if self._size < self._max_points: + mask = self._timestamps[: self._size] >= cutoff + return self._positions[: self._size][mask] + + ts = np.concatenate([self._timestamps[self._index :], self._timestamps[: self._index]]) + pos = np.concatenate([self._positions[self._index :], self._positions[: self._index]]) + mask = ts >= cutoff + return cast("NDArray[np.float32]", pos[mask]) + + def is_stuck(self) -> bool: + with self._lock: + recent = self._get_recent_positions() + + if len(recent) == 0: + return False + + centroid = recent.mean(axis=0) + distances = np.linalg.norm(recent - centroid, axis=1) + + return bool(np.all(distances < self._threshold)) diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 60eb2dc37e..04cdf435f4 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -18,6 +18,7 @@ all_blueprints = { "unitree-go2": "dimos.robot.unitree_webrtc.unitree_go2_blueprints:standard", "unitree-go2-basic": "dimos.robot.unitree_webrtc.unitree_go2_blueprints:basic", + "unitree-go2-test-nav": "dimos.robot.unitree_webrtc.unitree_go2_blueprints:test_new_nav", "unitree-go2-shm": "dimos.robot.unitree_webrtc.unitree_go2_blueprints:standard_with_shm", "unitree-go2-jpegshm": "dimos.robot.unitree_webrtc.unitree_go2_blueprints:standard_with_jpegshm", "unitree-go2-jpeglcm": "dimos.robot.unitree_webrtc.unitree_go2_blueprints:standard_with_jpeglcm", @@ -64,6 +65,7 @@ "navigation_skill": "dimos.agents2.skills.navigation", "object_tracking": "dimos.perception.object_tracker", "osm_skill": "dimos.agents2.skills.osm", + "replanning_a_star_planner": "dimos.navigation.replanning_a_star.module", "ros_nav": "dimos.navigation.rosnav", "spatial_memory": "dimos.perception.spatial_perception", "speak_skill": "dimos.agents2.skills.speak_skill", diff --git a/dimos/robot/cli/dimos.py b/dimos/robot/cli/dimos.py index b70a59f96d..863dd77cd5 100644 --- a/dimos/robot/cli/dimos.py +++ b/dimos/robot/cli/dimos.py @@ -23,6 +23,7 @@ from dimos.core.global_config import GlobalConfig from dimos.protocol import pubsub from dimos.robot.all_blueprints import all_blueprints, get_blueprint_by_name, get_module_by_name +from dimos.robot.cli.topic import topic_echo, topic_send from dimos.utils.logging_config import setup_exception_handler RobotType = Enum("RobotType", {key.replace("-", "_").upper(): key for key in all_blueprints.keys()}) # type: ignore[misc] @@ -176,5 +177,25 @@ def humancli(ctx: typer.Context) -> None: humancli_main() +topic_app = typer.Typer(help="Topic commands for pub/sub") +main.add_typer(topic_app, name="topic") + + +@topic_app.command() +def echo( + topic: str = typer.Argument(..., help="Topic name to listen on (e.g., /goal_request)"), + type_name: str = typer.Argument(..., help="Message type (e.g., PoseStamped)"), +) -> None: + topic_echo(topic, type_name) + + +@topic_app.command() +def send( + topic: str = typer.Argument(..., help="Topic name to send to (e.g., /goal_request)"), + message_expr: str = typer.Argument(..., help="Python expression for the message"), +) -> None: + topic_send(topic, message_expr) + + if __name__ == "__main__": main() diff --git a/dimos/robot/cli/topic.py b/dimos/robot/cli/topic.py new file mode 100644 index 0000000000..05e8b084cd --- /dev/null +++ b/dimos/robot/cli/topic.py @@ -0,0 +1,102 @@ +# Copyright 2025 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 importlib +import time + +import typer + +from dimos.core.transport import LCMTransport, pLCMTransport + +_modules_to_try = [ + "dimos.msgs.geometry_msgs", + "dimos.msgs.nav_msgs", + "dimos.msgs.sensor_msgs", + "dimos.msgs.std_msgs", + "dimos.msgs.vision_msgs", + "dimos.msgs.foxglove_msgs", + "dimos.msgs.tf2_msgs", +] + + +def _resolve_type(type_name: str) -> type: + for module_name in _modules_to_try: + try: + module = importlib.import_module(module_name) + if hasattr(module, type_name): + return getattr(module, type_name) # type: ignore[no-any-return] + except ImportError: + continue + + raise ValueError(f"Could not find type '{type_name}' in any known message modules") + + +def topic_echo(topic: str, type_name: str) -> None: + msg_type = _resolve_type(type_name) + use_pickled = getattr(msg_type, "lcm_encode", None) is None + transport: pLCMTransport[object] | LCMTransport[object] = ( + pLCMTransport(topic) if use_pickled else LCMTransport(topic, msg_type) + ) + + def _on_message(msg: object) -> None: + print(msg) + + transport.subscribe(_on_message) + + typer.echo(f"Listening on {topic} for {type_name} messages... (Ctrl+C to stop)") + + try: + while True: + time.sleep(0.1) + except KeyboardInterrupt: + typer.echo("\nStopped.") + + +def topic_send(topic: str, message_expr: str) -> None: + eval_context: dict[str, object] = {} + modules_to_import = [ + "dimos.msgs.geometry_msgs", + "dimos.msgs.nav_msgs", + "dimos.msgs.sensor_msgs", + "dimos.msgs.std_msgs", + "dimos.msgs.vision_msgs", + "dimos.msgs.foxglove_msgs", + "dimos.msgs.tf2_msgs", + ] + + for module_name in modules_to_import: + try: + module = importlib.import_module(module_name) + for name in getattr(module, "__all__", dir(module)): + if not name.startswith("_"): + obj = getattr(module, name, None) + if obj is not None: + eval_context[name] = obj + except ImportError: + continue + + try: + message = eval(message_expr, eval_context) + except Exception as e: + typer.echo(f"Error parsing message: {e}", err=True) + raise typer.Exit(1) + + msg_type = type(message) + use_pickled = getattr(msg_type, "lcm_encode", None) is None + transport: pLCMTransport[object] | LCMTransport[object] = ( + pLCMTransport(topic) if use_pickled else LCMTransport(topic, msg_type) + ) + + transport.broadcast(None, message) # type: ignore[arg-type] + typer.echo(f"Sent to {topic}: {message}") diff --git a/dimos/robot/drone/test_drone.py b/dimos/robot/drone/test_drone.py index 385aef3e0c..1ff0c1a781 100644 --- a/dimos/robot/drone/test_drone.py +++ b/dimos/robot/drone/test_drone.py @@ -948,7 +948,7 @@ def test_output_clamping(self) -> None: ) # Large error should be clamped - vx, vy, vz = controller.compute_velocity_control( + vx, vy, _vz = controller.compute_velocity_control( target_x=1000, target_y=1000, center_x=0, center_y=0, dt=0.1 ) self.assertLessEqual(abs(vx), max_vel) @@ -1020,7 +1020,7 @@ def test_velocity_from_bbox_center_error(self) -> None: frame_center = (320, 180) bbox_center = (400, 180) - vx, vy, vz = controller.compute_velocity_control( + vx, vy, _vz = controller.compute_velocity_control( target_x=bbox_center[0], target_y=bbox_center[1], center_x=frame_center[0], diff --git a/dimos/robot/unitree_webrtc/type/map.py b/dimos/robot/unitree_webrtc/type/map.py index a1eac3b160..97c5041bee 100644 --- a/dimos/robot/unitree_webrtc/type/map.py +++ b/dimos/robot/unitree_webrtc/type/map.py @@ -12,15 +12,19 @@ # See the License for the specific language governing permissions and # limitations under the License. +from pathlib import Path import time +from typing import Any -import numpy as np import open3d as o3d # type: ignore[import-untyped] from reactivex import interval from reactivex.disposable import Disposable from dimos.core import DimosCluster, In, LCMTransport, Module, Out, rpc from dimos.core.global_config import GlobalConfig +from dimos.mapping.pointclouds.accumulators.general import GeneralPointCloudAccumulator +from dimos.mapping.pointclouds.accumulators.protocol import PointCloudAccumulator +from dimos.mapping.pointclouds.occupancy import general_occupancy from dimos.msgs.nav_msgs import OccupancyGrid from dimos.msgs.sensor_msgs import PointCloud2 from dimos.robot.unitree.connection.go2 import Go2ConnectionProtocol @@ -31,17 +35,18 @@ class Map(Module): lidar: In[LidarMessage] global_map: Out[LidarMessage] global_costmap: Out[OccupancyGrid] - local_costmap: Out[OccupancyGrid] - pointcloud: o3d.geometry.PointCloud = o3d.geometry.PointCloud() + _point_cloud_accumulator: PointCloudAccumulator + _global_config: GlobalConfig + _preloaded_occupancy: OccupancyGrid | None = None def __init__( # type: ignore[no-untyped-def] self, voxel_size: float = 0.05, cost_resolution: float = 0.05, global_publish_interval: float | None = None, - min_height: float = 0.15, - max_height: float = 0.6, + min_height: float = 0.10, + max_height: float = 0.5, global_config: GlobalConfig | None = None, **kwargs, ) -> None: @@ -50,10 +55,13 @@ def __init__( # type: ignore[no-untyped-def] self.global_publish_interval = global_publish_interval self.min_height = min_height self.max_height = max_height + self._global_config = global_config or GlobalConfig() + self._point_cloud_accumulator = GeneralPointCloudAccumulator( + self.voxel_size, self._global_config + ) - if global_config: - if global_config.simulation: - self.min_height = 0.3 + if self._global_config.simulation: + self.min_height = 0.3 super().__init__(**kwargs) @@ -61,26 +69,11 @@ def __init__( # type: ignore[no-untyped-def] def start(self) -> None: super().start() - unsub = self.lidar.subscribe(self.add_frame) - self._disposables.add(Disposable(unsub)) - - def publish(_) -> None: # type: ignore[no-untyped-def] - self.global_map.publish(self.to_lidar_message()) - - # temporary, not sure if it belogs in mapper - # used only for visualizations, not for any algo - occupancygrid = OccupancyGrid.from_pointcloud( - self.to_lidar_message(), - resolution=self.cost_resolution, - min_height=self.min_height, - max_height=self.max_height, - ) - - self.global_costmap.publish(occupancygrid) + self._disposables.add(Disposable(self.lidar.subscribe(self.add_frame))) if self.global_publish_interval is not None: - unsub = interval(self.global_publish_interval).subscribe(publish) # type: ignore[assignment] - self._disposables.add(unsub) # type: ignore[arg-type] + unsub = interval(self.global_publish_interval).subscribe(self._publish) + self._disposables.add(unsub) @rpc def stop(self) -> None: @@ -88,84 +81,45 @@ def stop(self) -> None: def to_PointCloud2(self) -> PointCloud2: return PointCloud2( - pointcloud=self.pointcloud, + pointcloud=self._point_cloud_accumulator.get_point_cloud(), ts=time.time(), ) def to_lidar_message(self) -> LidarMessage: return LidarMessage( - pointcloud=self.pointcloud, + pointcloud=self._point_cloud_accumulator.get_point_cloud(), origin=[0.0, 0.0, 0.0], resolution=self.voxel_size, ts=time.time(), ) + # TODO: Why is this RPC? @rpc - def add_frame(self, frame: LidarMessage) -> "Map": # type: ignore[return] - """Voxelise *frame* and splice it into the running map.""" - new_pct = frame.pointcloud.voxel_down_sample(voxel_size=self.voxel_size) - - # Skip for empty pointclouds. - if len(new_pct.points) == 0: - return self - - self.pointcloud = splice_cylinder(self.pointcloud, new_pct, shrink=0.5) - local_costmap = OccupancyGrid.from_pointcloud( - frame, - resolution=self.cost_resolution, - min_height=0.15, - max_height=0.6, - ).gradient(max_distance=0.25) - self.local_costmap.publish(local_costmap) + def add_frame(self, frame: LidarMessage) -> None: + self._point_cloud_accumulator.add(frame.pointcloud) @property def o3d_geometry(self) -> o3d.geometry.PointCloud: - return self.pointcloud - + return self._point_cloud_accumulator.get_point_cloud() -def splice_sphere( - map_pcd: o3d.geometry.PointCloud, - patch_pcd: o3d.geometry.PointCloud, - shrink: float = 0.95, -) -> o3d.geometry.PointCloud: - center = patch_pcd.get_center() - radius = np.linalg.norm(np.asarray(patch_pcd.points) - center, axis=1).max() * shrink - dists = np.linalg.norm(np.asarray(map_pcd.points) - center, axis=1) - victims = np.nonzero(dists < radius)[0] - survivors = map_pcd.select_by_index(victims, invert=True) - return survivors + patch_pcd + def _publish(self, _: Any) -> None: + self.global_map.publish(self.to_lidar_message()) + occupancygrid = general_occupancy( + self.to_lidar_message(), + resolution=self.cost_resolution, + min_height=self.min_height, + max_height=self.max_height, + ) -def splice_cylinder( - map_pcd: o3d.geometry.PointCloud, - patch_pcd: o3d.geometry.PointCloud, - axis: int = 2, - shrink: float = 0.95, -) -> o3d.geometry.PointCloud: - center = patch_pcd.get_center() - patch_pts = np.asarray(patch_pcd.points) - - # Axes perpendicular to cylinder - axes = [0, 1, 2] - axes.remove(axis) - - planar_dists = np.linalg.norm(patch_pts[:, axes] - center[axes], axis=1) - radius = planar_dists.max() * shrink - - axis_min = (patch_pts[:, axis].min() - center[axis]) * shrink + center[axis] - axis_max = (patch_pts[:, axis].max() - center[axis]) * shrink + center[axis] - - map_pts = np.asarray(map_pcd.points) - planar_dists_map = np.linalg.norm(map_pts[:, axes] - center[axes], axis=1) - - victims = np.nonzero( - (planar_dists_map < radius) - & (map_pts[:, axis] >= axis_min) - & (map_pts[:, axis] <= axis_max) - )[0] + # When debugging occupancy navigation, load a predefined occupancy grid. + if self._global_config.mujoco_global_costmap_from_occupancy: + if self._preloaded_occupancy is None: + path = Path(self._global_config.mujoco_global_costmap_from_occupancy) + self._preloaded_occupancy = OccupancyGrid.from_path(path) + occupancygrid = self._preloaded_occupancy - survivors = map_pcd.select_by_index(victims, invert=True) - return survivors + patch_pcd + self.global_costmap.publish(occupancygrid) mapper = Map.blueprint @@ -175,7 +129,6 @@ def deploy(dimos: DimosCluster, connection: Go2ConnectionProtocol): # type: ign mapper = dimos.deploy(Map, global_publish_interval=1.0) # type: ignore[attr-defined] mapper.global_map.transport = LCMTransport("/global_map", LidarMessage) mapper.global_costmap.transport = LCMTransport("/global_costmap", OccupancyGrid) - mapper.local_costmap.transport = LCMTransport("/local_costmap", OccupancyGrid) mapper.lidar.connect(connection.pointcloud) # type: ignore[attr-defined] mapper.start() return mapper diff --git a/dimos/robot/unitree_webrtc/type/test_map.py b/dimos/robot/unitree_webrtc/type/test_map.py index 12ee8f832d..d0479a782c 100644 --- a/dimos/robot/unitree_webrtc/type/test_map.py +++ b/dimos/robot/unitree_webrtc/type/test_map.py @@ -14,10 +14,11 @@ import pytest +from dimos.mapping.pointclouds.accumulators.general import _splice_cylinder from dimos.robot.unitree_webrtc.testing.helpers import show3d from dimos.robot.unitree_webrtc.testing.mock import Mock from dimos.robot.unitree_webrtc.type.lidar import LidarMessage -from dimos.robot.unitree_webrtc.type.map import Map, splice_sphere +from dimos.robot.unitree_webrtc.type.map import Map from dimos.utils.testing import SensorReplay @@ -48,7 +49,7 @@ def test_reconstruction_with_realtime_vis() -> None: for frame in mock.iterate(): map.add_frame(frame) - show3d(map.pointcloud, title="Reconstructed Map").run() + show3d(map.o3d_geometry, title="Reconstructed Map").run() @pytest.mark.vis @@ -56,7 +57,7 @@ def test_splice_vis() -> None: mock = Mock("test") target = mock.load("a") insert = mock.load("b") - show3d(splice_sphere(target.pointcloud, insert.pointcloud, shrink=0.7)).run() + show3d(_splice_cylinder(target.pointcloud, insert.pointcloud, shrink=0.7)).run() @pytest.mark.vis @@ -69,32 +70,35 @@ def test_robot_vis() -> None: for frame in mock.iterate(): map.add_frame(frame) - show3d(map.pointcloud, title="global dynamic map test").run() + show3d(map.o3d_geometry, title="global dynamic map test").run() -def test_robot_mapping() -> None: - lidar_replay = SensorReplay("office_lidar", autocast=LidarMessage.from_msg) +@pytest.fixture +def map_(): map = Map(voxel_size=0.5) + yield map + map.stop() + + +def test_robot_mapping(map_) -> None: + lidar_replay = SensorReplay("office_lidar", autocast=LidarMessage.from_msg) # Mock the output streams to avoid publishing errors class MockStream: def publish(self, msg) -> None: pass # Do nothing - map.local_costmap = MockStream() - map.global_costmap = MockStream() - map.global_map = MockStream() + map_.global_costmap = MockStream() + map_.global_map = MockStream() # Process all frames from replay for frame in lidar_replay.iterate(): - map.add_frame(frame) + map_.add_frame(frame) # Check the built map - global_map = map.to_lidar_message() + global_map = map_.to_lidar_message() pointcloud = global_map.pointcloud # Verify map has points assert len(pointcloud.points) > 0 print(f"Map contains {len(pointcloud.points)} points") - - map._close_module() diff --git a/dimos/robot/unitree_webrtc/unitree_g1_blueprints.py b/dimos/robot/unitree_webrtc/unitree_g1_blueprints.py index 0ebceac51e..0cc76ae51b 100644 --- a/dimos/robot/unitree_webrtc/unitree_g1_blueprints.py +++ b/dimos/robot/unitree_webrtc/unitree_g1_blueprints.py @@ -51,7 +51,7 @@ from dimos.navigation.frontier_exploration import ( wavefront_frontier_explorer, ) -from dimos.navigation.global_planner import astar_planner +from dimos.navigation.global_planner.planner import astar_planner from dimos.navigation.local_planner.holonomic_local_planner import ( holonomic_local_planner, ) diff --git a/dimos/robot/unitree_webrtc/unitree_go2_blueprints.py b/dimos/robot/unitree_webrtc/unitree_go2_blueprints.py index 77be9797ad..c5366cb460 100644 --- a/dimos/robot/unitree_webrtc/unitree_go2_blueprints.py +++ b/dimos/robot/unitree_webrtc/unitree_go2_blueprints.py @@ -34,10 +34,13 @@ from dimos.navigation.frontier_exploration import ( wavefront_frontier_explorer, ) -from dimos.navigation.global_planner import astar_planner +from dimos.navigation.global_planner.planner import astar_planner from dimos.navigation.local_planner.holonomic_local_planner import ( holonomic_local_planner, ) +from dimos.navigation.replanning_a_star.module import ( + replanning_a_star_planner, +) from dimos.perception.object_tracker import object_tracking from dimos.perception.spatial_perception import spatial_memory from dimos.robot.foxglove_bridge import foxglove_bridge @@ -83,6 +86,28 @@ utilization(), ).global_config(n_dask_workers=8) +test_new_nav = ( + autoconnect( + go2_connection(), + mapper(voxel_size=0.5, global_publish_interval=2.5), + # astar_planner(), + # holonomic_local_planner(), + # behavior_tree_navigator(), + replanning_a_star_planner(), + wavefront_frontier_explorer(), + websocket_vis(), + foxglove_bridge(), + ) + .global_config(n_dask_workers=4, robot_model="unitree_go2") + .transports( + { + ("color_image", Image): LCMTransport("/go2/color_image", Image), + ("camera_pose", PoseStamped): LCMTransport("/go2/camera_pose", PoseStamped), + ("camera_info", CameraInfo): LCMTransport("/go2/camera_info", CameraInfo), + } + ) +) + standard_with_jpeglcm = standard.transports( { ("color_image", Image): JpegLcmTransport("/go2/color_image", Image), diff --git a/dimos/simulation/mujoco/constants.py b/dimos/simulation/mujoco/constants.py index 59e8f580dc..2368768954 100644 --- a/dimos/simulation/mujoco/constants.py +++ b/dimos/simulation/mujoco/constants.py @@ -28,7 +28,7 @@ LIDAR_RESOLUTION = 0.05 # Simulation timing constants -STEPS_PER_FRAME = 7 +STEPS_PER_FRAME = 1 VIDEO_FPS = 20 LIDAR_FPS = 2 diff --git a/dimos/simulation/mujoco/model.py b/dimos/simulation/mujoco/model.py index 43975c86e1..ae8055be48 100644 --- a/dimos/simulation/mujoco/model.py +++ b/dimos/simulation/mujoco/model.py @@ -15,6 +15,7 @@ # limitations under the License. +from pathlib import Path import xml.etree.ElementTree as ET from etils import epath # type: ignore[import-untyped] @@ -22,6 +23,9 @@ from mujoco_playground._src import mjx_env # type: ignore[import-untyped] import numpy as np +from dimos.core.global_config import GlobalConfig +from dimos.mapping.occupancy.extrude_occupancy import generate_mujoco_scene +from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid from dimos.simulation.mujoco.input_controller import InputController from dimos.simulation.mujoco.policy import G1OnnxController, Go1OnnxController, OnnxController @@ -41,11 +45,11 @@ def get_assets() -> dict[str, bytes]: def load_model( - input_device: InputController, robot: str, scene: str + input_device: InputController, robot: str, scene_xml: str ) -> tuple[mujoco.MjModel, mujoco.MjData]: mujoco.set_mjcb_control(None) - xml_string = get_model_xml(robot, scene) + xml_string = get_model_xml(robot, scene_xml) model = mujoco.MjModel.from_xml_string(xml_string, assets=get_assets()) data = mujoco.MjData(model) @@ -83,11 +87,19 @@ def load_model( return model, data -def get_model_xml(robot: str, scene: str) -> str: - xml_file = (DATA_DIR / f"scene_{scene}.xml").as_posix() - - tree = ET.parse(xml_file) - root = tree.getroot() - root.set("model", f"{robot}_{scene}") +def get_model_xml(robot: str, scene_xml: str) -> str: + root = ET.fromstring(scene_xml) + root.set("model", f"{robot}_scene") root.insert(0, ET.Element("include", file=f"{robot}.xml")) return ET.tostring(root, encoding="unicode") + + +def load_scene_xml(config: GlobalConfig) -> str: + if config.mujoco_room_from_occupancy: + path = Path(config.mujoco_room_from_occupancy) + return generate_mujoco_scene(OccupancyGrid.from_path(path)) + + mujoco_room = config.mujoco_room or "office1" + xml_file = (DATA_DIR / f"scene_{mujoco_room}.xml").as_posix() + with open(xml_file) as f: + return f.read() diff --git a/dimos/simulation/mujoco/mujoco_process.py b/dimos/simulation/mujoco/mujoco_process.py index dd98a283da..540498b84d 100755 --- a/dimos/simulation/mujoco/mujoco_process.py +++ b/dimos/simulation/mujoco/mujoco_process.py @@ -41,7 +41,7 @@ VIDEO_WIDTH, ) from dimos.simulation.mujoco.depth_camera import depth_image_to_point_cloud -from dimos.simulation.mujoco.model import load_model +from dimos.simulation.mujoco.model import load_model, load_scene_xml from dimos.simulation.mujoco.shared_memory import ShmReader from dimos.utils.logging_config import setup_logger @@ -76,12 +76,8 @@ def _run_simulation(config: GlobalConfig, shm: ShmReader) -> None: if robot_name == "unitree_go2": robot_name = "unitree_go1" - mujoco_room = getattr(config, "mujoco_room", "office1") - if mujoco_room is None: - mujoco_room = "office1" - controller = MockController(shm) - model, data = load_model(controller, robot=robot_name, scene=mujoco_room) + model, data = load_model(controller, robot=robot_name, scene_xml=load_scene_xml(config)) if model is None or data is None: raise ValueError("Failed to load MuJoCo model: model or data is None") @@ -94,7 +90,9 @@ def _run_simulation(config: GlobalConfig, shm: ShmReader) -> None: case _: z = 0 - data.qpos[0:3] = [-1, 1, z] + pos = config.mujoco_start_pos_float + + data.qpos[0:3] = [pos[0], pos[1], z] mujoco.mj_forward(model, data) @@ -129,6 +127,12 @@ def _run_simulation(config: GlobalConfig, shm: ShmReader) -> None: video_interval = 1.0 / VIDEO_FPS lidar_interval = 1.0 / LIDAR_FPS + if config.mujoco_camera_position_float is not None: + m_viewer.cam.lookat = config.mujoco_camera_position_float[0:3] + m_viewer.cam.distance = config.mujoco_camera_position_float[3] + m_viewer.cam.azimuth = config.mujoco_camera_position_float[4] + m_viewer.cam.elevation = config.mujoco_camera_position_float[5] + while m_viewer.is_running() and not shm.should_stop(): step_start = time.time() diff --git a/dimos/web/websocket_vis/websocket_vis_module.py b/dimos/web/websocket_vis/websocket_vis_module.py index 341c19a66a..6ce7ef8212 100644 --- a/dimos/web/websocket_vis/websocket_vis_module.py +++ b/dimos/web/websocket_vis/websocket_vis_module.py @@ -32,6 +32,8 @@ import uvicorn from dimos.core import In, Module, Out, rpc +from dimos.mapping.occupancy.gradient import gradient +from dimos.mapping.occupancy.inflation import simple_inflate from dimos.mapping.types import LatLon from dimos.msgs.geometry_msgs import PoseStamped, Twist, TwistStamped, Vector3 from dimos.msgs.nav_msgs import OccupancyGrid, Path @@ -217,7 +219,9 @@ async def click(sid, position) -> None: # type: ignore[no-untyped-def] frame_id="world", ) self.goal_request.publish(goal) - logger.info(f"Click goal published: ({goal.position.x:.2f}, {goal.position.y:.2f})") + logger.info( + "Click goal published", x=round(goal.position.x, 3), y=round(goal.position.y, 3) + ) @self.sio.event # type: ignore[misc, untyped-decorator] async def gps_goal(sid: str, goal: dict[str, float]) -> None: @@ -312,7 +316,7 @@ def _on_global_costmap(self, msg: OccupancyGrid) -> None: def _process_costmap(self, costmap: OccupancyGrid) -> dict[str, Any]: """Convert OccupancyGrid to visualization format.""" - costmap = costmap.inflate(0.1).gradient(max_distance=1.0) + costmap = gradient(simple_inflate(costmap, 0.1), max_distance=1.0) grid_data = self.costmap_encoder.encode_costmap(costmap.grid) return { diff --git a/pyproject.toml b/pyproject.toml index 676dc9b32e..b4889ec6b3 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -1,5 +1,5 @@ [build-system] -requires = ["setuptools>=70", "wheel"] +requires = ["setuptools>=70", "wheel", "pybind11>=2.12"] build-backend = "setuptools.build_meta" [tool.setuptools] diff --git a/setup.py b/setup.py index 15fa5aa750..2ce49d0087 100644 --- a/setup.py +++ b/setup.py @@ -12,9 +12,30 @@ # See the License for the specific language governing permissions and # limitations under the License. +import os + +from pybind11.setup_helpers import Pybind11Extension, build_ext from setuptools import find_packages, setup +# C++ extensions +ext_modules = [ + Pybind11Extension( + "dimos.navigation.global_planner.min_cost_astar_ext", + [os.path.join("dimos", "navigation", "global_planner", "min_cost_astar_cpp.cpp")], + extra_compile_args=[ + "-O3", # Maximum optimization + "-march=native", # Optimize for current CPU + "-ffast-math", # Fast floating point + ], + define_macros=[ + ("NDEBUG", "1"), + ], + ), +] + setup( packages=find_packages(), package_dir={"": "."}, + ext_modules=ext_modules, + cmdclass={"build_ext": build_ext}, )