From c3b2544eb9994575f33c1ef7970941d9053d0a21 Mon Sep 17 00:00:00 2001 From: Paul Nechifor Date: Sat, 29 Nov 2025 06:57:24 +0200 Subject: [PATCH 01/42] mapping --- data/.lfs/apartment.tar.gz | 3 + data/.lfs/astar_general.png.tar.gz | 3 + data/.lfs/astar_min_cost.png.tar.gz | 3 + data/.lfs/occupancy_general.png.tar.gz | 3 + data/.lfs/occupancy_simple.npy.tar.gz | 3 + data/.lfs/occupancy_simple.png.tar.gz | 3 + .../visualize_occupancy_rainbow.png.tar.gz | 3 + .../.lfs/visualize_occupancy_turbo.png.tar.gz | 3 + .../mapping/occupancy/test_visualizations.py | 41 +++ dimos/mapping/occupancy/visualizations.py | 160 +++++++++++ .../pointclouds/accumulators/general.py | 70 +++++ .../pointclouds/accumulators/protocol.py | 28 ++ dimos/mapping/pointclouds/demo.py | 85 ++++++ dimos/mapping/pointclouds/occupancy.py | 268 ++++++++++++++++++ dimos/mapping/pointclouds/test_occupancy.py | 48 ++++ dimos/mapping/pointclouds/util.py | 58 ++++ dimos/msgs/nav_msgs/OccupancyGrid.py | 140 +-------- dimos/msgs/nav_msgs/test_OccupancyGrid.py | 9 +- dimos/navigation/global_planner/__init__.py | 3 +- dimos/navigation/global_planner/astar.py | 54 ++++ .../{algo.py => general_astar.py} | 2 +- .../global_planner/min_cost_astar.py | 210 ++++++++++++++ dimos/navigation/global_planner/planner.py | 4 +- dimos/navigation/global_planner/test_astar.py | 47 +++ dimos/robot/unitree_webrtc/type/map.py | 77 +---- dimos/robot/unitree_webrtc/type/test_map.py | 31 +- 26 files changed, 1132 insertions(+), 227 deletions(-) create mode 100644 data/.lfs/apartment.tar.gz create mode 100644 data/.lfs/astar_general.png.tar.gz create mode 100644 data/.lfs/astar_min_cost.png.tar.gz create mode 100644 data/.lfs/occupancy_general.png.tar.gz create mode 100644 data/.lfs/occupancy_simple.npy.tar.gz create mode 100644 data/.lfs/occupancy_simple.png.tar.gz create mode 100644 data/.lfs/visualize_occupancy_rainbow.png.tar.gz create mode 100644 data/.lfs/visualize_occupancy_turbo.png.tar.gz create mode 100644 dimos/mapping/occupancy/test_visualizations.py create mode 100644 dimos/mapping/occupancy/visualizations.py create mode 100644 dimos/mapping/pointclouds/accumulators/general.py create mode 100644 dimos/mapping/pointclouds/accumulators/protocol.py create mode 100644 dimos/mapping/pointclouds/demo.py create mode 100644 dimos/mapping/pointclouds/occupancy.py create mode 100644 dimos/mapping/pointclouds/test_occupancy.py create mode 100644 dimos/mapping/pointclouds/util.py create mode 100644 dimos/navigation/global_planner/astar.py rename dimos/navigation/global_planner/{algo.py => general_astar.py} (99%) create mode 100644 dimos/navigation/global_planner/min_cost_astar.py create mode 100644 dimos/navigation/global_planner/test_astar.py 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_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/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/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/mapping/occupancy/test_visualizations.py b/dimos/mapping/occupancy/test_visualizations.py new file mode 100644 index 0000000000..7f1f3fa638 --- /dev/null +++ b/dimos/mapping/occupancy/test_visualizations.py @@ -0,0 +1,41 @@ +#!/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 +import typer + +from dimos.mapping.occupancy.visualizations import visualize_occupancy_grid +from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid +from dimos.utils.data import get_data + +app = typer.Typer() + + +@pytest.fixture +def occupancy() -> PointCloud: + return OccupancyGrid(np.load(get_data("occupancy_simple.npy"))).gradient(max_distance=1.5) + + +@pytest.mark.parametrize("palette", ["rainbow", "turbo"]) +def test_visualize_occupancy_grid(occupancy, palette) -> None: + expected = cv2.imread(get_data(f"visualize_occupancy_{palette}.png"), cv2.IMREAD_COLOR) + + result = visualize_occupancy_grid(occupancy, 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/pointclouds/accumulators/general.py b/dimos/mapping/pointclouds/accumulators/general.py new file mode 100644 index 0000000000..20fb0c0657 --- /dev/null +++ b/dimos/mapping/pointclouds/accumulators/general.py @@ -0,0 +1,70 @@ +# 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] + + +class GeneralPointCloudAccumulator: + _point_cloud: PointCloud + _voxel_size: float + + def __init__(self, voxel_size: float) -> None: + self._point_cloud = PointCloud() + self._voxel_size = voxel_size + + 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..5d3c8475b5 --- /dev/null +++ b/dimos/mapping/pointclouds/demo.py @@ -0,0 +1,85 @@ +#!/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.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 = _get_occupancy_grid().gradient(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..4941fc92a2 --- /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 + +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/msgs/nav_msgs/OccupancyGrid.py b/dimos/msgs/nav_msgs/OccupancyGrid.py index 068422a7f2..bd931faa02 100644 --- a/dimos/msgs/nav_msgs/OccupancyGrid.py +++ b/dimos/msgs/nav_msgs/OccupancyGrid.py @@ -16,7 +16,7 @@ from enum import IntEnum import time -from typing import TYPE_CHECKING, BinaryIO +from typing import BinaryIO from dimos_lcm.nav_msgs import ( # type: ignore[import-untyped] MapMetaData, @@ -29,9 +29,6 @@ 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 - class CostValues(IntEnum): """Standard cost values for occupancy grid cells. @@ -329,141 +326,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. diff --git a/dimos/msgs/nav_msgs/test_OccupancyGrid.py b/dimos/msgs/nav_msgs/test_OccupancyGrid.py index a4cd36f9c0..595acb37f8 100644 --- a/dimos/msgs/nav_msgs/test_OccupancyGrid.py +++ b/dimos/msgs/nav_msgs/test_OccupancyGrid.py @@ -20,6 +20,7 @@ import numpy as np import pytest +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,9 +178,7 @@ 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) @@ -375,9 +374,7 @@ 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) diff --git a/dimos/navigation/global_planner/__init__.py b/dimos/navigation/global_planner/__init__.py index 275619659b..16571be7f1 100644 --- a/dimos/navigation/global_planner/__init__.py +++ b/dimos/navigation/global_planner/__init__.py @@ -1,4 +1,3 @@ -from dimos.navigation.global_planner.algo import astar from dimos.navigation.global_planner.planner import AstarPlanner, astar_planner -__all__ = ["AstarPlanner", "astar", "astar_planner"] +__all__ = ["AstarPlanner", "astar_planner"] diff --git a/dimos/navigation/global_planner/astar.py b/dimos/navigation/global_planner/astar.py new file mode 100644 index 0000000000..ba62d9008d --- /dev/null +++ b/dimos/navigation/global_planner/astar.py @@ -0,0 +1,54 @@ +# 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.msgs.geometry_msgs import VectorLike +from dimos.msgs.nav_msgs import OccupancyGrid, Path +from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid +from dimos.navigation.global_planner.general_astar import general_astar +from dimos.navigation.global_planner.min_cost_astar import min_cost_astar +from dimos.utils.logging_config import setup_logger + +Algorithm: TypeAlias = Literal["general", "min_cost"] + +logger = setup_logger(__file__) + + +def astar( + algorithm: Algorithm, + costmap: OccupancyGrid, + goal: VectorLike, + start: VectorLike, +) -> 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]) + + 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) + 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 fcd58cf5f0..0d6b021dda 100644 --- a/dimos/navigation/global_planner/algo.py +++ b/dimos/navigation/global_planner/general_astar.py @@ -21,7 +21,7 @@ logger = setup_logger("dimos.robot.unitree.global_planner.astar") -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..8174dd9cbe --- /dev/null +++ b/dimos/navigation/global_planner/min_cost_astar.py @@ -0,0 +1,210 @@ +# 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 + +# 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: + # Reconstruct the path + waypoints: list[PoseStamped] = [] + while current in parents: + world_point = costmap.grid_to_world(current) + # Create PoseStamped with identity quaternion (no orientation) + 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] + + # Add the start position + 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) + + # Reverse the path (start to goal) + 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 min_cost_astar( + costmap: OccupancyGrid, + goal: VectorLike, + start: VectorLike = (0.0, 0.0), + cost_threshold: int = 100, + unknown_penalty: float = 0.8, +) -> Path | None: + # Convert world coordinates to grid coordinates directly using vector-like inputs + start_vector = costmap.world_to_grid(start) + goal_vector = costmap.world_to_grid(goal) + + # Store positions as tuples for dictionary keys + start_tuple = (int(start_vector.x), int(start_vector.y)) + goal_tuple = (int(goal_vector.x), int(goal_vector.y)) + + # Check if goal is out of bounds + if not (0 <= goal_tuple[0] < costmap.width and 0 <= goal_tuple[1] < costmap.height): + return None + + # A* algorithm implementation + 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)) + + # Track nodes already in open set to avoid duplicates + open_set_hash: set[tuple[int, int]] = {start_tuple} + + while open_set: + # Get the node with the lowest priority (cost first, then distance) + _current_cost, _current_dist, current = heapq.heappop(open_set) + current_x, current_y = current + + # Remove from open set hash + if current in open_set_hash: + open_set_hash.remove(current) + + # Skip if already processed (can happen with duplicate entries) + if current in closed_set: + continue + + # Check if we've reached the goal + if current == goal_tuple: + return _reconstruct_path(parents, current, costmap, start_tuple, goal_tuple) + + # Add current node to closed set + closed_set.add(current) + + # Explore neighbors + for i, (dx, dy) in enumerate(_directions): + neighbor_x, neighbor_y = current_x + dx, current_y + dy + neighbor = (neighbor_x, neighbor_y) + + # Check if the neighbor is valid + if not (0 <= neighbor_x < costmap.width and 0 <= neighbor_y < costmap.height): + continue + + # Check if the neighbor is already explored + if neighbor in closed_set: + continue + + # Get the neighbor's cost value + neighbor_val = costmap.grid[neighbor_y, neighbor_x] + + # Skip if it's a hard obstacle + if neighbor_val >= cost_threshold: + continue + + # Calculate movement cost with penalties + # Unknown cells get half the penalty of obstacles + if neighbor_val == CostValues.UNKNOWN: # Unknown cell (-1) + # Unknown cells have a moderate traversal cost (half of obstacle threshold) + cell_cost = cost_threshold * unknown_penalty + elif neighbor_val == CostValues.FREE: # Free space (0) + # Free cells have minimal cost + cell_cost = 0.0 + else: + # Other cells use their actual cost value (1-99) + cell_cost = neighbor_val + + # Calculate tentative scores: accumulate cost and distance separately + 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 + # Only add if not already in open set to reduce duplicates + if neighbor not in open_set_hash: + heapq.heappush(open_set, (priority_cost, priority_dist, neighbor)) + open_set_hash.add(neighbor) + + return None diff --git a/dimos/navigation/global_planner/planner.py b/dimos/navigation/global_planner/planner.py index 7d4dbf8327..e44586f3ed 100644 --- a/dimos/navigation/global_planner/planner.py +++ b/dimos/navigation/global_planner/planner.py @@ -18,7 +18,7 @@ from dimos.core import In, Module, Out, rpc 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.general_astar import general_astar from dimos.utils.logging_config import setup_logger from dimos.utils.transform_utils import euler_to_quaternion @@ -208,7 +208,7 @@ def plan(self, goal: Pose) -> Path | None: costmap = self.latest_costmap.inflate(0.2).gradient(max_distance=1.5) # Run A* planning - path = astar(costmap, goal.position, robot_pos) + path = general_astar(costmap, goal.position, robot_pos) if path: path = resample_path(path, 0.1) diff --git a/dimos/navigation/global_planner/test_astar.py b/dimos/navigation/global_planner/test_astar.py new file mode 100644 index 0000000000..37732d640c --- /dev/null +++ b/dimos/navigation/global_planner/test_astar.py @@ -0,0 +1,47 @@ +# 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] +import pytest + +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 OccupancyGrid(np.load(get_data("occupancy_simple.npy"))).gradient(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, 0) + goal = Vector3(6.15, 10.0) + expected = Image.from_file(get_data(expected_image)) + + path = astar(mode, costmap, goal, start) + actual = visualize_occupancy_grid(costmap, "rainbow", path) + + np.testing.assert_array_equal(actual.data, expected.data) diff --git a/dimos/robot/unitree_webrtc/type/map.py b/dimos/robot/unitree_webrtc/type/map.py index 436afea811..2b1bb093d1 100644 --- a/dimos/robot/unitree_webrtc/type/map.py +++ b/dimos/robot/unitree_webrtc/type/map.py @@ -14,13 +14,15 @@ import time -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 @@ -33,12 +35,11 @@ class Map(Module): global_costmap: Out[OccupancyGrid] = None # type: ignore[assignment] local_costmap: Out[OccupancyGrid] = None # type: ignore[assignment] - pointcloud: o3d.geometry.PointCloud = o3d.geometry.PointCloud() + _point_cloud_accumulator: PointCloudAccumulator 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, @@ -46,10 +47,10 @@ def __init__( # type: ignore[no-untyped-def] **kwargs, ) -> None: self.voxel_size = voxel_size - self.cost_resolution = cost_resolution self.global_publish_interval = global_publish_interval self.min_height = min_height self.max_height = max_height + self._point_cloud_accumulator = GeneralPointCloudAccumulator(self.voxel_size) if global_config: if global_config.simulation: @@ -69,9 +70,9 @@ def publish(_) -> None: # type: ignore[no-untyped-def] # temporary, not sure if it belogs in mapper # used only for visualizations, not for any algo - occupancygrid = OccupancyGrid.from_pointcloud( + occupancygrid = general_occupancy( self.to_lidar_message(), - resolution=self.cost_resolution, + resolution=self.voxel_size, min_height=self.min_height, max_height=self.max_height, ) @@ -88,13 +89,13 @@ 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(), @@ -102,17 +103,10 @@ def to_lidar_message(self) -> LidarMessage: @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( + self._point_cloud_accumulator.add(frame.pointcloud) + local_costmap = general_occupancy( frame, - resolution=self.cost_resolution, + resolution=self.voxel_size, min_height=0.15, max_height=0.6, ).gradient(max_distance=0.25) @@ -120,52 +114,7 @@ def add_frame(self, frame: LidarMessage) -> "Map": # type: ignore[return] @property def o3d_geometry(self) -> o3d.geometry.PointCloud: - return self.pointcloud - - -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 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] - - survivors = map_pcd.select_by_index(victims, invert=True) - return survivors + patch_pcd + return self._point_cloud_accumulator.get_point_cloud() mapper = Map.blueprint diff --git a/dimos/robot/unitree_webrtc/type/test_map.py b/dimos/robot/unitree_webrtc/type/test_map.py index 12ee8f832d..bde2022a8f 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,36 @@ 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_.local_costmap = 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() From ddf381fba51373a71d21b0431eea41450e3bbbe2 Mon Sep 17 00:00:00 2001 From: Paul Nechifor Date: Tue, 9 Dec 2025 08:15:19 +0200 Subject: [PATCH 02/42] move inflation --- dimos/mapping/occupancy/inflation.py | 56 +++++++++++++++++++ dimos/msgs/nav_msgs/OccupancyGrid.py | 35 ------------ dimos/msgs/nav_msgs/test_OccupancyGrid.py | 5 +- dimos/navigation/bt_navigator/navigator.py | 3 +- .../wavefront_frontier_goal_selector.py | 3 +- dimos/navigation/global_planner/planner.py | 3 +- .../web/websocket_vis/websocket_vis_module.py | 3 +- 7 files changed, 67 insertions(+), 41 deletions(-) create mode 100644 dimos/mapping/occupancy/inflation.py diff --git a/dimos/mapping/occupancy/inflation.py b/dimos/mapping/occupancy/inflation.py new file mode 100644 index 0000000000..9a9da70975 --- /dev/null +++ b/dimos/mapping/occupancy/inflation.py @@ -0,0 +1,56 @@ +# 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 + +import numpy as np +from scipy import ndimage + +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 + 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=occupancy_grid.resolution, + origin=occupancy_grid.origin, + frame_id=occupancy_grid.frame_id, + ts=occupancy_grid.ts, + ) diff --git a/dimos/msgs/nav_msgs/OccupancyGrid.py b/dimos/msgs/nav_msgs/OccupancyGrid.py index bd931faa02..dba0d78539 100644 --- a/dimos/msgs/nav_msgs/OccupancyGrid.py +++ b/dimos/msgs/nav_msgs/OccupancyGrid.py @@ -172,41 +172,6 @@ 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, - ) - def world_to_grid(self, point: VectorLike) -> Vector3: """Convert world coordinates to grid coordinates. diff --git a/dimos/msgs/nav_msgs/test_OccupancyGrid.py b/dimos/msgs/nav_msgs/test_OccupancyGrid.py index 595acb37f8..c6fe8a472e 100644 --- a/dimos/msgs/nav_msgs/test_OccupancyGrid.py +++ b/dimos/msgs/nav_msgs/test_OccupancyGrid.py @@ -20,6 +20,7 @@ import numpy as np import pytest +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 @@ -180,7 +181,7 @@ def test_from_pointcloud() -> None: # Convert pointcloud to occupancy grid 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 @@ -376,7 +377,7 @@ def test_lcm_broadcast() -> None: # Create occupancy grid from pointcloud 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) diff --git a/dimos/navigation/bt_navigator/navigator.py b/dimos/navigation/bt_navigator/navigator.py index dc0ef35943..405c2832fa 100644 --- a/dimos/navigation/bt_navigator/navigator.py +++ b/dimos/navigation/bt_navigator/navigator.py @@ -27,6 +27,7 @@ from dimos.core import In, Module, Out, rpc from dimos.core.rpc_client import RpcCall +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 +286,7 @@ def _control_loop(self) -> None: self.cancel_goal() continue - costmap = self.latest_costmap.inflate(0.1).gradient(max_distance=1.0) + costmap = simple_inflate(self.latest_costmap, 0.1).gradient(max_distance=1.0) # Find safe goal position safe_goal_pos = find_safe_goal( diff --git a/dimos/navigation/frontier_exploration/wavefront_frontier_goal_selector.py b/dimos/navigation/frontier_exploration/wavefront_frontier_goal_selector.py index e8818a810f..c7bdc4b64c 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/planner.py b/dimos/navigation/global_planner/planner.py index e44586f3ed..22d36609f4 100644 --- a/dimos/navigation/global_planner/planner.py +++ b/dimos/navigation/global_planner/planner.py @@ -16,6 +16,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 Pose, PoseStamped from dimos.msgs.nav_msgs import OccupancyGrid, Path from dimos.navigation.global_planner.general_astar import general_astar @@ -205,7 +206,7 @@ def plan(self, goal: Pose) -> Path | None: # Get current position from odometry robot_pos = self.latest_odom.position - costmap = self.latest_costmap.inflate(0.2).gradient(max_distance=1.5) + costmap = simple_inflate(self.latest_costmap, 0.2).gradient(max_distance=1.5) # Run A* planning path = general_astar(costmap, goal.position, robot_pos) diff --git a/dimos/web/websocket_vis/websocket_vis_module.py b/dimos/web/websocket_vis/websocket_vis_module.py index 9926c0cd0b..43f50e5324 100644 --- a/dimos/web/websocket_vis/websocket_vis_module.py +++ b/dimos/web/websocket_vis/websocket_vis_module.py @@ -32,6 +32,7 @@ import uvicorn from dimos.core import In, Module, Out, rpc +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 @@ -278,7 +279,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 = simple_inflate(costmap, 0.1).gradient(max_distance=1.0) grid_data = self.costmap_encoder.encode_costmap(costmap.grid) return { From 130da36a42e3a64dbd82773a39fb54f8f89b2688 Mon Sep 17 00:00:00 2001 From: Paul Nechifor Date: Tue, 9 Dec 2025 08:38:07 +0200 Subject: [PATCH 03/42] break things out --- data/.lfs/inflation_simple.png.tar.gz | 3 + data/.lfs/inflation_voronoi.png.tar.gz | 3 + .../.lfs/make_navigation_map_mixed.png.tar.gz | 3 + .../make_navigation_map_simple.png.tar.gz | 3 + data/.lfs/overlay_occupied.png.tar.gz | 3 + data/.lfs/smooth_occupied.png.tar.gz | 3 + dimos/core/global_config.py | 6 ++ dimos/mapping/occupancy/conftest.py | 29 ++++++ dimos/mapping/occupancy/inflation.py | 65 +++++++++++++- dimos/mapping/occupancy/operations.py | 88 +++++++++++++++++++ dimos/mapping/occupancy/path_map.py | 41 +++++++++ dimos/mapping/occupancy/test_inflation.py | 39 ++++++++ dimos/mapping/occupancy/test_operations.py | 40 +++++++++ dimos/mapping/occupancy/test_path_map.py | 34 +++++++ .../mapping/occupancy/test_visualizations.py | 14 +-- dimos/msgs/nav_msgs/OccupancyGrid.py | 14 +++ dimos/navigation/global_planner/astar.py | 4 +- dimos/navigation/global_planner/planner.py | 37 +++++--- 18 files changed, 399 insertions(+), 30 deletions(-) create mode 100644 data/.lfs/inflation_simple.png.tar.gz create mode 100644 data/.lfs/inflation_voronoi.png.tar.gz create mode 100644 data/.lfs/make_navigation_map_mixed.png.tar.gz create mode 100644 data/.lfs/make_navigation_map_simple.png.tar.gz create mode 100644 data/.lfs/overlay_occupied.png.tar.gz create mode 100644 data/.lfs/smooth_occupied.png.tar.gz create mode 100644 dimos/mapping/occupancy/conftest.py create mode 100644 dimos/mapping/occupancy/operations.py create mode 100644 dimos/mapping/occupancy/path_map.py create mode 100644 dimos/mapping/occupancy/test_inflation.py create mode 100644 dimos/mapping/occupancy/test_operations.py create mode 100644 dimos/mapping/occupancy/test_path_map.py 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/inflation_voronoi.png.tar.gz b/data/.lfs/inflation_voronoi.png.tar.gz new file mode 100644 index 0000000000..41c8857980 --- /dev/null +++ b/data/.lfs/inflation_voronoi.png.tar.gz @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:6ca526be5908e6231a9915f2927f014778db6aca76e599dc22f413ba3595855e +size 3776 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..7870a1114a --- /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:e8fcb045521f356ab23e0daff9ed1d1102409498a7ff987edcba4e57255f4070 +size 8155 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..5831ee4aad --- /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:ec371b962c853377387af52ef4d742e8312b13d76ea6cc1e9182a1f0f23d884b +size 7474 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/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/dimos/core/global_config.py b/dimos/core/global_config.py index 69bd33beb8..1fae15cb7b 100644 --- a/dimos/core/global_config.py +++ b/dimos/core/global_config.py @@ -16,6 +16,9 @@ from pydantic_settings import BaseSettings, SettingsConfigDict +from dimos.mapping.occupancy.path_map import NavigationStrategy +from dimos.navigation.global_planner.astar import AStarAlgorithm + class GlobalConfig(BaseSettings): robot_ip: str | None = None @@ -24,6 +27,9 @@ class GlobalConfig(BaseSettings): n_dask_workers: int = 2 mujoco_room: str | None = None robot_model: str | None = None + robot_width: float = 0.4 + planner_strategy: NavigationStrategy = "mixed" + astar_algorithm: AStarAlgorithm = "min_cost" model_config = SettingsConfigDict( env_file=".env", diff --git a/dimos/mapping/occupancy/conftest.py b/dimos/mapping/occupancy/conftest.py new file mode 100644 index 0000000000..d5dec276a8 --- /dev/null +++ b/dimos/mapping/occupancy/conftest.py @@ -0,0 +1,29 @@ +# 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.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 occupancy.gradient(max_distance=1.5) diff --git a/dimos/mapping/occupancy/inflation.py b/dimos/mapping/occupancy/inflation.py index 9a9da70975..97d3cca250 100644 --- a/dimos/mapping/occupancy/inflation.py +++ b/dimos/mapping/occupancy/inflation.py @@ -12,8 +12,6 @@ # See the License for the specific language governing permissions and # limitations under the License. -from __future__ import annotations - import numpy as np from scipy import ndimage @@ -34,7 +32,6 @@ def simple_inflate(occupancy_grid: OccupancyGrid, radius: float) -> OccupancyGri grid_array = occupancy_grid.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) @@ -54,3 +51,65 @@ def simple_inflate(occupancy_grid: OccupancyGrid, radius: float) -> OccupancyGri frame_id=occupancy_grid.frame_id, ts=occupancy_grid.ts, ) + + +def voronoi_inflate(occupancy_grid: OccupancyGrid, radius: float) -> OccupancyGrid: + """Inflate obstacles by a given radius using Voronoi partitioning. + + Unlike simple_inflate, this function ensures that separate occupied zones + do not merge together during inflation. Each zone grows into free space + up to the specified radius, stopping at Voronoi boundaries to maintain + separation between distinct obstacles. + + Args: + occupancy_grid: Input occupancy grid + radius: Inflation radius in meters + Returns: + New OccupancyGrid with inflated obstacles (zones remain separate) + """ + # Convert radius to grid cells + cell_radius = radius / occupancy_grid.resolution + + grid_array = occupancy_grid.grid + occupied_mask = grid_array >= CostValues.OCCUPIED + + # Label connected occupied regions (8-connectivity). + structure_8conn = np.ones((3, 3), dtype=np.uint8) + labeled_zones, num_zones = ndimage.label(occupied_mask, structure=structure_8conn) + + if num_zones == 0: + return OccupancyGrid( + grid=grid_array.copy(), + resolution=occupancy_grid.resolution, + origin=occupancy_grid.origin, + frame_id=occupancy_grid.frame_id, + ts=occupancy_grid.ts, + ) + + # Compute distance from each free pixel to nearest occupied pixel + # and get indices of which occupied pixel is nearest. + distances, nearest_indices = ndimage.distance_transform_edt(~occupied_mask, return_indices=True) + + # Voronoi assignment: for each pixel, which zone is it closest to. + voronoi_labels = labeled_zones[nearest_indices[0], nearest_indices[1]] + + # Find Voronoi boundaries: pixels adjacent to a pixel with a different + # label. This creates a 1-pixel boundary between zones. + boundary = np.zeros_like(voronoi_labels, dtype=bool) + boundary[:-1, :] |= voronoi_labels[:-1, :] != voronoi_labels[1:, :] + boundary[1:, :] |= voronoi_labels[1:, :] != voronoi_labels[:-1, :] + boundary[:, :-1] |= voronoi_labels[:, :-1] != voronoi_labels[:, 1:] + boundary[:, 1:] |= voronoi_labels[:, 1:] != voronoi_labels[:, :-1] + + # Inflate: mark cells within radius that are not on Voronoi boundaries. + result_grid = grid_array.copy() + inflate_mask = (distances <= cell_radius) & (distances > 0) & ~boundary + result_grid[inflate_mask] = CostValues.OCCUPIED + + 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..522d0b3fa6 --- /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 + +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..d871f83e0b --- /dev/null +++ b/dimos/mapping/occupancy/path_map.py @@ -0,0 +1,41 @@ +# 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.inflation import simple_inflate, voronoi_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": + simple_inflate_ammount = 0.1 + costmap = smooth_occupied(occupancy_grid) + costmap = simple_inflate(costmap, simple_inflate_ammount) + costmap = overlay_occupied(costmap, occupancy_grid) + costmap = voronoi_inflate(costmap, half_width - simple_inflate_ammount) + else: + raise ValueError(f"Unknown strategy: {strategy}") + + return costmap.gradient(max_distance=gradient_distance) diff --git a/dimos/mapping/occupancy/test_inflation.py b/dimos/mapping/occupancy/test_inflation.py new file mode 100644 index 0000000000..0fb4317fc2 --- /dev/null +++ b/dimos/mapping/occupancy/test_inflation.py @@ -0,0 +1,39 @@ +#!/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.inflation import simple_inflate, voronoi_inflate +from dimos.mapping.occupancy.visualizations import visualize_occupancy_grid +from dimos.utils.data import get_data + + +@pytest.mark.parametrize("method", ["simple", "voronoi"]) +def test_inflation(occupancy, method) -> None: + expected = cv2.imread(get_data(f"inflation_{method}.png"), cv2.IMREAD_COLOR) + + match method: + case "simple": + og = simple_inflate(occupancy, 0.2) + case "voronoi": + og = voronoi_inflate(occupancy, 0.2) + case _: + raise ValueError(f"Unknown inflation method: {method}") + + 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_visualizations.py b/dimos/mapping/occupancy/test_visualizations.py index 7f1f3fa638..9823fa9855 100644 --- a/dimos/mapping/occupancy/test_visualizations.py +++ b/dimos/mapping/occupancy/test_visualizations.py @@ -16,26 +16,16 @@ import cv2 import numpy as np -from open3d.geometry import PointCloud # type: ignore[import-untyped] import pytest -import typer from dimos.mapping.occupancy.visualizations import visualize_occupancy_grid -from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid from dimos.utils.data import get_data -app = typer.Typer() - - -@pytest.fixture -def occupancy() -> PointCloud: - return OccupancyGrid(np.load(get_data("occupancy_simple.npy"))).gradient(max_distance=1.5) - @pytest.mark.parametrize("palette", ["rainbow", "turbo"]) -def test_visualize_occupancy_grid(occupancy, palette) -> None: +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, palette) + result = visualize_occupancy_grid(occupancy_gradient, palette) np.testing.assert_array_equal(result.data, expected) diff --git a/dimos/msgs/nav_msgs/OccupancyGrid.py b/dimos/msgs/nav_msgs/OccupancyGrid.py index dba0d78539..0e603a9103 100644 --- a/dimos/msgs/nav_msgs/OccupancyGrid.py +++ b/dimos/msgs/nav_msgs/OccupancyGrid.py @@ -436,3 +436,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/navigation/global_planner/astar.py b/dimos/navigation/global_planner/astar.py index ba62d9008d..f7c3a7478b 100644 --- a/dimos/navigation/global_planner/astar.py +++ b/dimos/navigation/global_planner/astar.py @@ -21,13 +21,13 @@ from dimos.navigation.global_planner.min_cost_astar import min_cost_astar from dimos.utils.logging_config import setup_logger -Algorithm: TypeAlias = Literal["general", "min_cost"] +AStarAlgorithm: TypeAlias = Literal["general", "min_cost"] logger = setup_logger(__file__) def astar( - algorithm: Algorithm, + algorithm: AStarAlgorithm, costmap: OccupancyGrid, goal: VectorLike, start: VectorLike, diff --git a/dimos/navigation/global_planner/planner.py b/dimos/navigation/global_planner/planner.py index 22d36609f4..2a0a18fbf9 100644 --- a/dimos/navigation/global_planner/planner.py +++ b/dimos/navigation/global_planner/planner.py @@ -16,10 +16,11 @@ from reactivex.disposable import Disposable from dimos.core import In, Module, Out, rpc -from dimos.mapping.occupancy.inflation import simple_inflate +from dimos.core.global_config import GlobalConfig +from dimos.mapping.occupancy.path_map import make_navigation_map from dimos.msgs.geometry_msgs import Pose, PoseStamped from dimos.msgs.nav_msgs import OccupancyGrid, Path -from dimos.navigation.global_planner.general_astar import general_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 @@ -150,13 +151,19 @@ class AstarPlanner(Module): # LCM outputs path: Out[Path] = None # type: ignore[assignment] - 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() @@ -206,18 +213,22 @@ def plan(self, goal: Pose) -> Path | None: # Get current position from odometry robot_pos = self.latest_odom.position - costmap = simple_inflate(self.latest_costmap, 0.2).gradient(max_distance=1.5) - # Run A* planning - path = general_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.position, robot_pos) - logger.warning("No path found to the goal.") - return None + if not path: + logger.warning("No path found to the goal.") + return None + + path = resample_path(path, 0.1) + logger.debug(f"Path found with {len(path.poses)} waypoints") + return path astar_planner = AstarPlanner.blueprint From 98c899be3c673779e6e421904276ad59d2bb5eed Mon Sep 17 00:00:00 2001 From: Paul Nechifor Date: Wed, 10 Dec 2025 08:32:28 +0200 Subject: [PATCH 04/42] fix typing and imports --- dimos/core/global_config.py | 2 +- dimos/mapping/occupancy/inflation.py | 2 +- dimos/mapping/occupancy/operations.py | 2 +- dimos/mapping/pointclouds/occupancy.py | 2 +- dimos/navigation/global_planner/__init__.py | 3 --- dimos/navigation/global_planner/astar.py | 7 ++----- dimos/navigation/global_planner/planner.py | 7 +++---- dimos/navigation/global_planner/types.py | 17 +++++++++++++++++ .../unitree_webrtc/unitree_g1_blueprints.py | 2 +- .../unitree_webrtc/unitree_go2_blueprints.py | 2 +- 10 files changed, 28 insertions(+), 18 deletions(-) delete mode 100644 dimos/navigation/global_planner/__init__.py create mode 100644 dimos/navigation/global_planner/types.py diff --git a/dimos/core/global_config.py b/dimos/core/global_config.py index 1fae15cb7b..bd2246f562 100644 --- a/dimos/core/global_config.py +++ b/dimos/core/global_config.py @@ -17,7 +17,7 @@ from pydantic_settings import BaseSettings, SettingsConfigDict from dimos.mapping.occupancy.path_map import NavigationStrategy -from dimos.navigation.global_planner.astar import AStarAlgorithm +from dimos.navigation.global_planner.types import AStarAlgorithm class GlobalConfig(BaseSettings): diff --git a/dimos/mapping/occupancy/inflation.py b/dimos/mapping/occupancy/inflation.py index 97d3cca250..f0279f9c57 100644 --- a/dimos/mapping/occupancy/inflation.py +++ b/dimos/mapping/occupancy/inflation.py @@ -13,7 +13,7 @@ # limitations under the License. import numpy as np -from scipy import ndimage +from scipy import ndimage # type: ignore[import-untyped] from dimos.msgs.nav_msgs.OccupancyGrid import CostValues, OccupancyGrid diff --git a/dimos/mapping/occupancy/operations.py b/dimos/mapping/occupancy/operations.py index 522d0b3fa6..bc22a2afa4 100644 --- a/dimos/mapping/occupancy/operations.py +++ b/dimos/mapping/occupancy/operations.py @@ -13,7 +13,7 @@ # limitations under the License. import numpy as np -from scipy import ndimage +from scipy import ndimage # type: ignore[import-untyped] from dimos.msgs.nav_msgs.OccupancyGrid import CostValues, OccupancyGrid diff --git a/dimos/mapping/pointclouds/occupancy.py b/dimos/mapping/pointclouds/occupancy.py index 4941fc92a2..849be722bc 100644 --- a/dimos/mapping/pointclouds/occupancy.py +++ b/dimos/mapping/pointclouds/occupancy.py @@ -17,7 +17,7 @@ from typing import TYPE_CHECKING import numpy as np -from scipy import ndimage +from scipy import ndimage # type: ignore[import-untyped] from dimos.msgs.geometry_msgs import Pose from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid diff --git a/dimos/navigation/global_planner/__init__.py b/dimos/navigation/global_planner/__init__.py deleted file mode 100644 index 16571be7f1..0000000000 --- a/dimos/navigation/global_planner/__init__.py +++ /dev/null @@ -1,3 +0,0 @@ -from dimos.navigation.global_planner.planner import AstarPlanner, astar_planner - -__all__ = ["AstarPlanner", "astar_planner"] diff --git a/dimos/navigation/global_planner/astar.py b/dimos/navigation/global_planner/astar.py index f7c3a7478b..54711a9107 100644 --- a/dimos/navigation/global_planner/astar.py +++ b/dimos/navigation/global_planner/astar.py @@ -12,18 +12,15 @@ # See the License for the specific language governing permissions and # limitations under the License. -from typing import Literal, TypeAlias - from dimos.msgs.geometry_msgs import VectorLike from dimos.msgs.nav_msgs import OccupancyGrid, Path from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid 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 -AStarAlgorithm: TypeAlias = Literal["general", "min_cost"] - -logger = setup_logger(__file__) +logger = setup_logger() def astar( diff --git a/dimos/navigation/global_planner/planner.py b/dimos/navigation/global_planner/planner.py index d930933073..90182af43c 100644 --- a/dimos/navigation/global_planner/planner.py +++ b/dimos/navigation/global_planner/planner.py @@ -13,12 +13,14 @@ # limitations under the License. +import math + 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.msgs.geometry_msgs import Pose, PoseStamped +from dimos.msgs.geometry_msgs import Pose, PoseStamped, Quaternion, Vector3 from dimos.msgs.nav_msgs import OccupancyGrid, Path from dimos.navigation.global_planner.astar import astar from dimos.utils.logging_config import setup_logger @@ -26,9 +28,6 @@ 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] 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/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 17f25e8560..a34e72fb2c 100644 --- a/dimos/robot/unitree_webrtc/unitree_go2_blueprints.py +++ b/dimos/robot/unitree_webrtc/unitree_go2_blueprints.py @@ -34,7 +34,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, ) From 3edcf9b1b21a6162dcb02a9c4f030e1e575c913f Mon Sep 17 00:00:00 2001 From: Paul Nechifor Date: Wed, 10 Dec 2025 09:09:20 +0200 Subject: [PATCH 05/42] move resampling --- dimos/mapping/occupancy/path_resampling.py | 136 ++++++++++++++++++ dimos/navigation/global_planner/planner.py | 152 ++------------------- 2 files changed, 148 insertions(+), 140 deletions(-) create mode 100644 dimos/mapping/occupancy/path_resampling.py diff --git a/dimos/mapping/occupancy/path_resampling.py b/dimos/mapping/occupancy/path_resampling.py new file mode 100644 index 0000000000..7777fa919e --- /dev/null +++ b/dimos/mapping/occupancy/path_resampling.py @@ -0,0 +1,136 @@ +# 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 + +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 None + + # 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 + + +def 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 diff --git a/dimos/navigation/global_planner/planner.py b/dimos/navigation/global_planner/planner.py index 90182af43c..e8a7c184f6 100644 --- a/dimos/navigation/global_planner/planner.py +++ b/dimos/navigation/global_planner/planner.py @@ -13,134 +13,20 @@ # limitations under the License. -import math - 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.msgs.geometry_msgs import Pose, PoseStamped, Quaternion, Vector3 +from dimos.mapping.occupancy.path_resampling import resample_path +from dimos.msgs.geometry_msgs import Pose, PoseStamped from dimos.msgs.nav_msgs import OccupancyGrid, Path 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() - -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 target: In[PoseStamped] = None # type: ignore[assignment] @@ -167,50 +53,38 @@ def __init__( 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 = make_navigation_map( @@ -219,15 +93,13 @@ def plan(self, goal: Pose) -> Path | None: strategy=self._global_config.planner_strategy, ) - path = astar(self._global_config.astar_algorithm, costmap, goal.position, robot_pos) + 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 - path = resample_path(path, 0.1) - logger.debug(f"Path found with {len(path.poses)} waypoints") - return path + return resample_path(path, goal_pose, 0.1) astar_planner = AstarPlanner.blueprint From e1a51d34944710f180fee95c2cdeda1730615b15 Mon Sep 17 00:00:00 2001 From: Paul Nechifor Date: Wed, 10 Dec 2025 09:40:40 +0200 Subject: [PATCH 06/42] add test for resampling --- data/.lfs/resample_path_simple.png.tar.gz | 3 + dimos/mapping/occupancy/path_resampling.py | 2 +- .../mapping/occupancy/test_path_resampling.py | 42 +++++++++ dimos/mapping/occupancy/visualize_path.py | 85 +++++++++++++++++++ dimos/navigation/global_planner/planner.py | 4 +- 5 files changed, 133 insertions(+), 3 deletions(-) create mode 100644 data/.lfs/resample_path_simple.png.tar.gz create mode 100644 dimos/mapping/occupancy/test_path_resampling.py create mode 100644 dimos/mapping/occupancy/visualize_path.py 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/dimos/mapping/occupancy/path_resampling.py b/dimos/mapping/occupancy/path_resampling.py index 7777fa919e..7281141aa5 100644 --- a/dimos/mapping/occupancy/path_resampling.py +++ b/dimos/mapping/occupancy/path_resampling.py @@ -65,7 +65,7 @@ def _add_orientations_to_path(path: Path, goal_orientation: Quaternion) -> None: path.poses[-1].orientation = identity_quat -def resample_path(path: Path, goal_pose: Pose, spacing: float) -> Path: +def simple_resample_path(path: Path, goal_pose: Pose, spacing: float) -> Path: """Resample a path to have approximately uniform spacing between poses. Args: diff --git a/dimos/mapping/occupancy/test_path_resampling.py b/dimos/mapping/occupancy/test_path_resampling.py new file mode 100644 index 0000000000..656b559258 --- /dev/null +++ b/dimos/mapping/occupancy/test_path_resampling.py @@ -0,0 +1,42 @@ +# 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_resampling import simple_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 OccupancyGrid(np.load(get_data("occupancy_simple.npy"))).gradient(max_distance=1.5) + + +def test_resample_path(costmap) -> 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("resample_path_simple.png")) + path = astar("min_cost", costmap, goal_pose.position, start) + + resampled = simple_resample_path(path, goal_pose, 0.1) + + actual = visualize_path(costmap, resampled, 0.2, 0.4) + np.testing.assert_array_equal(actual.data, expected.data) diff --git a/dimos/mapping/occupancy/visualize_path.py b/dimos/mapping/occupancy/visualize_path.py new file mode 100644 index 0000000000..6698e7e35f --- /dev/null +++ b/dimos/mapping/occupancy/visualize_path.py @@ -0,0 +1,85 @@ +# 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 +) -> Image: + image = visualize_occupancy_grid(occupancy_grid, "rainbow") + bgr = image.data + + scale = 8 + 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=1) + + return Image( + data=bgr, + format=ImageFormat.BGR, + frame_id=occupancy_grid.frame_id, + ts=occupancy_grid.ts, + ) diff --git a/dimos/navigation/global_planner/planner.py b/dimos/navigation/global_planner/planner.py index e8a7c184f6..7332b8508f 100644 --- a/dimos/navigation/global_planner/planner.py +++ b/dimos/navigation/global_planner/planner.py @@ -18,7 +18,7 @@ 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 resample_path +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.astar import astar @@ -99,7 +99,7 @@ def plan(self, goal_pose: Pose) -> Path | None: logger.warning("No path found to the goal.") return None - return resample_path(path, goal_pose, 0.1) + return simple_resample_path(path, goal_pose, 0.1) astar_planner = AstarPlanner.blueprint From bb6a809dbf754fd8300464081c479cb62c2779b4 Mon Sep 17 00:00:00 2001 From: Paul Nechifor Date: Wed, 10 Dec 2025 09:57:45 +0200 Subject: [PATCH 07/42] smooth_path --- data/.lfs/resample_path_smooth.png.tar.gz | 3 + dimos/mapping/occupancy/path_resampling.py | 108 ++++++++++++++++++ .../mapping/occupancy/test_path_resampling.py | 15 ++- 3 files changed, 122 insertions(+), 4 deletions(-) create mode 100644 data/.lfs/resample_path_smooth.png.tar.gz 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/dimos/mapping/occupancy/path_resampling.py b/dimos/mapping/occupancy/path_resampling.py index 7281141aa5..00574b6650 100644 --- a/dimos/mapping/occupancy/path_resampling.py +++ b/dimos/mapping/occupancy/path_resampling.py @@ -15,6 +15,9 @@ 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 @@ -134,3 +137,108 @@ def simple_resample_path(path: Path, goal_pose: Pose, spacing: float) -> Path: _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_path_resampling.py b/dimos/mapping/occupancy/test_path_resampling.py index 656b559258..8b0ef23146 100644 --- a/dimos/mapping/occupancy/test_path_resampling.py +++ b/dimos/mapping/occupancy/test_path_resampling.py @@ -15,7 +15,7 @@ import numpy as np import pytest -from dimos.mapping.occupancy.path_resampling import simple_resample_path +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 @@ -30,13 +30,20 @@ def costmap() -> OccupancyGrid: return OccupancyGrid(np.load(get_data("occupancy_simple.npy"))).gradient(max_distance=1.5) -def test_resample_path(costmap) -> None: +@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("resample_path_simple.png")) + expected = Image.from_file(get_data(f"resample_path_{method}.png")) path = astar("min_cost", costmap, goal_pose.position, start) - resampled = simple_resample_path(path, goal_pose, 0.1) + 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) From 9814f63e76e2d7cac903c93594894881545da36a Mon Sep 17 00:00:00 2001 From: Paul Nechifor Date: Thu, 11 Dec 2025 03:56:35 +0200 Subject: [PATCH 08/42] many changes --- .gitignore | 2 + data/.lfs/expected_occupancy_scene.xml.tar.gz | 3 + data/.lfs/make_path_mask_full.png.tar.gz | 3 + .../.lfs/make_path_mask_two_meters.png.tar.gz | 3 + data/.lfs/three_paths.ply.tar.gz | 3 + data/.lfs/three_paths.png.tar.gz | 3 + dimos/core/global_config.py | 23 ++ dimos/core/transport.py | 4 +- dimos/mapping/occupancy/extrude_occupancy.py | 235 ++++++++++++++++ dimos/mapping/occupancy/path_mask.py | 98 +++++++ dimos/mapping/occupancy/path_resampling.py | 1 + .../occupancy/test_extrude_occupancy.py | 25 ++ dimos/mapping/occupancy/test_path_mask.py | 49 ++++ .../mapping/occupancy/test_path_resampling.py | 2 +- .../pointclouds/accumulators/general.py | 9 +- dimos/msgs/nav_msgs/OccupancyGrid.py | 23 +- .../navigation/bt_navigator/goal_validator.py | 76 +++++ .../bt_navigator/test_goal_validator.py | 53 ++++ dimos/navigation/global_planner/astar.py | 4 +- .../global_planner/min_cost_astar.py | 75 +++-- .../global_planner/min_cost_astar_cpp.cpp | 265 ++++++++++++++++++ .../global_planner/min_cost_astar_ext.pyi | 26 ++ dimos/navigation/global_planner/test_astar.py | 34 ++- dimos/navigation/local_planner/__init__.py | 1 + .../navigation/local_planner/local_planner.py | 14 +- dimos/navigation/local_planner/simple.py | 246 ++++++++++++++++ .../replanning_a_star/global_planner.py | 130 +++++++++ .../replanning_a_star/local_planner.py | 259 +++++++++++++++++ dimos/navigation/replanning_a_star/module.py | 89 ++++++ dimos/robot/all_blueprints.py | 2 + dimos/robot/cli/dimos.py | 21 ++ dimos/robot/cli/topic.py | 102 +++++++ .../robot/unitree_webrtc/mujoco_connection.py | 2 +- dimos/robot/unitree_webrtc/type/map.py | 72 +++-- .../unitree_webrtc/unitree_go2_blueprints.py | 24 ++ dimos/simulation/mujoco/constants.py | 2 +- dimos/simulation/mujoco/model.py | 28 +- dimos/simulation/mujoco/mujoco_process.py | 18 +- pyproject.toml | 2 +- setup.py | 21 ++ 40 files changed, 1962 insertions(+), 90 deletions(-) create mode 100644 data/.lfs/expected_occupancy_scene.xml.tar.gz create mode 100644 data/.lfs/make_path_mask_full.png.tar.gz create mode 100644 data/.lfs/make_path_mask_two_meters.png.tar.gz create mode 100644 data/.lfs/three_paths.ply.tar.gz create mode 100644 data/.lfs/three_paths.png.tar.gz create mode 100644 dimos/mapping/occupancy/extrude_occupancy.py create mode 100644 dimos/mapping/occupancy/path_mask.py create mode 100644 dimos/mapping/occupancy/test_extrude_occupancy.py create mode 100644 dimos/mapping/occupancy/test_path_mask.py create mode 100644 dimos/navigation/bt_navigator/test_goal_validator.py create mode 100644 dimos/navigation/global_planner/min_cost_astar_cpp.cpp create mode 100644 dimos/navigation/global_planner/min_cost_astar_ext.pyi create mode 100644 dimos/navigation/local_planner/simple.py create mode 100644 dimos/navigation/replanning_a_star/global_planner.py create mode 100644 dimos/navigation/replanning_a_star/local_planner.py create mode 100644 dimos/navigation/replanning_a_star/module.py create mode 100644 dimos/robot/cli/topic.py diff --git a/.gitignore b/.gitignore index ec3d1925d5..d943d94034 100644 --- a/.gitignore +++ b/.gitignore @@ -51,3 +51,5 @@ yolo11n.pt .claude /logs + +*.so 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..ad51dc8b4b --- /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:345a15c6ea70244bacc65c6490d39346378eba68c55a09ebdf9cf1ad53666f97 +size 6740 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..f4093a2080 --- /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:dbfedcccd0f284bc66156d30d664d392456fd03d0438a5fa4e05c23af1eb698c +size 11231 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..3ac1f4bbf4 --- /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:24f618bb9f07685d9f21663ace62dec70d9b79e7ea1ef61f07a30e3475f0d0a6 +size 11408 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/dimos/core/global_config.py b/dimos/core/global_config.py index bd2246f562..cd81ea1d67 100644 --- a/dimos/core/global_config.py +++ b/dimos/core/global_config.py @@ -13,6 +13,7 @@ # limitations under the License. from functools import cached_property +import re from pydantic_settings import BaseSettings, SettingsConfigDict @@ -20,16 +21,27 @@ 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 simulation: bool = False replay: bool = False n_dask_workers: int = 2 + 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.4 + robot_rotation_diameter: float = 0.6 planner_strategy: NavigationStrategy = "mixed" astar_algorithm: AStarAlgorithm = "min_cost" + planner_robot_speed: float | None = None model_config = SettingsConfigDict( env_file=".env", @@ -45,3 +57,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/extrude_occupancy.py b/dimos/mapping/occupancy/extrude_occupancy.py new file mode 100644 index 0000000000..358e667085 --- /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("") + + xml_content = "\n".join(xml_lines) + + return xml_content diff --git a/dimos/mapping/occupancy/path_mask.py b/dimos/mapping/occupancy/path_mask.py new file mode 100644 index 0000000000..48c9b8ed2d --- /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 Exception( + 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 index 00574b6650..0ae6a1a089 100644 --- a/dimos/mapping/occupancy/path_resampling.py +++ b/dimos/mapping/occupancy/path_resampling.py @@ -68,6 +68,7 @@ def _add_orientations_to_path(path: Path, goal_orientation: Quaternion) -> None: 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. 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_path_mask.py b/dimos/mapping/occupancy/test_path_mask.py new file mode 100644 index 0000000000..bf5cc7d3ac --- /dev/null +++ b/dimos/mapping/occupancy/test_path_mask.py @@ -0,0 +1,49 @@ +# 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) + 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] + actual.save(f"data/{expected_image}") + + 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 index 8b0ef23146..c80c1c434b 100644 --- a/dimos/mapping/occupancy/test_path_resampling.py +++ b/dimos/mapping/occupancy/test_path_resampling.py @@ -35,7 +35,7 @@ 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) + path = astar("min_cost", costmap, goal_pose.position, start, use_cpp=False) match method: case "simple": diff --git a/dimos/mapping/pointclouds/accumulators/general.py b/dimos/mapping/pointclouds/accumulators/general.py index 20fb0c0657..21fe1f7b08 100644 --- a/dimos/mapping/pointclouds/accumulators/general.py +++ b/dimos/mapping/pointclouds/accumulators/general.py @@ -14,16 +14,23 @@ 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) -> None: + 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 diff --git a/dimos/msgs/nav_msgs/OccupancyGrid.py b/dimos/msgs/nav_msgs/OccupancyGrid.py index af8baa7df4..6c45bd3ec2 100644 --- a/dimos/msgs/nav_msgs/OccupancyGrid.py +++ b/dimos/msgs/nav_msgs/OccupancyGrid.py @@ -16,7 +16,7 @@ from enum import IntEnum import time -from typing import BinaryIO +from typing import TYPE_CHECKING, BinaryIO from dimos_lcm.nav_msgs import ( # type: ignore[import-untyped] MapMetaData, @@ -24,11 +24,17 @@ ) from dimos_lcm.std_msgs import Time as LCMTime # type: ignore[import-untyped] import numpy as np +from PIL import Image from scipy import ndimage # type: ignore[import-untyped] from dimos.msgs.geometry_msgs import Pose, Vector3, VectorLike from dimos.types.timestamped import Timestamped +if TYPE_CHECKING: + from pathlib import Path + + from numpy.typing import NDArray + class CostValues(IntEnum): """Standard cost values for occupancy grid cells. @@ -56,11 +62,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, @@ -172,6 +178,17 @@ 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 + @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. diff --git a/dimos/navigation/bt_navigator/goal_validator.py b/dimos/navigation/bt_navigator/goal_validator.py index cdf86ccac5..5db76aca1f 100644 --- a/dimos/navigation/bt_navigator/goal_validator.py +++ b/dimos/navigation/bt_navigator/goal_validator.py @@ -54,6 +54,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 +153,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, 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/global_planner/astar.py b/dimos/navigation/global_planner/astar.py index 54711a9107..c9159e40cd 100644 --- a/dimos/navigation/global_planner/astar.py +++ b/dimos/navigation/global_planner/astar.py @@ -28,6 +28,7 @@ def astar( costmap: OccupancyGrid, goal: VectorLike, start: VectorLike, + use_cpp: bool = True, ) -> Path | None: """ A* path planning algorithm from start to goal position. @@ -37,6 +38,7 @@ def astar( 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 @@ -46,6 +48,6 @@ def astar( case "general": return general_astar(costmap, goal, start) case "min_cost": - return min_cost_astar(costmap, goal, start) + return min_cost_astar(costmap, goal, start, use_cpp=use_cpp) case _: raise NotImplementedError() diff --git a/dimos/navigation/global_planner/min_cost_astar.py b/dimos/navigation/global_planner/min_cost_astar.py index 8174dd9cbe..c05a4286bb 100644 --- a/dimos/navigation/global_planner/min_cost_astar.py +++ b/dimos/navigation/global_planner/min_cost_astar.py @@ -17,6 +17,16 @@ from dimos.msgs.geometry_msgs import PoseStamped, Quaternion, VectorLike from dimos.msgs.nav_msgs import CostValues, OccupancyGrid, Path +# 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 + # Define possible movements (8-connected grid with diagonal movements) _directions = [ (0, 1), @@ -50,11 +60,9 @@ def _reconstruct_path( start_tuple: tuple[int, int], goal_tuple: tuple[int, int], ) -> Path: - # Reconstruct the path waypoints: list[PoseStamped] = [] while current in parents: world_point = costmap.grid_to_world(current) - # Create PoseStamped with identity quaternion (no orientation) pose = PoseStamped( frame_id="world", position=[world_point.x, world_point.y, 0.0], @@ -63,7 +71,6 @@ def _reconstruct_path( waypoints.append(pose) current = parents[current] - # Add the start position start_world_point = costmap.grid_to_world(start_tuple) start_pose = PoseStamped( frame_id="world", @@ -72,7 +79,6 @@ def _reconstruct_path( ) waypoints.append(start_pose) - # Reverse the path (start to goal) waypoints.reverse() # Add the goal position if it's not already included @@ -92,26 +98,55 @@ def _reconstruct_path( 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: - # Convert world coordinates to grid coordinates directly using vector-like inputs start_vector = costmap.world_to_grid(start) goal_vector = costmap.world_to_grid(goal) - # Store positions as tuples for dictionary keys start_tuple = (int(start_vector.x), int(start_vector.y)) goal_tuple = (int(goal_vector.x), int(goal_vector.y)) - # Check if goal is out of bounds if not (0 <= goal_tuple[0] < costmap.width and 0 <= goal_tuple[1] < costmap.height): return None - # A* algorithm implementation + if use_cpp and _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) + 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 @@ -130,58 +165,43 @@ def min_cost_astar( open_set_hash: set[tuple[int, int]] = {start_tuple} while open_set: - # Get the node with the lowest priority (cost first, then distance) - _current_cost, _current_dist, current = heapq.heappop(open_set) + _, _, current = heapq.heappop(open_set) current_x, current_y = current - # Remove from open set hash if current in open_set_hash: open_set_hash.remove(current) - # Skip if already processed (can happen with duplicate entries) if current in closed_set: continue - # Check if we've reached the goal if current == goal_tuple: return _reconstruct_path(parents, current, costmap, start_tuple, goal_tuple) - # Add current node to closed set closed_set.add(current) - # Explore neighbors for i, (dx, dy) in enumerate(_directions): neighbor_x, neighbor_y = current_x + dx, current_y + dy neighbor = (neighbor_x, neighbor_y) - # Check if the neighbor is valid if not (0 <= neighbor_x < costmap.width and 0 <= neighbor_y < costmap.height): continue - # Check if the neighbor is already explored if neighbor in closed_set: continue - # Get the neighbor's cost value neighbor_val = costmap.grid[neighbor_y, neighbor_x] - # Skip if it's a hard obstacle if neighbor_val >= cost_threshold: continue - # Calculate movement cost with penalties - # Unknown cells get half the penalty of obstacles - if neighbor_val == CostValues.UNKNOWN: # Unknown cell (-1) - # Unknown cells have a moderate traversal cost (half of obstacle threshold) + if neighbor_val == CostValues.UNKNOWN: + # Unknown cells have a moderate traversal cost cell_cost = cost_threshold * unknown_penalty - elif neighbor_val == CostValues.FREE: # Free space (0) - # Free cells have minimal cost + elif neighbor_val == CostValues.FREE: cell_cost = 0.0 else: - # Other cells use their actual cost value (1-99) cell_cost = neighbor_val - # Calculate tentative scores: accumulate cost and distance separately tentative_cost = cost_score[current] + cell_cost tentative_dist = dist_score[current] + _movement_costs[i] @@ -202,7 +222,6 @@ def min_cost_astar( priority_dist = tentative_dist + h_dist # Add the neighbor to the open set with its priority - # Only add if not already in open set to reduce duplicates if neighbor not in open_set_hash: heapq.heappush(open_set, (priority_cost, priority_dist, neighbor)) open_set_hash.add(neighbor) 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/test_astar.py b/dimos/navigation/global_planner/test_astar.py index 37732d640c..18838bfaed 100644 --- a/dimos/navigation/global_planner/test_astar.py +++ b/dimos/navigation/global_planner/test_astar.py @@ -12,6 +12,8 @@ # 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 @@ -41,7 +43,37 @@ def test_astar(costmap, mode, expected_image) -> None: goal = Vector3(6.15, 10.0) expected = Image.from_file(get_data(expected_image)) - path = astar(mode, costmap, goal, start) + 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) + + +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 times_better > 5, "there's an issue C++ if less than 5x faster" + + # 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/local_planner/__init__.py b/dimos/navigation/local_planner/__init__.py index 9e0f62931a..bebcc17e2d 100644 --- a/dimos/navigation/local_planner/__init__.py +++ b/dimos/navigation/local_planner/__init__.py @@ -1,2 +1,3 @@ from dimos.navigation.local_planner.holonomic_local_planner import HolonomicLocalPlanner from dimos.navigation.local_planner.local_planner import BaseLocalPlanner +from dimos.navigation.local_planner.simple import SimplePlanner diff --git a/dimos/navigation/local_planner/local_planner.py b/dimos/navigation/local_planner/local_planner.py index 660c1b5894..79f136b48e 100644 --- a/dimos/navigation/local_planner/local_planner.py +++ b/dimos/navigation/local_planner/local_planner.py @@ -92,14 +92,9 @@ 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.local_costmap.subscribe(self._on_costmap))) + 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: @@ -120,14 +115,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/local_planner/simple.py b/dimos/navigation/local_planner/simple.py new file mode 100644 index 0000000000..52a20cd048 --- /dev/null +++ b/dimos/navigation/local_planner/simple.py @@ -0,0 +1,246 @@ +# 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 enum import Enum, auto + +import numpy as np +from numpy.typing import NDArray +from reactivex.disposable import Disposable + +from dimos.core import In, Out, rpc +from dimos.core.global_config import GlobalConfig +from dimos.msgs.geometry_msgs import PoseStamped, Twist, Vector3 +from dimos.msgs.nav_msgs import CostValues, OccupancyGrid, Path +from dimos.navigation.bt_navigator.goal_validator import find_safe_goal +from dimos.navigation.local_planner.local_planner import BaseLocalPlanner +from dimos.utils.logging_config import setup_logger +from dimos.utils.transform_utils import normalize_angle, quaternion_to_euler + +logger = setup_logger() + + +class NavigationStage(Enum): + INITIAL_ROTATION = auto() + PATH_FOLLOWING = auto() + FINAL_ROTATION = auto() + + +class SimplePlanner(BaseLocalPlanner): + global_costmap: In[OccupancyGrid] = None # type: ignore[assignment] + goal_request: In[PoseStamped] = None # type: ignore[assignment] + target: Out[PoseStamped] = None # type: ignore[assignment] + + _global_config: GlobalConfig + _latest_costmap: OccupancyGrid | None = None + + def __init__( + self, + speed: float = 1.0, + k_angular: float = 2.0, + lookahead_dist: float = 0.5, + goal_tolerance: float = 0.3, + orientation_tolerance: float = 0.2, + control_frequency: float = 20.0, + global_config: GlobalConfig | None = None, + ) -> None: + super().__init__( + goal_tolerance=goal_tolerance, + orientation_tolerance=orientation_tolerance, + control_frequency=control_frequency, + ) + + self._global_config = global_config or GlobalConfig() + + self.speed = self._global_config.planner_robot_speed or speed + self.k_angular = k_angular + self.lookahead_dist = lookahead_dist + self._stage = NavigationStage.INITIAL_ROTATION + self._goal_reached = False + + @rpc + def start(self) -> None: + super().start() + self._disposables.add(Disposable(self.goal_request.subscribe(self._set_target))) + + def set_latest_costmap(msg: OccupancyGrid) -> None: + self._latest_costmap = msg + + self._disposables.add(Disposable(self.global_costmap.subscribe(set_latest_costmap))) + + @rpc + def stop(self) -> None: + super().stop() + + def _on_path(self, msg: Path) -> None: + self._stage = NavigationStage.INITIAL_ROTATION + self._goal_reached = False + super()._on_path(msg) + + def compute_velocity(self) -> Twist | None: + if self._goal_reached: + return None + + if self.latest_odom is None or self.latest_path is None: + return None + + if not len(self.latest_path.poses): + return None + + if self._stage == NavigationStage.INITIAL_ROTATION: + return self._compute_initial_rotation() + elif self._stage == NavigationStage.PATH_FOLLOWING: + return self._compute_path_following() + else: # FINAL_ROTATION + return self._compute_final_rotation() + + def _compute_initial_rotation(self) -> Twist: + assert self.latest_path is not None + assert self.latest_odom is not None + first_pose = self.latest_path.poses[0] + first_yaw = quaternion_to_euler(first_pose.orientation).z + robot_yaw = self.latest_odom.orientation.euler[2] + yaw_error = normalize_angle(first_yaw - robot_yaw) + + if abs(yaw_error) < self.orientation_tolerance: + self._stage = NavigationStage.PATH_FOLLOWING + return self._compute_path_following() + + max_angular_speed = self.speed + angular_velocity = np.clip( + self.k_angular * yaw_error, -max_angular_speed, max_angular_speed + ) + + return Twist( + linear=Vector3(0.0, 0.0, 0.0), + angular=Vector3(0.0, 0.0, angular_velocity), + ) + + def _compute_path_following(self) -> Twist: + assert self.latest_path is not None + assert self.latest_odom is not None + current_pose = np.array([self.latest_odom.position.x, self.latest_odom.position.y]) + path = np.array([[p.position.x, p.position.y] for p in self.latest_path.poses]) + + # Check if we've reached the final position + goal_position = path[-1] + distance_to_goal = np.linalg.norm(goal_position - current_pose) + + if distance_to_goal < self.goal_tolerance: + logger.info("Reached goal position, starting final rotation") + self._stage = NavigationStage.FINAL_ROTATION + return self._compute_final_rotation() + + closest_idx = self._find_closest_point_idx(current_pose, path) + lookahead_point = self._find_lookahead_point(current_pose, path, closest_idx) + + direction = lookahead_point - current_pose + distance = np.linalg.norm(direction) + + if distance < 1e-6: + return Twist() + + robot_yaw = self.latest_odom.orientation.euler[2] + desired_yaw = np.arctan2(direction[1], direction[0]) + yaw_error = normalize_angle(desired_yaw - robot_yaw) + + max_angular_speed = 0.5 + angular_velocity = np.clip( + self.k_angular * yaw_error, -max_angular_speed, max_angular_speed + ) + + # Linear velocity - reduce speed when turning + angular_slowdown = 1.0 - 0.7 * min(abs(angular_velocity) / max_angular_speed, 1.0) + linear_velocity = self.speed * angular_slowdown + + return Twist( + linear=Vector3(linear_velocity, 0.0, 0.0), + angular=Vector3(0.0, 0.0, angular_velocity), + ) + + def _compute_final_rotation(self) -> Twist: + assert self.latest_path is not None + assert self.latest_odom is not None + last_pose = self.latest_path.poses[-1] + goal_yaw = quaternion_to_euler(last_pose.orientation).z + robot_yaw = self.latest_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") + self._goal_reached = True + return Twist() + + max_angular_speed = self.speed + angular_velocity = np.clip( + self.k_angular * yaw_error, -max_angular_speed, max_angular_speed + ) + + return Twist( + linear=Vector3(0.0, 0.0, 0.0), + angular=Vector3(0.0, 0.0, angular_velocity), + ) + + def _find_closest_point_idx(self, pose: NDArray[np.float64], path: NDArray[np.float64]) -> int: + """Find the index of the closest point on the path.""" + distances = np.linalg.norm(path - pose, axis=1) + return int(np.argmin(distances)) + + def _find_lookahead_point( + self, pose: NDArray[np.float64], path: NDArray[np.float64], start_idx: int + ) -> NDArray[np.float64]: + """Find a lookahead point on the path at the specified distance.""" + accumulated_dist: float = 0.0 + + for i in range(start_idx, len(path) - 1): + segment_dist = float(np.linalg.norm(path[i + 1] - path[i])) + + if accumulated_dist + segment_dist >= self.lookahead_dist: + remaining_dist = self.lookahead_dist - accumulated_dist + if segment_dist > 0: + t = remaining_dist / segment_dist + result: NDArray[np.float64] = path[i] + t * (path[i + 1] - path[i]) + return result + result_i: NDArray[np.float64] = path[i] + return result_i + + accumulated_dist += segment_dist + + # Return the last point if we've traversed the whole path + result_last: NDArray[np.float64] = path[-1] + return result_last + + def _set_target(self, pose_stamped: PoseStamped) -> None: + if self._latest_costmap is None: + logger.warning("Cannot set target: missing costmap") + return + + safe_goal = find_safe_goal( + self._latest_costmap, + pose_stamped.position, + algorithm="bfs_contiguous", + cost_threshold=CostValues.OCCUPIED, + min_clearance=self._global_config.robot_rotation_diameter / 2, + max_search_distance=5.0, + ) + + if safe_goal is None: + logger.warning("No safe goal found near requested target") + return + logger.info("Set safe goal", x=safe_goal.x, y=safe_goal.y) + self.target.publish(pose_stamped) + + +simple_planner = SimplePlanner.blueprint + +__all__ = ["SimplePlanner", "simple_planner"] 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..ecf4557271 --- /dev/null +++ b/dimos/navigation/replanning_a_star/global_planner.py @@ -0,0 +1,130 @@ +# 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 reactivex import Subject + +from dimos.core.global_config import GlobalConfig +from dimos.core.resource import Resource +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 Twist +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.nav_msgs.OccupancyGrid import CostValues, OccupancyGrid +from dimos.msgs.nav_msgs.Path import Path +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 +from dimos.utils.logging_config import setup_logger + +logger = setup_logger() + + +class GlobalPlanner(Resource): + path: Subject[Path] + + _current_odom: PoseStamped | None = None + _current_global_costmap: OccupancyGrid | None = None + _local_planner: LocalPlanner + _global_config: GlobalConfig + _last_global_costmap_used: OccupancyGrid | None = None + _current_navigation_map: OccupancyGrid | None = None + + def __init__(self, global_config: GlobalConfig) -> None: + self.path = Subject() + self._local_planner = LocalPlanner() + self._global_config = global_config + + def start(self) -> None: + self._local_planner.start() + + def stop(self) -> None: + self.cancel_goal() + self._local_planner.stop() + + def handle_odom(self, msg: PoseStamped) -> None: + self._current_odom = msg + self._local_planner.handle_odom(msg) + + def handle_global_costmap(self, msg: OccupancyGrid) -> None: + self._current_global_costmap = msg + + def handle_goal_request(self, goal: PoseStamped) -> None: + self.cancel_goal() + + if self._current_global_costmap is None: + logger.warning("Cannot handle goal request: missing costmap") + return + + if self._current_odom is None: + logger.warning("Cannot handle goal request: missing odometry") + return + + safe_goal = find_safe_goal( + self._current_global_costmap, + goal.position, + algorithm="bfs_contiguous", + cost_threshold=CostValues.OCCUPIED, + min_clearance=self._global_config.robot_rotation_diameter / 2, + max_search_distance=5.0, + ) + + if safe_goal is None: + logger.warning("No safe goal found near requested target") + return + + logger.info("Found safe goal", x=safe_goal.x, y=safe_goal.y) + + costmap = self._get_latest_navigation_map(self._current_global_costmap) + robot_pos = self._current_odom.position + path = astar(self._global_config.astar_algorithm, costmap, safe_goal, robot_pos) + + if not path: + logger.warning("No path found to the goal.", x=safe_goal.x, y=safe_goal.y) + return + + resampled_path = simple_resample_path(path, goal, 0.1) + + self.path.on_next(resampled_path) + + self._local_planner.start_planning(resampled_path) + + def _get_latest_navigation_map(self, global_costmap: OccupancyGrid) -> OccupancyGrid: + # "is", not "==", to check for reference equality + if global_costmap is self._last_global_costmap_used: + assert self._current_navigation_map is not None + return self._current_navigation_map + + self._current_navigation_map = make_navigation_map( + global_costmap, + self._global_config.robot_width, + strategy=self._global_config.planner_strategy, + ) + self._last_global_costmap_used = global_costmap + + return self._current_navigation_map + + def cancel_goal(self) -> None: + self.path.on_next(Path()) + self._local_planner.stop_planning() + + def get_state(self) -> NavigationState: + return self._local_planner.get_state() + + def is_goal_reached(self) -> bool: + return self._local_planner.is_goal_reached() + + @property + def cmd_vel(self) -> Subject[Twist]: + return self._local_planner.cmd_vel 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..a5334fb9a3 --- /dev/null +++ b/dimos/navigation/replanning_a_star/local_planner.py @@ -0,0 +1,259 @@ +# 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 +import time +from typing import Literal, TypeAlias + +import numpy as np +from numpy.typing import NDArray +from reactivex import Subject + +from dimos.core.resource import Resource +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.navigation.base import NavigationState +from dimos.utils.logging_config import setup_logger +from dimos.utils.transform_utils import normalize_angle, quaternion_to_euler + +logger = setup_logger() + +PlannerState: TypeAlias = Literal["idle", "initial_rotation", "path_following", "final_rotation"] + + +class LocalPlanner(Resource): + cmd_vel: Subject[Twist] + + _thread: Thread | None = None + _path: Path | None = None + _np_path: NDArray[np.float64] | None = None + _current_odom: PoseStamped | None = None + _goal_reached: bool = False + _lock: RLock + _stop_planning_event: Event + _state: PlannerState + + speed: float = 1.0 + k_angular: float = 2.0 + lookahead_dist: float = 0.5 + goal_tolerance: float = 0.3 + orientation_tolerance: float = 0.2 + control_frequency: float = 20.0 + + def __init__(self) -> None: + self.cmd_vel = Subject() + self._lock = RLock() + self._stop_planning_event = Event() + self._state = "idle" + + def start(self) -> None: + pass + + def stop(self) -> None: + self.stop_planning() + + def handle_odom(self, msg: PoseStamped) -> None: + self._current_odom = msg + + def start_planning(self, path: Path) -> None: + self.stop_planning() + + self._stop_planning_event = Event() + + with self._lock: + self._goal_reached = False + self._path = path + self._np_path = np.array([[p.position.x, p.position.y] for p in self._path.poses]) + self._thread = Thread(target=self._loop, daemon=True) + self._thread.start() + + def stop_planning(self) -> None: + self._stop_planning_event.set() + + with self._lock: + if self._thread is not None: + self._thread.join(2) + self._thread = None + + def _loop(self) -> None: + stop_event = self._stop_planning_event + + with self._lock: + path = self._path + self._state = "initial_rotation" + + if path is None: + raise RuntimeError("No path set for local planner.") + + while not stop_event.is_set(): + start_time = time.perf_counter() + + 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 == "idle": + with self._lock: + self._goal_reached = True + break + + 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) + + self.cmd_vel.on_next(Twist()) + + with self._lock: + self._state = "idle" + + def _compute_initial_rotation(self) -> Twist: + assert self._path is not None + assert self._current_odom is not None + first_pose = self._path.poses[0] + first_yaw = quaternion_to_euler(first_pose.orientation).z + robot_yaw = self._current_odom.orientation.euler[2] + yaw_error = normalize_angle(first_yaw - robot_yaw) + + if abs(yaw_error) < self.orientation_tolerance: + with self._lock: + self._state = "path_following" + return self._compute_path_following() + + max_angular_speed = self.speed + angular_velocity = np.clip( + self.k_angular * yaw_error, -max_angular_speed, max_angular_speed + ) + + return Twist( + linear=Vector3(0.0, 0.0, 0.0), + angular=Vector3(0.0, 0.0, angular_velocity), + ) + + def _compute_path_following(self) -> Twist: + assert self._np_path is not None + assert self._current_odom is not None + current_pose = np.array([self._current_odom.position.x, self._current_odom.position.y]) + + # Check if we've reached the final position + goal_position = self._np_path[-1] + distance_to_goal = np.linalg.norm(goal_position - current_pose) + + if distance_to_goal < self.goal_tolerance: + logger.info("Reached goal position, starting final rotation") + self._state = "final_rotation" + return self._compute_final_rotation() + + closest_idx = self._find_closest_point_idx(current_pose, self._np_path) + lookahead_point = self._find_lookahead_point(self._np_path, closest_idx) + + direction = lookahead_point - current_pose + distance = np.linalg.norm(direction) + + if distance < 1e-6: + return Twist() + + robot_yaw = self._current_odom.orientation.euler[2] + desired_yaw = np.arctan2(direction[1], direction[0]) + yaw_error = normalize_angle(desired_yaw - robot_yaw) + + max_angular_speed = 0.5 + angular_velocity = np.clip( + self.k_angular * yaw_error, -max_angular_speed, max_angular_speed + ) + + # Linear velocity - reduce speed when turning + angular_slowdown = 1.0 - 0.7 * min(abs(angular_velocity) / max_angular_speed, 1.0) + linear_velocity = self.speed * angular_slowdown + + return Twist( + linear=Vector3(linear_velocity, 0.0, 0.0), + angular=Vector3(0.0, 0.0, angular_velocity), + ) + + def _compute_final_rotation(self) -> Twist: + assert self._path is not None + assert self._current_odom is not None + last_pose = self._path.poses[-1] + goal_yaw = quaternion_to_euler(last_pose.orientation).z + robot_yaw = self._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._state = "idle" + return Twist() + + max_angular_speed = self.speed + angular_velocity = np.clip( + self.k_angular * yaw_error, -max_angular_speed, max_angular_speed + ) + + return Twist( + linear=Vector3(0.0, 0.0, 0.0), + angular=Vector3(0.0, 0.0, angular_velocity), + ) + + def _find_closest_point_idx(self, pose: NDArray[np.float64], path: NDArray[np.float64]) -> int: + """Find the index of the closest point on the path.""" + distances = np.linalg.norm(path - pose, axis=1) + return int(np.argmin(distances)) + + def _find_lookahead_point( + self, path: NDArray[np.float64], start_idx: int + ) -> NDArray[np.float64]: + """Find a lookahead point on the path at the specified distance.""" + accumulated_dist: float = 0.0 + + for i in range(start_idx, len(path) - 1): + segment_dist = float(np.linalg.norm(path[i + 1] - path[i])) + + if accumulated_dist + segment_dist >= self.lookahead_dist: + remaining_dist = self.lookahead_dist - accumulated_dist + if segment_dist > 0: + t = remaining_dist / segment_dist + result: NDArray[np.float64] = path[i] + t * (path[i + 1] - path[i]) + return result + result_i: NDArray[np.float64] = path[i] + return result_i + + accumulated_dist += segment_dist + + # Return the last point if we've traversed the whole path + result_last: NDArray[np.float64] = path[-1] + return result_last + + def get_state(self) -> NavigationState: + with self._lock: + state = self._state + + match state: + case "idle": + return NavigationState.IDLE + case "initial_rotation" | "path_following" | "final_rotation": + return NavigationState.FOLLOWING_PATH + case _: + raise ValueError(f"Unknown planner state: {state}") + + def is_goal_reached(self) -> bool: + with self._lock: + return self._goal_reached diff --git a/dimos/navigation/replanning_a_star/module.py b/dimos/navigation/replanning_a_star/module.py new file mode 100644 index 0000000000..034e6ac6cf --- /dev/null +++ b/dimos/navigation/replanning_a_star/module.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 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.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: Out[PoseStamped] + goal_reached: Out[Bool] + navigation_state: Out[String] + cmd_vel: Out[Twist] + path: Out[Path] + + _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)) + + self._disposables.add(self._planner.path.subscribe(self.path.publish)) + + self._disposables.add(self._planner.cmd_vel.subscribe(self.cmd_vel.publish)) + + @rpc + def stop(self) -> None: + self.cancel_goal() + + 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/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 ee7e25b060..1d72d76131 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/unitree_webrtc/mujoco_connection.py b/dimos/robot/unitree_webrtc/mujoco_connection.py index a214182925..f4c585e2d4 100644 --- a/dimos/robot/unitree_webrtc/mujoco_connection.py +++ b/dimos/robot/unitree_webrtc/mujoco_connection.py @@ -89,7 +89,7 @@ def start(self) -> None: raise RuntimeError(f"Failed to start MuJoCo subprocess: {e}") from e # Wait for process to be ready - ready_timeout = 10 + ready_timeout = 60.0 start_time = time.time() assert self.process is not None while time.time() - start_time < ready_timeout: diff --git a/dimos/robot/unitree_webrtc/type/map.py b/dimos/robot/unitree_webrtc/type/map.py index a16b30e1a9..d66a78d8ce 100644 --- a/dimos/robot/unitree_webrtc/type/map.py +++ b/dimos/robot/unitree_webrtc/type/map.py @@ -12,7 +12,9 @@ # 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 open3d as o3d # type: ignore[import-untyped] from reactivex import interval @@ -36,10 +38,13 @@ class Map(Module): local_costmap: Out[OccupancyGrid] = None # type: ignore[assignment] _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, @@ -47,14 +52,17 @@ def __init__( # type: ignore[no-untyped-def] **kwargs, ) -> None: self.voxel_size = voxel_size + self.cost_resolution = cost_resolution self.global_publish_interval = global_publish_interval self.min_height = min_height self.max_height = max_height - self._point_cloud_accumulator = GeneralPointCloudAccumulator(self.voxel_size) + 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) @@ -62,26 +70,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 = general_occupancy( - self.to_lidar_message(), - resolution=self.voxel_size, - 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: @@ -101,21 +94,52 @@ def to_lidar_message(self) -> LidarMessage: ts=time.time(), ) + # Is this RPC? @rpc - def add_frame(self, frame: LidarMessage) -> "Map": # type: ignore[return] + def add_frame(self, frame: LidarMessage) -> None: self._point_cloud_accumulator.add(frame.pointcloud) + + # TODO: MOVE THIS + ################################################################################# + ################################################################################# + ################################################################################# + ################################################################################# + ################################################################################# + ################################################################################# + ################################################################################# + ################################################################################# local_costmap = general_occupancy( frame, - resolution=self.voxel_size, + resolution=self.cost_resolution, min_height=0.15, max_height=0.6, ).gradient(max_distance=0.25) + self.local_costmap.publish(local_costmap) @property def o3d_geometry(self) -> o3d.geometry.PointCloud: return self._point_cloud_accumulator.get_point_cloud() + 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, + ) + + # 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 + + self.global_costmap.publish(occupancygrid) + mapper = Map.blueprint diff --git a/dimos/robot/unitree_webrtc/unitree_go2_blueprints.py b/dimos/robot/unitree_webrtc/unitree_go2_blueprints.py index a34e72fb2c..764ef22b36 100644 --- a/dimos/robot/unitree_webrtc/unitree_go2_blueprints.py +++ b/dimos/robot/unitree_webrtc/unitree_go2_blueprints.py @@ -38,6 +38,9 @@ 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 @@ -77,6 +80,27 @@ utilization(), ).global_config(n_dask_workers=8) +test_new_nav = ( + autoconnect( + go2_connection(), + mapper(voxel_size=0.5, global_publish_interval=2.5), + replanning_a_star_planner(), + # astar_planner(), + # simple_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_shm = autoconnect( standard.transports( { 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/pyproject.toml b/pyproject.toml index 1b3e879ce0..a01ee38720 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}, ) From 8548c0b30ef9227ef5dd745d7a5c72d112090e40 Mon Sep 17 00:00:00 2001 From: Paul Nechifor Date: Mon, 15 Dec 2025 13:49:25 +0200 Subject: [PATCH 09/42] remove path when stopped --- dimos/navigation/replanning_a_star/global_planner.py | 10 ++++++++++ dimos/navigation/replanning_a_star/local_planner.py | 3 +++ dimos/navigation/replanning_a_star/module.py | 3 +++ 3 files changed, 16 insertions(+) diff --git a/dimos/navigation/replanning_a_star/global_planner.py b/dimos/navigation/replanning_a_star/global_planner.py index ecf4557271..1b5e92b83b 100644 --- a/dimos/navigation/replanning_a_star/global_planner.py +++ b/dimos/navigation/replanning_a_star/global_planner.py @@ -13,6 +13,7 @@ # limitations under the License. from reactivex import Subject +from reactivex.disposable import CompositeDisposable from dimos.core.global_config import GlobalConfig from dimos.core.resource import Resource @@ -40,18 +41,24 @@ class GlobalPlanner(Resource): _global_config: GlobalConfig _last_global_costmap_used: OccupancyGrid | None = None _current_navigation_map: OccupancyGrid | None = None + _disposables: CompositeDisposable def __init__(self, global_config: GlobalConfig) -> None: self.path = Subject() self._local_planner = LocalPlanner() self._global_config = global_config + self._disposables = CompositeDisposable() def start(self) -> None: self._local_planner.start() + self._disposables.add( + self._local_planner.stopped_navigating.subscribe(self._on_stopped_navigating) + ) def stop(self) -> None: self.cancel_goal() self._local_planner.stop() + self._disposables.dispose() def handle_odom(self, msg: PoseStamped) -> None: self._current_odom = msg @@ -125,6 +132,9 @@ def get_state(self) -> NavigationState: def is_goal_reached(self) -> bool: return self._local_planner.is_goal_reached() + def _on_stopped_navigating(self, _: None) -> None: + self.path.on_next(Path()) + @property def cmd_vel(self) -> Subject[Twist]: return self._local_planner.cmd_vel diff --git a/dimos/navigation/replanning_a_star/local_planner.py b/dimos/navigation/replanning_a_star/local_planner.py index a5334fb9a3..d79a715843 100644 --- a/dimos/navigation/replanning_a_star/local_planner.py +++ b/dimos/navigation/replanning_a_star/local_planner.py @@ -35,6 +35,7 @@ class LocalPlanner(Resource): cmd_vel: Subject[Twist] + stopped_navigating: Subject[None] _thread: Thread | None = None _path: Path | None = None @@ -54,6 +55,7 @@ class LocalPlanner(Resource): def __init__(self) -> None: self.cmd_vel = Subject() + self.stopped_navigating = Subject() self._lock = RLock() self._stop_planning_event = Event() self._state = "idle" @@ -112,6 +114,7 @@ def _loop(self) -> None: elif state == "idle": with self._lock: self._goal_reached = True + self.stopped_navigating.on_next(None) break self.cmd_vel.on_next(cmd_vel) diff --git a/dimos/navigation/replanning_a_star/module.py b/dimos/navigation/replanning_a_star/module.py index 034e6ac6cf..6dad40d697 100644 --- a/dimos/navigation/replanning_a_star/module.py +++ b/dimos/navigation/replanning_a_star/module.py @@ -59,9 +59,12 @@ def start(self) -> None: self._disposables.add(self._planner.cmd_vel.subscribe(self.cmd_vel.publish)) + self._planner.start() + @rpc def stop(self) -> None: self.cancel_goal() + self._planner.stop() super().stop() From 62ce4deca424adc37d48d90d63119df854df2ad7 Mon Sep 17 00:00:00 2001 From: Paul Nechifor Date: Mon, 15 Dec 2025 13:55:54 +0200 Subject: [PATCH 10/42] remove voronoi --- data/.lfs/inflation_voronoi.png.tar.gz | 3 -- dimos/mapping/occupancy/inflation.py | 62 ----------------------- dimos/mapping/occupancy/path_map.py | 6 +-- dimos/mapping/occupancy/test_inflation.py | 16 ++---- 4 files changed, 6 insertions(+), 81 deletions(-) delete mode 100644 data/.lfs/inflation_voronoi.png.tar.gz diff --git a/data/.lfs/inflation_voronoi.png.tar.gz b/data/.lfs/inflation_voronoi.png.tar.gz deleted file mode 100644 index 41c8857980..0000000000 --- a/data/.lfs/inflation_voronoi.png.tar.gz +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:6ca526be5908e6231a9915f2927f014778db6aca76e599dc22f413ba3595855e -size 3776 diff --git a/dimos/mapping/occupancy/inflation.py b/dimos/mapping/occupancy/inflation.py index f0279f9c57..fdade7de63 100644 --- a/dimos/mapping/occupancy/inflation.py +++ b/dimos/mapping/occupancy/inflation.py @@ -51,65 +51,3 @@ def simple_inflate(occupancy_grid: OccupancyGrid, radius: float) -> OccupancyGri frame_id=occupancy_grid.frame_id, ts=occupancy_grid.ts, ) - - -def voronoi_inflate(occupancy_grid: OccupancyGrid, radius: float) -> OccupancyGrid: - """Inflate obstacles by a given radius using Voronoi partitioning. - - Unlike simple_inflate, this function ensures that separate occupied zones - do not merge together during inflation. Each zone grows into free space - up to the specified radius, stopping at Voronoi boundaries to maintain - separation between distinct obstacles. - - Args: - occupancy_grid: Input occupancy grid - radius: Inflation radius in meters - Returns: - New OccupancyGrid with inflated obstacles (zones remain separate) - """ - # Convert radius to grid cells - cell_radius = radius / occupancy_grid.resolution - - grid_array = occupancy_grid.grid - occupied_mask = grid_array >= CostValues.OCCUPIED - - # Label connected occupied regions (8-connectivity). - structure_8conn = np.ones((3, 3), dtype=np.uint8) - labeled_zones, num_zones = ndimage.label(occupied_mask, structure=structure_8conn) - - if num_zones == 0: - return OccupancyGrid( - grid=grid_array.copy(), - resolution=occupancy_grid.resolution, - origin=occupancy_grid.origin, - frame_id=occupancy_grid.frame_id, - ts=occupancy_grid.ts, - ) - - # Compute distance from each free pixel to nearest occupied pixel - # and get indices of which occupied pixel is nearest. - distances, nearest_indices = ndimage.distance_transform_edt(~occupied_mask, return_indices=True) - - # Voronoi assignment: for each pixel, which zone is it closest to. - voronoi_labels = labeled_zones[nearest_indices[0], nearest_indices[1]] - - # Find Voronoi boundaries: pixels adjacent to a pixel with a different - # label. This creates a 1-pixel boundary between zones. - boundary = np.zeros_like(voronoi_labels, dtype=bool) - boundary[:-1, :] |= voronoi_labels[:-1, :] != voronoi_labels[1:, :] - boundary[1:, :] |= voronoi_labels[1:, :] != voronoi_labels[:-1, :] - boundary[:, :-1] |= voronoi_labels[:, :-1] != voronoi_labels[:, 1:] - boundary[:, 1:] |= voronoi_labels[:, 1:] != voronoi_labels[:, :-1] - - # Inflate: mark cells within radius that are not on Voronoi boundaries. - result_grid = grid_array.copy() - inflate_mask = (distances <= cell_radius) & (distances > 0) & ~boundary - result_grid[inflate_mask] = CostValues.OCCUPIED - - 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/path_map.py b/dimos/mapping/occupancy/path_map.py index d871f83e0b..08ec71bb4f 100644 --- a/dimos/mapping/occupancy/path_map.py +++ b/dimos/mapping/occupancy/path_map.py @@ -14,7 +14,7 @@ from typing import Literal, TypeAlias -from dimos.mapping.occupancy.inflation import simple_inflate, voronoi_inflate +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 @@ -30,11 +30,9 @@ def make_navigation_map( if strategy == "simple": costmap = simple_inflate(occupancy_grid, half_width) elif strategy == "mixed": - simple_inflate_ammount = 0.1 costmap = smooth_occupied(occupancy_grid) - costmap = simple_inflate(costmap, simple_inflate_ammount) + costmap = simple_inflate(costmap, half_width) costmap = overlay_occupied(costmap, occupancy_grid) - costmap = voronoi_inflate(costmap, half_width - simple_inflate_ammount) else: raise ValueError(f"Unknown strategy: {strategy}") diff --git a/dimos/mapping/occupancy/test_inflation.py b/dimos/mapping/occupancy/test_inflation.py index 0fb4317fc2..04763777b8 100644 --- a/dimos/mapping/occupancy/test_inflation.py +++ b/dimos/mapping/occupancy/test_inflation.py @@ -16,24 +16,16 @@ import cv2 import numpy as np -import pytest -from dimos.mapping.occupancy.inflation import simple_inflate, voronoi_inflate +from dimos.mapping.occupancy.inflation import simple_inflate from dimos.mapping.occupancy.visualizations import visualize_occupancy_grid from dimos.utils.data import get_data -@pytest.mark.parametrize("method", ["simple", "voronoi"]) -def test_inflation(occupancy, method) -> None: - expected = cv2.imread(get_data(f"inflation_{method}.png"), cv2.IMREAD_COLOR) +def test_inflation(occupancy) -> None: + expected = cv2.imread(get_data("inflation_simple.png"), cv2.IMREAD_COLOR) - match method: - case "simple": - og = simple_inflate(occupancy, 0.2) - case "voronoi": - og = voronoi_inflate(occupancy, 0.2) - case _: - raise ValueError(f"Unknown inflation method: {method}") + og = simple_inflate(occupancy, 0.2) result = visualize_occupancy_grid(og, "rainbow") np.testing.assert_array_equal(result.data, expected) From e6a1ad731d994c2cfa935ceefa836a97f57dba3f Mon Sep 17 00:00:00 2001 From: Paul Nechifor Date: Mon, 15 Dec 2025 14:59:21 +0200 Subject: [PATCH 11/42] add voronoi gradient --- dimos/core/global_config.py | 4 +- dimos/mapping/occupancy/gradient.py | 209 ++++++++++++++++++ dimos/mapping/occupancy/path_map.py | 3 +- .../mapping/occupancy/test_path_resampling.py | 3 +- dimos/mapping/pointclouds/demo.py | 3 +- dimos/msgs/nav_msgs/OccupancyGrid.py | 159 +------------ dimos/msgs/nav_msgs/test_OccupancyGrid.py | 7 +- .../navigation/bt_navigator/goal_validator.py | 5 +- dimos/navigation/bt_navigator/navigator.py | 3 +- dimos/navigation/global_planner/astar.py | 8 +- dimos/navigation/global_planner/test_astar.py | 3 +- dimos/robot/unitree_webrtc/type/map.py | 16 +- .../web/websocket_vis/websocket_vis_module.py | 3 +- 13 files changed, 248 insertions(+), 178 deletions(-) create mode 100644 dimos/mapping/occupancy/gradient.py diff --git a/dimos/core/global_config.py b/dimos/core/global_config.py index cd81ea1d67..d3dbe00449 100644 --- a/dimos/core/global_config.py +++ b/dimos/core/global_config.py @@ -37,9 +37,9 @@ class GlobalConfig(BaseSettings): 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.4 + robot_width: float = 0.3 robot_rotation_diameter: float = 0.6 - planner_strategy: NavigationStrategy = "mixed" + planner_strategy: NavigationStrategy = "simple" astar_algorithm: AStarAlgorithm = "min_cost" planner_robot_speed: float | None = None diff --git a/dimos/mapping/occupancy/gradient.py b/dimos/mapping/occupancy/gradient.py new file mode 100644 index 0000000000..b5752af0bf --- /dev/null +++ b/dimos/mapping/occupancy/gradient.py @@ -0,0 +1,209 @@ +# 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 + +from dimos.msgs.nav_msgs.OccupancyGrid import CostValues, OccupancyGrid + + +def gradient( + ocupancy_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 = ocupancy_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 = (ocupancy_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 * ocupancy_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=ocupancy_grid.resolution, + origin=ocupancy_grid.origin, + frame_id=ocupancy_grid.frame_id, + ts=ocupancy_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 + voronoi_edges = np.zeros_like(obstacle_map, dtype=bool) + + # Check 8-connectivity neighbors using roll for efficient comparison + for dy in [-1, 0, 1]: + for dx in [-1, 0, 1]: + if dy == 0 and dx == 0: + continue + # Shift and compare cluster labels + shifted = np.roll(np.roll(nearest_obstacle_cluster, dy, axis=0), dx, axis=1) + voronoi_edges |= nearest_obstacle_cluster != shifted + + # Remove edges at boundaries (roll wraps around, creating false edges) + voronoi_edges[0, :] = False + voronoi_edges[-1, :] = False + voronoi_edges[:, 0] = False + voronoi_edges[:, -1] = False + + # 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/path_map.py b/dimos/mapping/occupancy/path_map.py index 08ec71bb4f..aa0a5599f5 100644 --- a/dimos/mapping/occupancy/path_map.py +++ b/dimos/mapping/occupancy/path_map.py @@ -14,6 +14,7 @@ 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 @@ -36,4 +37,4 @@ def make_navigation_map( else: raise ValueError(f"Unknown strategy: {strategy}") - return costmap.gradient(max_distance=gradient_distance) + return voronoi_gradient(costmap, max_distance=gradient_distance) diff --git a/dimos/mapping/occupancy/test_path_resampling.py b/dimos/mapping/occupancy/test_path_resampling.py index c80c1c434b..8f3408ef33 100644 --- a/dimos/mapping/occupancy/test_path_resampling.py +++ b/dimos/mapping/occupancy/test_path_resampling.py @@ -15,6 +15,7 @@ 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 @@ -27,7 +28,7 @@ @pytest.fixture def costmap() -> OccupancyGrid: - return OccupancyGrid(np.load(get_data("occupancy_simple.npy"))).gradient(max_distance=1.5) + return gradient(OccupancyGrid(np.load(get_data("occupancy_simple.npy"))), max_distance=1.5) @pytest.mark.parametrize("method", ["simple", "smooth"]) diff --git a/dimos/mapping/pointclouds/demo.py b/dimos/mapping/pointclouds/demo.py index 5d3c8475b5..abf52edd13 100644 --- a/dimos/mapping/pointclouds/demo.py +++ b/dimos/mapping/pointclouds/demo.py @@ -17,6 +17,7 @@ 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 ( @@ -77,7 +78,7 @@ def view_map() -> None: @app.command() def view_map_inflated() -> None: - og = _get_occupancy_grid().gradient(max_distance=1.5) + og = gradient(_get_occupancy_grid(), max_distance=1.5) _show_occupancy_grid(og) diff --git a/dimos/msgs/nav_msgs/OccupancyGrid.py b/dimos/msgs/nav_msgs/OccupancyGrid.py index 6c45bd3ec2..39b81a1a77 100644 --- a/dimos/msgs/nav_msgs/OccupancyGrid.py +++ b/dimos/msgs/nav_msgs/OccupancyGrid.py @@ -309,161 +309,4 @@ def lcm_decode(cls, data: bytes | BinaryIO) -> OccupancyGrid: return instance 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. - - Args: - threshold: Keep cells with values > threshold - - Returns: - New OccupancyGrid where: - - Cells > threshold: kept as-is - - Cells <= threshold: set to -1 (unknown) - - Unknown cells (-1): preserved - """ - new_grid = self.grid.copy() - - # Create mask for cells to filter (not unknown and <= threshold) - filter_mask = (new_grid != -1) & (new_grid <= threshold) - - # Set filtered cells to unknown - new_grid[filter_mask] = -1 - - # Create new OccupancyGrid - filtered = OccupancyGrid( - new_grid, - resolution=self.resolution, - origin=self.origin, - frame_id=self.frame_id, - ts=self.ts, - ) - - return filtered - - def filter_below(self, threshold: int) -> OccupancyGrid: - """Create a new OccupancyGrid with only values below threshold. - - Args: - threshold: Keep cells with values < threshold - - Returns: - New OccupancyGrid where: - - Cells < threshold: kept as-is - - Cells >= threshold: set to -1 (unknown) - - Unknown cells (-1): preserved - """ - new_grid = self.grid.copy() - - # Create mask for cells to filter (not unknown and >= threshold) - filter_mask = (new_grid != -1) & (new_grid >= threshold) - - # Set filtered cells to unknown - new_grid[filter_mask] = -1 - - # Create new OccupancyGrid - filtered = OccupancyGrid( - new_grid, - resolution=self.resolution, - origin=self.origin, - frame_id=self.frame_id, - ts=self.ts, - ) - - return filtered - - def max(self) -> OccupancyGrid: - """Create a new OccupancyGrid with all non-unknown cells set to maximum value. - - Returns: - New OccupancyGrid where: - - All non-unknown cells: set to CostValues.OCCUPIED (100) - - Unknown cells: preserved as CostValues.UNKNOWN (-1) - """ - new_grid = self.grid.copy() - - # Set all non-unknown cells to max - new_grid[new_grid != CostValues.UNKNOWN] = CostValues.OCCUPIED - - # Create new OccupancyGrid - maxed = OccupancyGrid( - new_grid, - resolution=self.resolution, - origin=self.origin, - frame_id=self.frame_id, - ts=self.ts, - ) - - 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, - ) + pass diff --git a/dimos/msgs/nav_msgs/test_OccupancyGrid.py b/dimos/msgs/nav_msgs/test_OccupancyGrid.py index c6fe8a472e..247aa9cc7f 100644 --- a/dimos/msgs/nav_msgs/test_OccupancyGrid.py +++ b/dimos/msgs/nav_msgs/test_OccupancyGrid.py @@ -20,6 +20,7 @@ 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 @@ -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 @@ -380,7 +381,7 @@ def test_lcm_broadcast() -> None: 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 5db76aca1f..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 @@ -386,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 a722385164..9ca341b661 100644 --- a/dimos/navigation/bt_navigator/navigator.py +++ b/dimos/navigation/bt_navigator/navigator.py @@ -27,6 +27,7 @@ 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 @@ -286,7 +287,7 @@ def _control_loop(self) -> None: self.cancel_goal() continue - costmap = simple_inflate(self.latest_costmap, 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/global_planner/astar.py b/dimos/navigation/global_planner/astar.py index c9159e40cd..7079bd6595 100644 --- a/dimos/navigation/global_planner/astar.py +++ b/dimos/navigation/global_planner/astar.py @@ -12,6 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. +from dimos.mapping.occupancy.visualizations import visualize_occupancy_grid from dimos.msgs.geometry_msgs import VectorLike from dimos.msgs.nav_msgs import OccupancyGrid, Path from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid @@ -48,6 +49,11 @@ def astar( case "general": return general_astar(costmap, goal, start) case "min_cost": - return min_cost_astar(costmap, goal, start, use_cpp=use_cpp) + ret = min_cost_astar(costmap, goal, start, use_cpp=False) + import numpy as np + + np.save("center_test.npy", costmap.grid) + visualize_occupancy_grid(costmap, "rainbow", ret).save("astar_min_cost_path.png") + return ret case _: raise NotImplementedError() diff --git a/dimos/navigation/global_planner/test_astar.py b/dimos/navigation/global_planner/test_astar.py index 18838bfaed..19c253cdc0 100644 --- a/dimos/navigation/global_planner/test_astar.py +++ b/dimos/navigation/global_planner/test_astar.py @@ -18,6 +18,7 @@ from open3d.geometry import PointCloud # type: ignore[import-untyped] import pytest +from dimos.mapping.occupancy.gradient import 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 @@ -28,7 +29,7 @@ @pytest.fixture def costmap() -> PointCloud: - return OccupancyGrid(np.load(get_data("occupancy_simple.npy"))).gradient(max_distance=1.5) + return gradient(OccupancyGrid(np.load(get_data("occupancy_simple.npy"))), max_distance=1.5) @pytest.mark.parametrize( diff --git a/dimos/robot/unitree_webrtc/type/map.py b/dimos/robot/unitree_webrtc/type/map.py index d66a78d8ce..36faca394d 100644 --- a/dimos/robot/unitree_webrtc/type/map.py +++ b/dimos/robot/unitree_webrtc/type/map.py @@ -22,6 +22,7 @@ from dimos.core import DimosCluster, In, LCMTransport, Module, Out, rpc from dimos.core.global_config import GlobalConfig +from dimos.mapping.occupancy.gradient import gradient from dimos.mapping.pointclouds.accumulators.general import GeneralPointCloudAccumulator from dimos.mapping.pointclouds.accumulators.protocol import PointCloudAccumulator from dimos.mapping.pointclouds.occupancy import general_occupancy @@ -108,12 +109,15 @@ def add_frame(self, frame: LidarMessage) -> None: ################################################################################# ################################################################################# ################################################################################# - local_costmap = general_occupancy( - frame, - resolution=self.cost_resolution, - min_height=0.15, - max_height=0.6, - ).gradient(max_distance=0.25) + local_costmap = gradient( + general_occupancy( + frame, + resolution=self.cost_resolution, + min_height=0.15, + max_height=0.6, + ), + max_distance=0.25, + ) self.local_costmap.publish(local_costmap) diff --git a/dimos/web/websocket_vis/websocket_vis_module.py b/dimos/web/websocket_vis/websocket_vis_module.py index feac036b8c..6e0e8d6343 100644 --- a/dimos/web/websocket_vis/websocket_vis_module.py +++ b/dimos/web/websocket_vis/websocket_vis_module.py @@ -32,6 +32,7 @@ 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 @@ -279,7 +280,7 @@ def _on_global_costmap(self, msg: OccupancyGrid) -> None: def _process_costmap(self, costmap: OccupancyGrid) -> dict[str, Any]: """Convert OccupancyGrid to visualization format.""" - costmap = simple_inflate(costmap, 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 { From 93685bb5f8ccc3001aac8b8e24f1e4ab822e7e8e Mon Sep 17 00:00:00 2001 From: Paul Nechifor Date: Mon, 15 Dec 2025 15:12:13 +0200 Subject: [PATCH 12/42] remove simple planner --- dimos/msgs/nav_msgs/OccupancyGrid.py | 1 - dimos/navigation/local_planner/__init__.py | 3 +- dimos/navigation/local_planner/simple.py | 246 ------------------ .../unitree_webrtc/unitree_go2_blueprints.py | 2 - 4 files changed, 2 insertions(+), 250 deletions(-) delete mode 100644 dimos/navigation/local_planner/simple.py diff --git a/dimos/msgs/nav_msgs/OccupancyGrid.py b/dimos/msgs/nav_msgs/OccupancyGrid.py index 39b81a1a77..e2f8bbcdce 100644 --- a/dimos/msgs/nav_msgs/OccupancyGrid.py +++ b/dimos/msgs/nav_msgs/OccupancyGrid.py @@ -25,7 +25,6 @@ from dimos_lcm.std_msgs import Time as LCMTime # type: ignore[import-untyped] import numpy as np from PIL import Image -from scipy import ndimage # type: ignore[import-untyped] from dimos.msgs.geometry_msgs import Pose, Vector3, VectorLike from dimos.types.timestamped import Timestamped diff --git a/dimos/navigation/local_planner/__init__.py b/dimos/navigation/local_planner/__init__.py index bebcc17e2d..b08e97b1aa 100644 --- a/dimos/navigation/local_planner/__init__.py +++ b/dimos/navigation/local_planner/__init__.py @@ -1,3 +1,4 @@ from dimos.navigation.local_planner.holonomic_local_planner import HolonomicLocalPlanner from dimos.navigation.local_planner.local_planner import BaseLocalPlanner -from dimos.navigation.local_planner.simple import SimplePlanner + +__all__ = ["BaseLocalPlanner", "HolonomicLocalPlanner"] diff --git a/dimos/navigation/local_planner/simple.py b/dimos/navigation/local_planner/simple.py deleted file mode 100644 index 52a20cd048..0000000000 --- a/dimos/navigation/local_planner/simple.py +++ /dev/null @@ -1,246 +0,0 @@ -# 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 enum import Enum, auto - -import numpy as np -from numpy.typing import NDArray -from reactivex.disposable import Disposable - -from dimos.core import In, Out, rpc -from dimos.core.global_config import GlobalConfig -from dimos.msgs.geometry_msgs import PoseStamped, Twist, Vector3 -from dimos.msgs.nav_msgs import CostValues, OccupancyGrid, Path -from dimos.navigation.bt_navigator.goal_validator import find_safe_goal -from dimos.navigation.local_planner.local_planner import BaseLocalPlanner -from dimos.utils.logging_config import setup_logger -from dimos.utils.transform_utils import normalize_angle, quaternion_to_euler - -logger = setup_logger() - - -class NavigationStage(Enum): - INITIAL_ROTATION = auto() - PATH_FOLLOWING = auto() - FINAL_ROTATION = auto() - - -class SimplePlanner(BaseLocalPlanner): - global_costmap: In[OccupancyGrid] = None # type: ignore[assignment] - goal_request: In[PoseStamped] = None # type: ignore[assignment] - target: Out[PoseStamped] = None # type: ignore[assignment] - - _global_config: GlobalConfig - _latest_costmap: OccupancyGrid | None = None - - def __init__( - self, - speed: float = 1.0, - k_angular: float = 2.0, - lookahead_dist: float = 0.5, - goal_tolerance: float = 0.3, - orientation_tolerance: float = 0.2, - control_frequency: float = 20.0, - global_config: GlobalConfig | None = None, - ) -> None: - super().__init__( - goal_tolerance=goal_tolerance, - orientation_tolerance=orientation_tolerance, - control_frequency=control_frequency, - ) - - self._global_config = global_config or GlobalConfig() - - self.speed = self._global_config.planner_robot_speed or speed - self.k_angular = k_angular - self.lookahead_dist = lookahead_dist - self._stage = NavigationStage.INITIAL_ROTATION - self._goal_reached = False - - @rpc - def start(self) -> None: - super().start() - self._disposables.add(Disposable(self.goal_request.subscribe(self._set_target))) - - def set_latest_costmap(msg: OccupancyGrid) -> None: - self._latest_costmap = msg - - self._disposables.add(Disposable(self.global_costmap.subscribe(set_latest_costmap))) - - @rpc - def stop(self) -> None: - super().stop() - - def _on_path(self, msg: Path) -> None: - self._stage = NavigationStage.INITIAL_ROTATION - self._goal_reached = False - super()._on_path(msg) - - def compute_velocity(self) -> Twist | None: - if self._goal_reached: - return None - - if self.latest_odom is None or self.latest_path is None: - return None - - if not len(self.latest_path.poses): - return None - - if self._stage == NavigationStage.INITIAL_ROTATION: - return self._compute_initial_rotation() - elif self._stage == NavigationStage.PATH_FOLLOWING: - return self._compute_path_following() - else: # FINAL_ROTATION - return self._compute_final_rotation() - - def _compute_initial_rotation(self) -> Twist: - assert self.latest_path is not None - assert self.latest_odom is not None - first_pose = self.latest_path.poses[0] - first_yaw = quaternion_to_euler(first_pose.orientation).z - robot_yaw = self.latest_odom.orientation.euler[2] - yaw_error = normalize_angle(first_yaw - robot_yaw) - - if abs(yaw_error) < self.orientation_tolerance: - self._stage = NavigationStage.PATH_FOLLOWING - return self._compute_path_following() - - max_angular_speed = self.speed - angular_velocity = np.clip( - self.k_angular * yaw_error, -max_angular_speed, max_angular_speed - ) - - return Twist( - linear=Vector3(0.0, 0.0, 0.0), - angular=Vector3(0.0, 0.0, angular_velocity), - ) - - def _compute_path_following(self) -> Twist: - assert self.latest_path is not None - assert self.latest_odom is not None - current_pose = np.array([self.latest_odom.position.x, self.latest_odom.position.y]) - path = np.array([[p.position.x, p.position.y] for p in self.latest_path.poses]) - - # Check if we've reached the final position - goal_position = path[-1] - distance_to_goal = np.linalg.norm(goal_position - current_pose) - - if distance_to_goal < self.goal_tolerance: - logger.info("Reached goal position, starting final rotation") - self._stage = NavigationStage.FINAL_ROTATION - return self._compute_final_rotation() - - closest_idx = self._find_closest_point_idx(current_pose, path) - lookahead_point = self._find_lookahead_point(current_pose, path, closest_idx) - - direction = lookahead_point - current_pose - distance = np.linalg.norm(direction) - - if distance < 1e-6: - return Twist() - - robot_yaw = self.latest_odom.orientation.euler[2] - desired_yaw = np.arctan2(direction[1], direction[0]) - yaw_error = normalize_angle(desired_yaw - robot_yaw) - - max_angular_speed = 0.5 - angular_velocity = np.clip( - self.k_angular * yaw_error, -max_angular_speed, max_angular_speed - ) - - # Linear velocity - reduce speed when turning - angular_slowdown = 1.0 - 0.7 * min(abs(angular_velocity) / max_angular_speed, 1.0) - linear_velocity = self.speed * angular_slowdown - - return Twist( - linear=Vector3(linear_velocity, 0.0, 0.0), - angular=Vector3(0.0, 0.0, angular_velocity), - ) - - def _compute_final_rotation(self) -> Twist: - assert self.latest_path is not None - assert self.latest_odom is not None - last_pose = self.latest_path.poses[-1] - goal_yaw = quaternion_to_euler(last_pose.orientation).z - robot_yaw = self.latest_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") - self._goal_reached = True - return Twist() - - max_angular_speed = self.speed - angular_velocity = np.clip( - self.k_angular * yaw_error, -max_angular_speed, max_angular_speed - ) - - return Twist( - linear=Vector3(0.0, 0.0, 0.0), - angular=Vector3(0.0, 0.0, angular_velocity), - ) - - def _find_closest_point_idx(self, pose: NDArray[np.float64], path: NDArray[np.float64]) -> int: - """Find the index of the closest point on the path.""" - distances = np.linalg.norm(path - pose, axis=1) - return int(np.argmin(distances)) - - def _find_lookahead_point( - self, pose: NDArray[np.float64], path: NDArray[np.float64], start_idx: int - ) -> NDArray[np.float64]: - """Find a lookahead point on the path at the specified distance.""" - accumulated_dist: float = 0.0 - - for i in range(start_idx, len(path) - 1): - segment_dist = float(np.linalg.norm(path[i + 1] - path[i])) - - if accumulated_dist + segment_dist >= self.lookahead_dist: - remaining_dist = self.lookahead_dist - accumulated_dist - if segment_dist > 0: - t = remaining_dist / segment_dist - result: NDArray[np.float64] = path[i] + t * (path[i + 1] - path[i]) - return result - result_i: NDArray[np.float64] = path[i] - return result_i - - accumulated_dist += segment_dist - - # Return the last point if we've traversed the whole path - result_last: NDArray[np.float64] = path[-1] - return result_last - - def _set_target(self, pose_stamped: PoseStamped) -> None: - if self._latest_costmap is None: - logger.warning("Cannot set target: missing costmap") - return - - safe_goal = find_safe_goal( - self._latest_costmap, - pose_stamped.position, - algorithm="bfs_contiguous", - cost_threshold=CostValues.OCCUPIED, - min_clearance=self._global_config.robot_rotation_diameter / 2, - max_search_distance=5.0, - ) - - if safe_goal is None: - logger.warning("No safe goal found near requested target") - return - logger.info("Set safe goal", x=safe_goal.x, y=safe_goal.y) - self.target.publish(pose_stamped) - - -simple_planner = SimplePlanner.blueprint - -__all__ = ["SimplePlanner", "simple_planner"] diff --git a/dimos/robot/unitree_webrtc/unitree_go2_blueprints.py b/dimos/robot/unitree_webrtc/unitree_go2_blueprints.py index 764ef22b36..a52618735b 100644 --- a/dimos/robot/unitree_webrtc/unitree_go2_blueprints.py +++ b/dimos/robot/unitree_webrtc/unitree_go2_blueprints.py @@ -85,8 +85,6 @@ go2_connection(), mapper(voxel_size=0.5, global_publish_interval=2.5), replanning_a_star_planner(), - # astar_planner(), - # simple_planner(), wavefront_frontier_explorer(), websocket_vis(), foxglove_bridge(), From ca5ead8a5a59c9492f84396b2767c90526114741 Mon Sep 17 00:00:00 2001 From: Paul Nechifor Date: Mon, 15 Dec 2025 15:46:38 +0200 Subject: [PATCH 13/42] navigation updates --- dimos/mapping/occupancy/visualize_path.py | 10 +- dimos/msgs/nav_msgs/OccupancyGrid.py | 3 - dimos/navigation/global_planner/astar.py | 14 +- .../replanning_a_star/global_planner.py | 96 +++++----- .../replanning_a_star/local_planner.py | 164 +++++++++++++----- dimos/navigation/replanning_a_star/module.py | 9 + .../replanning_a_star/navigation_map.py | 58 +++++++ .../replanning_a_star/path_clearance.py | 97 +++++++++++ 8 files changed, 352 insertions(+), 99 deletions(-) create mode 100644 dimos/navigation/replanning_a_star/navigation_map.py create mode 100644 dimos/navigation/replanning_a_star/path_clearance.py diff --git a/dimos/mapping/occupancy/visualize_path.py b/dimos/mapping/occupancy/visualize_path.py index 6698e7e35f..ec67eebf7c 100644 --- a/dimos/mapping/occupancy/visualize_path.py +++ b/dimos/mapping/occupancy/visualize_path.py @@ -23,12 +23,16 @@ def visualize_path( - occupancy_grid: OccupancyGrid, path: Path, robot_width: float, robot_length: float + 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 - scale = 8 bgr = cv2.resize( bgr, (bgr.shape[1] * scale, bgr.shape[0] * scale), @@ -75,7 +79,7 @@ def visualize_path( # 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=1) + cv2.polylines(bgr, [pts], isClosed=True, color=(0, 0, 0), thickness=thickness) return Image( data=bgr, diff --git a/dimos/msgs/nav_msgs/OccupancyGrid.py b/dimos/msgs/nav_msgs/OccupancyGrid.py index e2f8bbcdce..9119fa45bb 100644 --- a/dimos/msgs/nav_msgs/OccupancyGrid.py +++ b/dimos/msgs/nav_msgs/OccupancyGrid.py @@ -306,6 +306,3 @@ def lcm_decode(cls, data: bytes | BinaryIO) -> OccupancyGrid: ) instance.info = lcm_msg.info return instance - - def gradient(self, obstacle_threshold: int = 50, max_distance: float = 2.0) -> OccupancyGrid: - pass diff --git a/dimos/navigation/global_planner/astar.py b/dimos/navigation/global_planner/astar.py index 7079bd6595..49ca1c78ae 100644 --- a/dimos/navigation/global_planner/astar.py +++ b/dimos/navigation/global_planner/astar.py @@ -49,11 +49,17 @@ def astar( case "general": return general_astar(costmap, goal, start) case "min_cost": - ret = min_cost_astar(costmap, goal, start, use_cpp=False) - import numpy as np + ret = min_cost_astar(costmap, goal, start, use_cpp=use_cpp) + # TODO: XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX + # TODO: XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX + # TODO: XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX + # TODO: XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX + # TODO: XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX + # TODO: XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX + # import numpy as np - np.save("center_test.npy", costmap.grid) - visualize_occupancy_grid(costmap, "rainbow", ret).save("astar_min_cost_path.png") + # np.save("center_test.npy", costmap.grid) + # visualize_occupancy_grid(costmap, "rainbow", ret).save("astar_min_cost_path.png") return ret case _: raise NotImplementedError() diff --git a/dimos/navigation/replanning_a_star/global_planner.py b/dimos/navigation/replanning_a_star/global_planner.py index 1b5e92b83b..b612d0b8fb 100644 --- a/dimos/navigation/replanning_a_star/global_planner.py +++ b/dimos/navigation/replanning_a_star/global_planner.py @@ -12,21 +12,24 @@ # See the License for the specific language governing permissions and # limitations under the License. +from threading import RLock + 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_map import make_navigation_map -from dimos.mapping.occupancy.path_resampling import simple_resample_path +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.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 +from dimos.navigation.replanning_a_star.local_planner import LocalPlanner, StopMessage +from dimos.navigation.replanning_a_star.navigation_map import NavigationMap from dimos.utils.logging_config import setup_logger logger = setup_logger() @@ -36,18 +39,21 @@ class GlobalPlanner(Resource): path: Subject[Path] _current_odom: PoseStamped | None = None - _current_global_costmap: OccupancyGrid | None = None - _local_planner: LocalPlanner + _current_goal: PoseStamped | None = None + _goal_reached: bool = False _global_config: GlobalConfig - _last_global_costmap_used: OccupancyGrid | None = None - _current_navigation_map: OccupancyGrid | None = None + _navigation_map: NavigationMap + _local_planner: LocalPlanner _disposables: CompositeDisposable + _lock: RLock def __init__(self, global_config: GlobalConfig) -> None: self.path = Subject() - self._local_planner = LocalPlanner() self._global_config = global_config + self._navigation_map = NavigationMap(self._global_config) + self._local_planner = LocalPlanner(self._global_config, self._navigation_map) self._disposables = CompositeDisposable() + self._lock = RLock() def start(self) -> None: self._local_planner.start() @@ -65,21 +71,47 @@ def handle_odom(self, msg: PoseStamped) -> None: self._local_planner.handle_odom(msg) def handle_global_costmap(self, msg: OccupancyGrid) -> None: - self._current_global_costmap = msg + self._navigation_map.update(msg) def handle_goal_request(self, goal: PoseStamped) -> None: - self.cancel_goal() + self._current_goal = goal + with self._lock: + self._goal_reached = False + self._plan_path(self._current_goal) - if self._current_global_costmap is None: - logger.warning("Cannot handle goal request: missing costmap") - return + def cancel_goal(self) -> None: + self.path.on_next(Path()) + self._local_planner.stop_planning() + + def get_state(self) -> NavigationState: + return self._local_planner.get_state() + + def is_goal_reached(self) -> bool: + with self._lock: + return self._goal_reached + + def _on_stopped_navigating(self, stop_message: StopMessage) -> None: + self.path.on_next(Path()) + if stop_message == "obstacle_found": + logger.info("Replanning path due to obstacle found") + assert self._current_goal is not None + self._plan_path(self._current_goal) + elif stop_message == "arrived": + logger.info("Arrived at goal") + with self._lock: + self._goal_reached = True + elif stop_message == "error": + logger.info("Failure in navigation") + + def _plan_path(self, goal: PoseStamped) -> None: + self.cancel_goal() if self._current_odom is None: logger.warning("Cannot handle goal request: missing odometry") return safe_goal = find_safe_goal( - self._current_global_costmap, + self._navigation_map.binary_costmap, goal.position, algorithm="bfs_contiguous", cost_threshold=CostValues.OCCUPIED, @@ -93,48 +125,24 @@ def handle_goal_request(self, goal: PoseStamped) -> None: logger.info("Found safe goal", x=safe_goal.x, y=safe_goal.y) - costmap = self._get_latest_navigation_map(self._current_global_costmap) robot_pos = self._current_odom.position + costmap = self._navigation_map.gradient_costmap path = astar(self._global_config.astar_algorithm, costmap, safe_goal, robot_pos) if not path: logger.warning("No path found to the goal.", x=safe_goal.x, y=safe_goal.y) return - resampled_path = simple_resample_path(path, goal, 0.1) + resampled_path = smooth_resample_path(path, goal, 0.1) self.path.on_next(resampled_path) self._local_planner.start_planning(resampled_path) - def _get_latest_navigation_map(self, global_costmap: OccupancyGrid) -> OccupancyGrid: - # "is", not "==", to check for reference equality - if global_costmap is self._last_global_costmap_used: - assert self._current_navigation_map is not None - return self._current_navigation_map - - self._current_navigation_map = make_navigation_map( - global_costmap, - self._global_config.robot_width, - strategy=self._global_config.planner_strategy, - ) - self._last_global_costmap_used = global_costmap - - return self._current_navigation_map - - def cancel_goal(self) -> None: - self.path.on_next(Path()) - self._local_planner.stop_planning() - - def get_state(self) -> NavigationState: - return self._local_planner.get_state() - - def is_goal_reached(self) -> bool: - return self._local_planner.is_goal_reached() - - def _on_stopped_navigating(self, _: None) -> None: - self.path.on_next(Path()) - @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 diff --git a/dimos/navigation/replanning_a_star/local_planner.py b/dimos/navigation/replanning_a_star/local_planner.py index d79a715843..bd36af326b 100644 --- a/dimos/navigation/replanning_a_star/local_planner.py +++ b/dimos/navigation/replanning_a_star/local_planner.py @@ -12,53 +12,70 @@ # See the License for the specific language governing permissions and # limitations under the License. +import os from threading import Event, RLock, Thread import time +import traceback from typing import Literal, TypeAlias import numpy as np from numpy.typing import NDArray from reactivex import Subject +from dimos.core.global_config import GlobalConfig from dimos.core.resource import Resource +from dimos.mapping.occupancy.visualizations import visualize_occupancy_grid +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.utils.logging_config import setup_logger from dimos.utils.transform_utils import normalize_angle, quaternion_to_euler -logger = setup_logger() - PlannerState: TypeAlias = Literal["idle", "initial_rotation", "path_following", "final_rotation"] +StopMessage: TypeAlias = Literal["arrived", "obstacle_found", "error"] + +logger = setup_logger() class LocalPlanner(Resource): cmd_vel: Subject[Twist] - stopped_navigating: Subject[None] + stopped_navigating: Subject[StopMessage] + debug_navigation: Subject[Image] _thread: Thread | None = None _path: Path | None = None _np_path: NDArray[np.float64] | None = None + _path_clearance: PathClearance | None _current_odom: PoseStamped | None = None - _goal_reached: bool = False + _pose_index: int _lock: RLock _stop_planning_event: Event _state: PlannerState + _global_config: GlobalConfig + _navigation_map: NavigationMap - speed: float = 1.0 - k_angular: float = 2.0 - lookahead_dist: float = 0.5 - goal_tolerance: float = 0.3 - orientation_tolerance: float = 0.2 - control_frequency: float = 20.0 + _speed: float = 1.0 + _k_angular: float = 2.0 + _lookahead_dist: float = 0.5 + _goal_tolerance: float = 0.3 + _orientation_tolerance: float = 0.2 + _control_frequency: float = 20.0 - def __init__(self) -> None: + 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._global_config = global_config + self._navigation_map = navigation_map def start(self) -> None: pass @@ -75,13 +92,23 @@ def start_planning(self, path: Path) -> None: self._stop_planning_event = Event() with self._lock: - self._goal_reached = False self._path = path self._np_path = np.array([[p.position.x, p.position.y] for p in self._path.poses]) - self._thread = Thread(target=self._loop, daemon=True) + self._path_clearance = PathClearance(self._global_config, self._path) + self._pose_index = 0 + self._thread = Thread(target=self._thread_entrypoint, daemon=True) self._thread.start() + def _reset_state(self) -> None: + with self._lock: + self._state = "idle" + self._path = None + self._np_path = None + self._path_clearance = None + self._pose_index = 0 + def stop_planning(self) -> None: + self.cmd_vel.on_next(Twist()) self._stop_planning_event.set() with self._lock: @@ -89,19 +116,75 @@ def stop_planning(self) -> None: self._thread.join(2) self._thread = None + self._reset_state() + + def _make_debug_navigation_image(self, path: Path) -> 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) + if self._path_clearance is not None: + mask = self._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 + if self._current_odom is not None: + grid_pos = self._navigation_map.gradient_costmap.world_to_grid( + self._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 + + 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 _loop(self) -> None: stop_event = self._stop_planning_event with self._lock: path = self._path + path_clearance = self._path_clearance self._state = "initial_rotation" - if path is None: + if path is None or path_clearance is None: raise RuntimeError("No path set for local planner.") while not stop_event.is_set(): start_time = time.perf_counter() + 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)) + + if path_clearance.is_obstacle_ahead(): + self.stopped_navigating.on_next("obstacle_found") + break + with self._lock: state: PlannerState = self._state @@ -112,22 +195,15 @@ def _loop(self) -> None: elif state == "final_rotation": cmd_vel = self._compute_final_rotation() elif state == "idle": - with self._lock: - self._goal_reached = True - self.stopped_navigating.on_next(None) + self.stopped_navigating.on_next("arrived") break self.cmd_vel.on_next(cmd_vel) elapsed = time.perf_counter() - start_time - sleep_time = max(0.0, (1.0 / self.control_frequency) - elapsed) + sleep_time = max(0.0, (1.0 / self._control_frequency) - elapsed) stop_event.wait(sleep_time) - self.cmd_vel.on_next(Twist()) - - with self._lock: - self._state = "idle" - def _compute_initial_rotation(self) -> Twist: assert self._path is not None assert self._current_odom is not None @@ -136,15 +212,12 @@ def _compute_initial_rotation(self) -> Twist: robot_yaw = self._current_odom.orientation.euler[2] yaw_error = normalize_angle(first_yaw - robot_yaw) - if abs(yaw_error) < self.orientation_tolerance: + if abs(yaw_error) < self._orientation_tolerance: with self._lock: self._state = "path_following" return self._compute_path_following() - max_angular_speed = self.speed - angular_velocity = np.clip( - self.k_angular * yaw_error, -max_angular_speed, max_angular_speed - ) + angular_velocity = np.clip(self._k_angular * yaw_error, -self._speed, self._speed) return Twist( linear=Vector3(0.0, 0.0, 0.0), @@ -160,12 +233,13 @@ def _compute_path_following(self) -> Twist: goal_position = self._np_path[-1] distance_to_goal = np.linalg.norm(goal_position - current_pose) - if distance_to_goal < self.goal_tolerance: + if distance_to_goal < self._goal_tolerance: logger.info("Reached goal position, starting final rotation") self._state = "final_rotation" return self._compute_final_rotation() closest_idx = self._find_closest_point_idx(current_pose, self._np_path) + self._pose_index = closest_idx lookahead_point = self._find_lookahead_point(self._np_path, closest_idx) direction = lookahead_point - current_pose @@ -178,14 +252,18 @@ def _compute_path_following(self) -> Twist: desired_yaw = np.arctan2(direction[1], direction[0]) yaw_error = normalize_angle(desired_yaw - robot_yaw) - max_angular_speed = 0.5 - angular_velocity = np.clip( - self.k_angular * yaw_error, -max_angular_speed, max_angular_speed - ) + # Rotate-then-drive: if heading error is large, rotate in place first + rotation_threshold = 0.5 # ~30 degrees + if abs(yaw_error) > rotation_threshold: + angular_velocity = np.clip(self._k_angular * yaw_error, -self._speed, self._speed) + return Twist( + linear=Vector3(0.0, 0.0, 0.0), + angular=Vector3(0.0, 0.0, angular_velocity), + ) - # Linear velocity - reduce speed when turning - angular_slowdown = 1.0 - 0.7 * min(abs(angular_velocity) / max_angular_speed, 1.0) - linear_velocity = self.speed * angular_slowdown + # When aligned, drive forward with proportional angular correction + angular_velocity = self._k_angular * yaw_error + linear_velocity = self._speed * (1.0 - abs(yaw_error) / rotation_threshold) return Twist( linear=Vector3(linear_velocity, 0.0, 0.0), @@ -200,15 +278,15 @@ def _compute_final_rotation(self) -> Twist: robot_yaw = self._current_odom.orientation.euler[2] yaw_error = normalize_angle(goal_yaw - robot_yaw) - if abs(yaw_error) < self.orientation_tolerance: + if abs(yaw_error) < self._orientation_tolerance: logger.info("Final rotation complete, goal reached") with self._lock: self._state = "idle" return Twist() - max_angular_speed = self.speed + max_angular_speed = self._speed angular_velocity = np.clip( - self.k_angular * yaw_error, -max_angular_speed, max_angular_speed + self._k_angular * yaw_error, -max_angular_speed, max_angular_speed ) return Twist( @@ -230,8 +308,8 @@ def _find_lookahead_point( for i in range(start_idx, len(path) - 1): segment_dist = float(np.linalg.norm(path[i + 1] - path[i])) - if accumulated_dist + segment_dist >= self.lookahead_dist: - remaining_dist = self.lookahead_dist - accumulated_dist + if accumulated_dist + segment_dist >= self._lookahead_dist: + remaining_dist = self._lookahead_dist - accumulated_dist if segment_dist > 0: t = remaining_dist / segment_dist result: NDArray[np.float64] = path[i] + t * (path[i + 1] - path[i]) @@ -256,7 +334,3 @@ def get_state(self) -> NavigationState: return NavigationState.FOLLOWING_PATH case _: raise ValueError(f"Unknown planner state: {state}") - - def is_goal_reached(self) -> bool: - with self._lock: - return self._goal_reached diff --git a/dimos/navigation/replanning_a_star/module.py b/dimos/navigation/replanning_a_star/module.py index 6dad40d697..d8d5f46a6d 100644 --- a/dimos/navigation/replanning_a_star/module.py +++ b/dimos/navigation/replanning_a_star/module.py @@ -12,6 +12,8 @@ # 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 @@ -19,6 +21,7 @@ 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 @@ -33,6 +36,7 @@ class ReplanningAStarPlanner(Module, NavigationInterface): navigation_state: Out[String] cmd_vel: Out[Twist] path: Out[Path] + debug_navigation: Out[Image] _planner: GlobalPlanner _global_config: GlobalConfig @@ -59,6 +63,11 @@ def start(self) -> None: self._disposables.add(self._planner.cmd_vel.subscribe(self.cmd_vel.publish)) + if "DEBUG_NAVIGATION" in os.environ: + self._disposables.add( + self._planner.debug_navigation.subscribe(self.debug_navigation.publish) + ) + self._planner.start() @rpc 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..f6fee3a61e --- /dev/null +++ b/dimos/navigation/replanning_a_star/navigation_map.py @@ -0,0 +1,58 @@ +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 + _last_binary: OccupancyGrid | None = None + _navigation_map: 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: + """ + Get the latest navigation map created from inflating and applying a + gradient to the binary costmap. + """ + + with self._lock: + if self._binary is None: + raise ValueError("No current global costmap available") + + # "is", not "==", to check for reference equality + if self._binary is self._last_binary: + assert self._navigation_map is not None + return self._navigation_map + + self._navigation_map = make_navigation_map( + self._binary, + self._global_config.robot_width, + strategy=self._global_config.planner_strategy, + ) + self._last_binary = self._binary + + return self._navigation_map 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..fb6ab95921 --- /dev/null +++ b/dimos/navigation/replanning_a_star/path_clearance.py @@ -0,0 +1,97 @@ +# 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_]: + assert self._costmap is not None + + if ( + self._last_mask is not None + and self._last_used_pose is not None + and self._costmap.grid.shape == self._last_used_shape + and self._pose_distance(self._last_used_pose, self._pose_index) + > self._max_distance_cache + ): + return self._last_mask + + self._last_mask = make_path_mask( + occupancy_grid=self._costmap, + path=self._path, + robot_width=self._global_config.robot_width, + pose_index=0, + max_length=self._path_lookup_distance, + ) + + self._last_used_shape = self._costmap.grid.shape + self._last_used_pose = self._pose_index + + x = bool(np.any(self._costmap.grid[self._last_mask] == CostValues.OCCUPIED)) + print( + "mask", + x, + self._last_used_pose, + self._pose_index, + self._costmap.grid.shape, + self._last_used_shape, + ) + + return self._last_mask + + def is_obstacle_ahead(self) -> bool: + if self._costmap is None: + return True + return bool(np.any(self._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) From 2d6ea5f21468bcfbdfd118359decd3ba2608034d Mon Sep 17 00:00:00 2001 From: Paul Nechifor Date: Tue, 16 Dec 2025 12:12:20 +0200 Subject: [PATCH 14/42] multiple changes --- data/.lfs/astar_corner_general.png.tar.gz | 3 + data/.lfs/astar_corner_min_cost.png.tar.gz | 3 + data/.lfs/gradient_simple.png.tar.gz | 3 + data/.lfs/gradient_voronoi.png.tar.gz | 3 + .../.lfs/make_navigation_map_mixed.png.tar.gz | 4 +- .../make_navigation_map_simple.png.tar.gz | 4 +- data/.lfs/three_paths.npy.tar.gz | 3 + dimos/mapping/occupancy/extrude_occupancy.py | 2 +- dimos/mapping/occupancy/test_gradient.py | 37 ++++ dimos/navigation/global_planner/astar.py | 14 +- .../global_planner/min_cost_astar.py | 32 +-- dimos/navigation/global_planner/test_astar.py | 27 ++- .../replanning_a_star/global_planner.py | 6 +- .../replanning_a_star/local_planner.py | 197 ++++++++---------- .../replanning_a_star/path_clearance.py | 4 +- .../replanning_a_star/path_distancer.py | 80 +++++++ .../web/websocket_vis/websocket_vis_module.py | 4 +- 17 files changed, 282 insertions(+), 144 deletions(-) create mode 100644 data/.lfs/astar_corner_general.png.tar.gz create mode 100644 data/.lfs/astar_corner_min_cost.png.tar.gz create mode 100644 data/.lfs/gradient_simple.png.tar.gz create mode 100644 data/.lfs/gradient_voronoi.png.tar.gz create mode 100644 data/.lfs/three_paths.npy.tar.gz create mode 100644 dimos/mapping/occupancy/test_gradient.py create mode 100644 dimos/navigation/replanning_a_star/path_distancer.py 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/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/make_navigation_map_mixed.png.tar.gz b/data/.lfs/make_navigation_map_mixed.png.tar.gz index 7870a1114a..4fcaa8134a 100644 --- a/data/.lfs/make_navigation_map_mixed.png.tar.gz +++ b/data/.lfs/make_navigation_map_mixed.png.tar.gz @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:e8fcb045521f356ab23e0daff9ed1d1102409498a7ff987edcba4e57255f4070 -size 8155 +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 index 5831ee4aad..f966b459e2 100644 --- a/data/.lfs/make_navigation_map_simple.png.tar.gz +++ b/data/.lfs/make_navigation_map_simple.png.tar.gz @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:ec371b962c853377387af52ef4d742e8312b13d76ea6cc1e9182a1f0f23d884b -size 7474 +oid sha256:a0d211fa1bc517ef78e8dc548ebff09f58ad34c86d28eb3bd48a09a577ee5d1e +size 11767 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/dimos/mapping/occupancy/extrude_occupancy.py b/dimos/mapping/occupancy/extrude_occupancy.py index 358e667085..71e6879456 100644 --- a/dimos/mapping/occupancy/extrude_occupancy.py +++ b/dimos/mapping/occupancy/extrude_occupancy.py @@ -228,7 +228,7 @@ def generate_mujoco_scene( xml_lines.append(" ") xml_lines.append(' ') - xml_lines.append("") + xml_lines.append("\n") xml_content = "\n".join(xml_lines) 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/navigation/global_planner/astar.py b/dimos/navigation/global_planner/astar.py index 49ca1c78ae..c9159e40cd 100644 --- a/dimos/navigation/global_planner/astar.py +++ b/dimos/navigation/global_planner/astar.py @@ -12,7 +12,6 @@ # See the License for the specific language governing permissions and # limitations under the License. -from dimos.mapping.occupancy.visualizations import visualize_occupancy_grid from dimos.msgs.geometry_msgs import VectorLike from dimos.msgs.nav_msgs import OccupancyGrid, Path from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid @@ -49,17 +48,6 @@ def astar( case "general": return general_astar(costmap, goal, start) case "min_cost": - ret = min_cost_astar(costmap, goal, start, use_cpp=use_cpp) - # TODO: XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX - # TODO: XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX - # TODO: XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX - # TODO: XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX - # TODO: XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX - # TODO: XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX - # import numpy as np - - # np.save("center_test.npy", costmap.grid) - # visualize_occupancy_grid(costmap, "rainbow", ret).save("astar_min_cost_path.png") - return ret + return min_cost_astar(costmap, goal, start, use_cpp=use_cpp) case _: raise NotImplementedError() diff --git a/dimos/navigation/global_planner/min_cost_astar.py b/dimos/navigation/global_planner/min_cost_astar.py index c05a4286bb..9ca81831be 100644 --- a/dimos/navigation/global_planner/min_cost_astar.py +++ b/dimos/navigation/global_planner/min_cost_astar.py @@ -16,6 +16,7 @@ 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: @@ -27,6 +28,8 @@ except ImportError: _USE_CPP = False +logger = setup_logger() + # Define possible movements (8-connected grid with diagonal movements) _directions = [ (0, 1), @@ -133,19 +136,22 @@ def min_cost_astar( if not (0 <= goal_tuple[0] < costmap.width and 0 <= goal_tuple[1] < costmap.height): return None - if use_cpp and _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) + 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 diff --git a/dimos/navigation/global_planner/test_astar.py b/dimos/navigation/global_planner/test_astar.py index 19c253cdc0..fd5784a3af 100644 --- a/dimos/navigation/global_planner/test_astar.py +++ b/dimos/navigation/global_planner/test_astar.py @@ -18,7 +18,7 @@ from open3d.geometry import PointCloud # type: ignore[import-untyped] import pytest -from dimos.mapping.occupancy.gradient import gradient +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 @@ -32,6 +32,11 @@ 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", [ @@ -40,7 +45,7 @@ def costmap() -> PointCloud: ], ) def test_astar(costmap, mode, expected_image) -> None: - start = Vector3(4.0, 2.0, 0) + start = Vector3(4.0, 2.0) goal = Vector3(6.15, 10.0) expected = Image.from_file(get_data(expected_image)) @@ -50,6 +55,24 @@ def test_astar(costmap, mode, expected_image) -> None: 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) diff --git a/dimos/navigation/replanning_a_star/global_planner.py b/dimos/navigation/replanning_a_star/global_planner.py index b612d0b8fb..a74b76c3c1 100644 --- a/dimos/navigation/replanning_a_star/global_planner.py +++ b/dimos/navigation/replanning_a_star/global_planner.py @@ -123,14 +123,16 @@ def _plan_path(self, goal: PoseStamped) -> None: logger.warning("No safe goal found near requested target") return - logger.info("Found safe goal", x=safe_goal.x, y=safe_goal.y) + logger.info("Found safe goal", x=round(safe_goal.x, 2), y=round(safe_goal.y, 2)) robot_pos = self._current_odom.position costmap = self._navigation_map.gradient_costmap path = astar(self._global_config.astar_algorithm, costmap, safe_goal, robot_pos) if not path: - logger.warning("No path found to the goal.", x=safe_goal.x, y=safe_goal.y) + 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, goal, 0.1) diff --git a/dimos/navigation/replanning_a_star/local_planner.py b/dimos/navigation/replanning_a_star/local_planner.py index bd36af326b..360420a9de 100644 --- a/dimos/navigation/replanning_a_star/local_planner.py +++ b/dimos/navigation/replanning_a_star/local_planner.py @@ -13,18 +13,16 @@ # limitations under the License. import os -from threading import Event, RLock, Thread +from threading import Event, RLock, Thread, current_thread import time import traceback from typing import Literal, TypeAlias import numpy as np -from numpy.typing import NDArray from reactivex import Subject from dimos.core.global_config import GlobalConfig from dimos.core.resource import Resource -from dimos.mapping.occupancy.visualizations import visualize_occupancy_grid 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 @@ -33,6 +31,7 @@ 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 @@ -49,8 +48,8 @@ class LocalPlanner(Resource): _thread: Thread | None = None _path: Path | None = None - _np_path: NDArray[np.float64] | None = None _path_clearance: PathClearance | None + _path_distancer: PathDistancer | None _current_odom: PoseStamped | None = None _pose_index: int _lock: RLock @@ -61,7 +60,6 @@ class LocalPlanner(Resource): _speed: float = 1.0 _k_angular: float = 2.0 - _lookahead_dist: float = 0.5 _goal_tolerance: float = 0.3 _orientation_tolerance: float = 0.2 _control_frequency: float = 20.0 @@ -84,7 +82,8 @@ def stop(self) -> None: self.stop_planning() def handle_odom(self, msg: PoseStamped) -> None: - self._current_odom = msg + with self._lock: + self._current_odom = msg def start_planning(self, path: Path) -> None: self.stop_planning() @@ -93,62 +92,35 @@ def start_planning(self, path: Path) -> None: with self._lock: self._path = path - self._np_path = np.array([[p.position.x, p.position.y] for p in self._path.poses]) 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 _reset_state(self) -> None: - with self._lock: - self._state = "idle" - self._path = None - self._np_path = None - self._path_clearance = None - self._pose_index = 0 - def stop_planning(self) -> None: self.cmd_vel.on_next(Twist()) self._stop_planning_event.set() with self._lock: if self._thread is not None: - self._thread.join(2) + if self._thread is not current_thread(): + self._thread.join(2) self._thread = None self._reset_state() - def _make_debug_navigation_image(self, path: Path) -> 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) - if self._path_clearance is not None: - mask = self._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 - if self._current_odom is not None: - grid_pos = self._navigation_map.gradient_costmap.world_to_grid( - self._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 + def get_state(self) -> NavigationState: + with self._lock: + state = self._state + + match state: + case "idle": + return NavigationState.IDLE + case "initial_rotation" | "path_following" | "final_rotation": + return NavigationState.FOLLOWING_PATH + case _: + raise ValueError(f"Unknown planner state: {state}") def _thread_entrypoint(self) -> None: try: @@ -205,11 +177,16 @@ def _loop(self) -> None: stop_event.wait(sleep_time) def _compute_initial_rotation(self) -> Twist: - assert self._path is not None - assert self._current_odom is not None - first_pose = self._path.poses[0] + 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 = self._current_odom.orientation.euler[2] + robot_yaw = current_odom.orientation.euler[2] yaw_error = normalize_angle(first_yaw - robot_yaw) if abs(yaw_error) < self._orientation_tolerance: @@ -225,30 +202,31 @@ def _compute_initial_rotation(self) -> Twist: ) def _compute_path_following(self) -> Twist: - assert self._np_path is not None - assert self._current_odom is not None - current_pose = np.array([self._current_odom.position.x, self._current_odom.position.y]) + 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 - # Check if we've reached the final position - goal_position = self._np_path[-1] - distance_to_goal = np.linalg.norm(goal_position - current_pose) + current_pos = np.array([current_odom.position.x, current_odom.position.y]) - if distance_to_goal < self._goal_tolerance: + if path_distancer.distance_to_goal(current_pos) < self._goal_tolerance: logger.info("Reached goal position, starting final rotation") self._state = "final_rotation" return self._compute_final_rotation() - closest_idx = self._find_closest_point_idx(current_pose, self._np_path) - self._pose_index = closest_idx - lookahead_point = self._find_lookahead_point(self._np_path, closest_idx) + closest_index = path_distancer.find_closest_point_index(current_pos) + self._pose_index = closest_index + lookahead_point = path_distancer.find_lookahead_point(closest_index) - direction = lookahead_point - current_pose + direction = lookahead_point - current_pos distance = np.linalg.norm(direction) if distance < 1e-6: - return Twist() + return Twist() # TODO: WOT??????????????????????????????????????????????????????????????????????????????? - robot_yaw = self._current_odom.orientation.euler[2] + robot_yaw = current_odom.orientation.euler[2] desired_yaw = np.arctan2(direction[1], direction[0]) yaw_error = normalize_angle(desired_yaw - robot_yaw) @@ -271,11 +249,15 @@ def _compute_path_following(self) -> Twist: ) def _compute_final_rotation(self) -> Twist: - assert self._path is not None - assert self._current_odom is not None - last_pose = self._path.poses[-1] - goal_yaw = quaternion_to_euler(last_pose.orientation).z - robot_yaw = self._current_odom.orientation.euler[2] + 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: @@ -294,43 +276,46 @@ def _compute_final_rotation(self) -> Twist: angular=Vector3(0.0, 0.0, angular_velocity), ) - def _find_closest_point_idx(self, pose: NDArray[np.float64], path: NDArray[np.float64]) -> int: - """Find the index of the closest point on the path.""" - distances = np.linalg.norm(path - pose, axis=1) - return int(np.argmin(distances)) - - def _find_lookahead_point( - self, path: NDArray[np.float64], start_idx: int - ) -> NDArray[np.float64]: - """Find a lookahead point on the path at the specified distance.""" - accumulated_dist: float = 0.0 - - for i in range(start_idx, len(path) - 1): - segment_dist = float(np.linalg.norm(path[i + 1] - path[i])) - - if accumulated_dist + segment_dist >= self._lookahead_dist: - remaining_dist = self._lookahead_dist - accumulated_dist - if segment_dist > 0: - t = remaining_dist / segment_dist - result: NDArray[np.float64] = path[i] + t * (path[i + 1] - path[i]) - return result - result_i: NDArray[np.float64] = path[i] - return result_i - - accumulated_dist += segment_dist + def _reset_state(self) -> None: + with self._lock: + self._state = "idle" + self._path = None + self._path_clearance = None + self._path_distancer = None + self._pose_index = 0 - # Return the last point if we've traversed the whole path - result_last: NDArray[np.float64] = path[-1] - return result_last + def _make_debug_navigation_image(self, path: Path) -> 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) - def get_state(self) -> NavigationState: - with self._lock: - state = self._state + if self._path_clearance is not None: + mask = self._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 - match state: - case "idle": - return NavigationState.IDLE - case "initial_rotation" | "path_following" | "final_rotation": - return NavigationState.FOLLOWING_PATH - case _: - raise ValueError(f"Unknown planner state: {state}") + if self._current_odom is not None: + grid_pos = self._navigation_map.gradient_costmap.world_to_grid( + self._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/path_clearance.py b/dimos/navigation/replanning_a_star/path_clearance.py index fb6ab95921..83924245be 100644 --- a/dimos/navigation/replanning_a_star/path_clearance.py +++ b/dimos/navigation/replanning_a_star/path_clearance.py @@ -59,7 +59,7 @@ def mask(self) -> NDArray[np.bool_]: and self._last_used_pose is not None and self._costmap.grid.shape == self._last_used_shape and self._pose_distance(self._last_used_pose, self._pose_index) - > self._max_distance_cache + < self._max_distance_cache ): return self._last_mask @@ -67,7 +67,7 @@ def mask(self) -> NDArray[np.bool_]: occupancy_grid=self._costmap, path=self._path, robot_width=self._global_config.robot_width, - pose_index=0, + pose_index=self._pose_index, max_length=self._path_lookup_distance, ) 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..03f35861ec --- /dev/null +++ b/dimos/navigation/replanning_a_star/path_distancer.py @@ -0,0 +1,80 @@ +# 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 import Path + + +class PathDistancer: + _path: NDArray[np.float64] | None = None + _cumulative_dists: NDArray[np.float64] | None = None + _lookahead_dist: float = 0.5 + + 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_cummulative_distance_array(self._path) + + def find_lookahead_point(self, start_idx: int) -> NDArray[np.float64]: + """ + Given a path, and a precomputed array of cummulative distances, find the + point which is `lookahead_dist` ahead of the current point. + """ + + if start_idx >= len(self._path) - 1: + return 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 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 self._path[idx] + t * (self._path[idx + 1] - self._path[idx]) + + return self._path[idx] + + def distance_to_goal(self, current_pos: NDArray[np.float64]) -> float: + return np.linalg.norm(self._path[-1] - current_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_cummulative_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/web/websocket_vis/websocket_vis_module.py b/dimos/web/websocket_vis/websocket_vis_module.py index 6e0e8d6343..27b6c339db 100644 --- a/dimos/web/websocket_vis/websocket_vis_module.py +++ b/dimos/web/websocket_vis/websocket_vis_module.py @@ -206,7 +206,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, goal) -> None: # type: ignore[no-untyped-def] From 4917ff4527b67803d9187ae893df71581916ee58 Mon Sep 17 00:00:00 2001 From: Paul Nechifor Date: Wed, 17 Dec 2025 06:34:04 +0200 Subject: [PATCH 15/42] add back deleted code --- dimos/msgs/nav_msgs/OccupancyGrid.py | 100 +++++++++++++++++++++++++++ 1 file changed, 100 insertions(+) diff --git a/dimos/msgs/nav_msgs/OccupancyGrid.py b/dimos/msgs/nav_msgs/OccupancyGrid.py index 9119fa45bb..6f91b2f9f0 100644 --- a/dimos/msgs/nav_msgs/OccupancyGrid.py +++ b/dimos/msgs/nav_msgs/OccupancyGrid.py @@ -306,3 +306,103 @@ def lcm_decode(cls, data: bytes | BinaryIO) -> OccupancyGrid: ) instance.info = lcm_msg.info return instance + + def filter_above(self, threshold: int) -> OccupancyGrid: + """Create a new OccupancyGrid with only values above threshold. + + Args: + threshold: Keep cells with values > threshold + + Returns: + New OccupancyGrid where: + - Cells > threshold: kept as-is + - Cells <= threshold: set to -1 (unknown) + - Unknown cells (-1): preserved + """ + new_grid = self.grid.copy() + + # Create mask for cells to filter (not unknown and <= threshold) + filter_mask = (new_grid != -1) & (new_grid <= threshold) + + # Set filtered cells to unknown + new_grid[filter_mask] = -1 + + # Create new OccupancyGrid + filtered = OccupancyGrid( + new_grid, + resolution=self.resolution, + origin=self.origin, + frame_id=self.frame_id, + ts=self.ts, + ) + + return filtered + + def filter_below(self, threshold: int) -> OccupancyGrid: + """Create a new OccupancyGrid with only values below threshold. + + Args: + threshold: Keep cells with values < threshold + + Returns: + New OccupancyGrid where: + - Cells < threshold: kept as-is + - Cells >= threshold: set to -1 (unknown) + - Unknown cells (-1): preserved + """ + new_grid = self.grid.copy() + + # Create mask for cells to filter (not unknown and >= threshold) + filter_mask = (new_grid != -1) & (new_grid >= threshold) + + # Set filtered cells to unknown + new_grid[filter_mask] = -1 + + # Create new OccupancyGrid + filtered = OccupancyGrid( + new_grid, + resolution=self.resolution, + origin=self.origin, + frame_id=self.frame_id, + ts=self.ts, + ) + + return filtered + + def max(self) -> OccupancyGrid: + """Create a new OccupancyGrid with all non-unknown cells set to maximum value. + + Returns: + New OccupancyGrid where: + - All non-unknown cells: set to CostValues.OCCUPIED (100) + - Unknown cells: preserved as CostValues.UNKNOWN (-1) + """ + new_grid = self.grid.copy() + + # Set all non-unknown cells to max + new_grid[new_grid != CostValues.UNKNOWN] = CostValues.OCCUPIED + + # Create new OccupancyGrid + maxed = OccupancyGrid( + new_grid, + resolution=self.resolution, + origin=self.origin, + frame_id=self.frame_id, + ts=self.ts, + ) + + 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, + ) From fd366cce46120fb37b56745741d1a107facdaad8 Mon Sep 17 00:00:00 2001 From: Paul Nechifor Date: Wed, 17 Dec 2025 06:37:37 +0200 Subject: [PATCH 16/42] fix tests --- dimos/mapping/occupancy/conftest.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/dimos/mapping/occupancy/conftest.py b/dimos/mapping/occupancy/conftest.py index d5dec276a8..9cc9006c72 100644 --- a/dimos/mapping/occupancy/conftest.py +++ b/dimos/mapping/occupancy/conftest.py @@ -15,6 +15,7 @@ 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 @@ -26,4 +27,4 @@ def occupancy() -> OccupancyGrid: @pytest.fixture def occupancy_gradient(occupancy) -> OccupancyGrid: - return occupancy.gradient(max_distance=1.5) + return gradient(occupancy, max_distance=1.5) From 61ad19173e9c6df883b626264042f8a0691d6410 Mon Sep 17 00:00:00 2001 From: Paul Nechifor Date: Wed, 17 Dec 2025 06:40:58 +0200 Subject: [PATCH 17/42] fix header --- .../navigation/replanning_a_star/navigation_map.py | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/dimos/navigation/replanning_a_star/navigation_map.py b/dimos/navigation/replanning_a_star/navigation_map.py index f6fee3a61e..44e3511f64 100644 --- a/dimos/navigation/replanning_a_star/navigation_map.py +++ b/dimos/navigation/replanning_a_star/navigation_map.py @@ -1,3 +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 threading import RLock from dimos.core.global_config import GlobalConfig From 0beef85add16cdb3f2bcc0c80d1df0a51aeba929 Mon Sep 17 00:00:00 2001 From: Paul Nechifor Date: Wed, 17 Dec 2025 06:45:10 +0200 Subject: [PATCH 18/42] fix bad fixture --- data/.lfs/expected_occupancy_scene.xml.tar.gz | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/data/.lfs/expected_occupancy_scene.xml.tar.gz b/data/.lfs/expected_occupancy_scene.xml.tar.gz index ad51dc8b4b..efbe7ce49d 100644 --- a/data/.lfs/expected_occupancy_scene.xml.tar.gz +++ b/data/.lfs/expected_occupancy_scene.xml.tar.gz @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:345a15c6ea70244bacc65c6490d39346378eba68c55a09ebdf9cf1ad53666f97 -size 6740 +oid sha256:e3eb91f3c7787882bf26a69df21bb1933d2f6cd71132ca5f0521e2808269bfa2 +size 6777 From d7ef9bb4e96ea0593c4a00b5d4e1f36656a1e5bc Mon Sep 17 00:00:00 2001 From: Paul Nechifor Date: Wed, 17 Dec 2025 06:57:28 +0200 Subject: [PATCH 19/42] fix linting --- dimos/mapping/occupancy/gradient.py | 2 +- .../replanning_a_star/path_distancer.py | 19 ++++++++++++------- 2 files changed, 13 insertions(+), 8 deletions(-) diff --git a/dimos/mapping/occupancy/gradient.py b/dimos/mapping/occupancy/gradient.py index b5752af0bf..45c889b921 100644 --- a/dimos/mapping/occupancy/gradient.py +++ b/dimos/mapping/occupancy/gradient.py @@ -13,7 +13,7 @@ # limitations under the License. import numpy as np -from scipy import ndimage +from scipy import ndimage # type: ignore[import-untyped] from dimos.msgs.nav_msgs.OccupancyGrid import CostValues, OccupancyGrid diff --git a/dimos/navigation/replanning_a_star/path_distancer.py b/dimos/navigation/replanning_a_star/path_distancer.py index 03f35861ec..05f57064b1 100644 --- a/dimos/navigation/replanning_a_star/path_distancer.py +++ b/dimos/navigation/replanning_a_star/path_distancer.py @@ -12,6 +12,8 @@ # 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 @@ -19,9 +21,9 @@ class PathDistancer: - _path: NDArray[np.float64] | None = None - _cumulative_dists: NDArray[np.float64] | None = None _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]) @@ -34,7 +36,7 @@ def find_lookahead_point(self, start_idx: int) -> NDArray[np.float64]: """ if start_idx >= len(self._path) - 1: - return 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 @@ -44,7 +46,7 @@ def find_lookahead_point(self, start_idx: int) -> NDArray[np.float64]: idx = int(np.searchsorted(self._cumulative_dists, target_dist)) if idx >= len(self._cumulative_dists): - return self._path[-1] + 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 @@ -53,12 +55,15 @@ def find_lookahead_point(self, start_idx: int) -> NDArray[np.float64]: if segment_dist > 0: t = remaining_dist / segment_dist - return self._path[idx] + t * (self._path[idx + 1] - self._path[idx]) + return cast( + "NDArray[np.float64]", + self._path[idx] + t * (self._path[idx + 1] - self._path[idx]), + ) - return self._path[idx] + return cast("NDArray[np.float64]", self._path[idx]) def distance_to_goal(self, current_pos: NDArray[np.float64]) -> float: - return np.linalg.norm(self._path[-1] - current_pos) + return float(np.linalg.norm(self._path[-1] - current_pos)) def find_closest_point_index(self, pos: NDArray[np.float64]) -> int: """Find the index of the closest point on the path.""" From 0ede7ebc2c4845def89411d0aece989d0f377f66 Mon Sep 17 00:00:00 2001 From: Paul Nechifor Date: Wed, 17 Dec 2025 21:32:12 +0200 Subject: [PATCH 20/42] 001 --- dimos/core/blueprints.py | 18 ++++++++++++++++++ dimos/navigation/replanning_a_star/module.py | 8 ++++++-- .../unitree_webrtc/unitree_go2_blueprints.py | 3 +++ 3 files changed, 27 insertions(+), 2 deletions(-) diff --git a/dimos/core/blueprints.py b/dimos/core/blueprints.py index 484e076af9..4df0300265 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=_get_topic(transport), + 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. @@ -350,3 +362,9 @@ def _eliminate_duplicates(blueprints: list[ModuleBlueprint]) -> list[ModuleBluep seen.add(bp.module) unique_blueprints.append(bp) return list(reversed(unique_blueprints)) + + +def _get_topic(transport: Any) -> str: + topic = getattr(transport, "topic", None) + topic = getattr(transport, "topic", topic) + return str(topic) diff --git a/dimos/navigation/replanning_a_star/module.py b/dimos/navigation/replanning_a_star/module.py index d8d5f46a6d..49befb4017 100644 --- a/dimos/navigation/replanning_a_star/module.py +++ b/dimos/navigation/replanning_a_star/module.py @@ -30,10 +30,11 @@ class ReplanningAStarPlanner(Module, NavigationInterface): odom: In[PoseStamped] # TODO: Use TF. global_costmap: In[OccupancyGrid] goal_request: In[PoseStamped] + target: In[PoseStamped] + local_costmap: In[OccupancyGrid] # TODO: Use this. - target: Out[PoseStamped] goal_reached: Out[Bool] - navigation_state: Out[String] + navigation_state: Out[String] # TODO: set it cmd_vel: Out[Twist] path: Out[Path] debug_navigation: Out[Image] @@ -59,6 +60,9 @@ def start(self) -> None: 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)) diff --git a/dimos/robot/unitree_webrtc/unitree_go2_blueprints.py b/dimos/robot/unitree_webrtc/unitree_go2_blueprints.py index a52618735b..30181c22f2 100644 --- a/dimos/robot/unitree_webrtc/unitree_go2_blueprints.py +++ b/dimos/robot/unitree_webrtc/unitree_go2_blueprints.py @@ -84,6 +84,9 @@ 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(), From 5e142a263df2c983b36c62bbe855106f28f43f9f Mon Sep 17 00:00:00 2001 From: Paul Nechifor Date: Thu, 18 Dec 2025 04:32:26 +0200 Subject: [PATCH 21/42] check if stuck --- dimos/core/module.py | 2 +- .../replanning_a_star/global_planner.py | 130 ++++++++++++++---- .../replanning_a_star/local_planner.py | 4 +- .../replanning_a_star/path_clearance.py | 10 -- .../replanning_a_star/position_tracker.py | 92 +++++++++++++ 5 files changed, 199 insertions(+), 39 deletions(-) create mode 100644 dimos/navigation/replanning_a_star/position_tracker.py diff --git a/dimos/core/module.py b/dimos/core/module.py index 3bf662d842..6b11fd7afd 100644 --- a/dimos/core/module.py +++ b/dimos/core/module.py @@ -319,7 +319,7 @@ def set_ref(self, ref) -> int: # type: ignore[no-untyped-def] def __str__(self) -> str: return f"{self.__class__.__name__}" - # called from remote + @rpc def set_transport(self, stream_name: str, transport: Transport) -> bool: # type: ignore[type-arg] stream = getattr(self, stream_name, None) if not stream: diff --git a/dimos/navigation/replanning_a_star/global_planner.py b/dimos/navigation/replanning_a_star/global_planner.py index a74b76c3c1..479b963652 100644 --- a/dimos/navigation/replanning_a_star/global_planner.py +++ b/dimos/navigation/replanning_a_star/global_planner.py @@ -12,7 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -from threading import RLock +from threading import Event, RLock, Thread, current_thread from reactivex import Subject from reactivex.disposable import CompositeDisposable @@ -30,6 +30,7 @@ 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() @@ -41,45 +42,75 @@ class GlobalPlanner(Resource): _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 _disposables: CompositeDisposable + _stop_planner: Event _lock: RLock + _replan_attempt: int + + _safe_goal_tolerance: float = 4.0 + _replan_goal_tolerance: float = 0.5 + _max_replan_attempts: int = 10 def __init__(self, global_config: GlobalConfig) -> None: self.path = 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._disposables = CompositeDisposable() self._lock = RLock() + self._stop_planner = Event() 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() + self._replan_attempt = 0 def stop(self) -> None: self.cancel_goal() self._local_planner.stop() self._disposables.dispose() + self._stop_planner.set() + if self._thread is not None: + if self._thread is not current_thread(): + self._thread.join(2) + self._thread = None def handle_odom(self, msg: PoseStamped) -> None: - self._current_odom = msg + 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: - self._current_goal = goal with self._lock: + self._current_goal = goal self._goal_reached = False - self._plan_path(self._current_goal) + self._replan_attempt = 0 + self._plan_path() + + def cancel_goal(self, *, but_will_try_again: bool = False, arrived: bool = False) -> None: + 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 - def cancel_goal(self) -> None: self.path.on_next(Path()) self._local_planner.stop_planning() @@ -90,42 +121,95 @@ 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: + while not self._stop_planner.is_set(): + with self._lock: + current_goal = self._current_goal + + if current_goal: + if self._position_tracker.is_stuck(): + self._replan_path() + + self._stop_planner.wait(0.1) + def _on_stopped_navigating(self, stop_message: StopMessage) -> None: self.path.on_next(Path()) if stop_message == "obstacle_found": - logger.info("Replanning path due to obstacle found") - assert self._current_goal is not None - self._plan_path(self._current_goal) + logger.info("Replanning path due to obstacle found.") + self._replan_path() elif stop_message == "arrived": - logger.info("Arrived at goal") - with self._lock: - self._goal_reached = True + logger.info("Arrived at goal.") + self.cancel_goal(arrived=True) elif stop_message == "error": - logger.info("Failure in navigation") + logger.info("Failure in navigation.") + self._replan_path() + else: + logger.error(f"No code to handle '{stop_message}'.") + self.cancel_goal() - def _plan_path(self, goal: PoseStamped) -> None: - 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 - if self._current_odom is None: + 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, - goal.position, + current_goal.position, algorithm="bfs_contiguous", cost_threshold=CostValues.OCCUPIED, min_clearance=self._global_config.robot_rotation_diameter / 2, - max_search_distance=5.0, + 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)) - robot_pos = self._current_odom.position + robot_pos = current_odom.position costmap = self._navigation_map.gradient_costmap path = astar(self._global_config.astar_algorithm, costmap, safe_goal, robot_pos) @@ -135,16 +219,8 @@ def _plan_path(self, goal: PoseStamped) -> None: ) return - resampled_path = smooth_resample_path(path, goal, 0.1) + resampled_path = smooth_resample_path(path, current_goal, 0.1) self.path.on_next(resampled_path) self._local_planner.start_planning(resampled_path) - - @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 diff --git a/dimos/navigation/replanning_a_star/local_planner.py b/dimos/navigation/replanning_a_star/local_planner.py index 360420a9de..54ef677251 100644 --- a/dimos/navigation/replanning_a_star/local_planner.py +++ b/dimos/navigation/replanning_a_star/local_planner.py @@ -12,6 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. +import math import os from threading import Event, RLock, Thread, current_thread import time @@ -63,6 +64,7 @@ class LocalPlanner(Resource): _goal_tolerance: float = 0.3 _orientation_tolerance: float = 0.2 _control_frequency: float = 20.0 + _rotation_threshold: float = 90 def __init__(self, global_config: GlobalConfig, navigation_map: NavigationMap) -> None: self.cmd_vel = Subject() @@ -231,7 +233,7 @@ def _compute_path_following(self) -> Twist: yaw_error = normalize_angle(desired_yaw - robot_yaw) # Rotate-then-drive: if heading error is large, rotate in place first - rotation_threshold = 0.5 # ~30 degrees + rotation_threshold = self._rotation_threshold * math.pi / 180 if abs(yaw_error) > rotation_threshold: angular_velocity = np.clip(self._k_angular * yaw_error, -self._speed, self._speed) return Twist( diff --git a/dimos/navigation/replanning_a_star/path_clearance.py b/dimos/navigation/replanning_a_star/path_clearance.py index 83924245be..2251659840 100644 --- a/dimos/navigation/replanning_a_star/path_clearance.py +++ b/dimos/navigation/replanning_a_star/path_clearance.py @@ -74,16 +74,6 @@ def mask(self) -> NDArray[np.bool_]: self._last_used_shape = self._costmap.grid.shape self._last_used_pose = self._pose_index - x = bool(np.any(self._costmap.grid[self._last_mask] == CostValues.OCCUPIED)) - print( - "mask", - x, - self._last_used_pose, - self._pose_index, - self._costmap.grid.shape, - self._last_used_shape, - ) - return self._last_mask def is_obstacle_ahead(self) -> bool: 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..fca00a1563 --- /dev/null +++ b/dimos/navigation/replanning_a_star/position_tracker.py @@ -0,0 +1,92 @@ +# 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 + +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 + _last_check: float + + def __init__(self) -> None: + self._lock = RLock() + self._time_window = 3.0 + self._threshold = 0.5 + 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 + self._last_check = time.perf_counter() + + def add_position(self, pose: PoseStamped) -> None: + with self._lock: + self._timestamps[self._index] = pose.ts + 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]: + now = time.time() + cutoff = now - self._time_window + + if self._size == 0: + return np.empty((0, 2)) + + 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 pos[mask] + + def is_stuck(self) -> bool: + with self._lock: + last_check = self._last_check + + if time.perf_counter() - last_check < self._time_window: + return False + + with self._lock: + self._last_check = time.perf_counter() + 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)) From 771df085f8a7efb9ef7518b7030643af067a0271 Mon Sep 17 00:00:00 2001 From: Paul Nechifor Date: Thu, 18 Dec 2025 07:26:51 +0200 Subject: [PATCH 22/42] try multiple sizes --- .../replanning_a_star/global_planner.py | 28 +++++++++++++++---- .../replanning_a_star/local_planner.py | 13 ++++++--- .../replanning_a_star/navigation_map.py | 26 +++++++---------- .../replanning_a_star/position_tracker.py | 2 +- dimos/robot/unitree_webrtc/type/map.py | 4 +-- 5 files changed, 44 insertions(+), 29 deletions(-) diff --git a/dimos/navigation/replanning_a_star/global_planner.py b/dimos/navigation/replanning_a_star/global_planner.py index 479b963652..d205646688 100644 --- a/dimos/navigation/replanning_a_star/global_planner.py +++ b/dimos/navigation/replanning_a_star/global_planner.py @@ -22,6 +22,7 @@ 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 @@ -103,6 +104,8 @@ def handle_goal_request(self, goal: PoseStamped) -> None: 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() @@ -136,6 +139,7 @@ def _thread_entrypoint(self) -> None: if current_goal: if self._position_tracker.is_stuck(): + logger.info("Robot is stuck. Replanning.") self._replan_path() self._stop_planner.wait(0.1) @@ -161,6 +165,8 @@ def _replan_path(self) -> None: 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 @@ -187,7 +193,7 @@ def _plan_path(self) -> None: assert current_goal is not None if current_odom is None: - logger.warning("Cannot handle goal request: missing odometry") + logger.warning("Cannot handle goal request: missing odometry.") return safe_goal = find_safe_goal( @@ -200,18 +206,16 @@ def _plan_path(self) -> None: ) if safe_goal is None: - logger.warning("No safe goal found near requested target") + 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)) + logger.info("Found safe goal.", x=round(safe_goal.x, 2), y=round(safe_goal.y, 2)) - robot_pos = current_odom.position - costmap = self._navigation_map.gradient_costmap - path = astar(self._global_config.astar_algorithm, costmap, safe_goal, robot_pos) + path = self._find_wide_path(safe_goal, current_odom.position) if not path: logger.warning( @@ -224,3 +228,15 @@ def _plan_path(self) -> None: 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: + 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 index 54ef677251..cba74fd1db 100644 --- a/dimos/navigation/replanning_a_star/local_planner.py +++ b/dimos/navigation/replanning_a_star/local_planner.py @@ -36,7 +36,9 @@ 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"] +PlannerState: TypeAlias = Literal[ + "idle", "initial_rotation", "path_following", "final_rotation", "arrived" +] StopMessage: TypeAlias = Literal["arrived", "obstacle_found", "error"] logger = setup_logger() @@ -168,11 +170,14 @@ def _loop(self) -> None: cmd_vel = self._compute_path_following() elif state == "final_rotation": cmd_vel = self._compute_final_rotation() - elif state == "idle": + elif state == "arrived": self.stopped_navigating.on_next("arrived") break + elif state == "idle": + cmd_vel = None - self.cmd_vel.on_next(cmd_vel) + 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) @@ -265,7 +270,7 @@ def _compute_final_rotation(self) -> Twist: if abs(yaw_error) < self._orientation_tolerance: logger.info("Final rotation complete, goal reached") with self._lock: - self._state = "idle" + self._state = "arrived" return Twist() max_angular_speed = self._speed diff --git a/dimos/navigation/replanning_a_star/navigation_map.py b/dimos/navigation/replanning_a_star/navigation_map.py index 44e3511f64..e29122784f 100644 --- a/dimos/navigation/replanning_a_star/navigation_map.py +++ b/dimos/navigation/replanning_a_star/navigation_map.py @@ -22,8 +22,6 @@ class NavigationMap: _global_config: GlobalConfig _binary: OccupancyGrid | None = None - _last_binary: OccupancyGrid | None = None - _navigation_map: OccupancyGrid | None = None _lock: RLock def __init__(self, global_config: GlobalConfig) -> None: @@ -48,25 +46,21 @@ def binary_costmap(self) -> OccupancyGrid: @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: - if self._binary is None: + binary = self._binary + if binary is None: raise ValueError("No current global costmap available") - # "is", not "==", to check for reference equality - if self._binary is self._last_binary: - assert self._navigation_map is not None - return self._navigation_map - - self._navigation_map = make_navigation_map( - self._binary, - self._global_config.robot_width, - strategy=self._global_config.planner_strategy, - ) - self._last_binary = self._binary - - return self._navigation_map + 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/position_tracker.py b/dimos/navigation/replanning_a_star/position_tracker.py index fca00a1563..6d411a20c3 100644 --- a/dimos/navigation/replanning_a_star/position_tracker.py +++ b/dimos/navigation/replanning_a_star/position_tracker.py @@ -36,7 +36,7 @@ class PositionTracker: def __init__(self) -> None: self._lock = RLock() - self._time_window = 3.0 + self._time_window = 10.0 self._threshold = 0.5 self._max_points = int(_max_points_per_second * self._time_window) self.reset_data() diff --git a/dimos/robot/unitree_webrtc/type/map.py b/dimos/robot/unitree_webrtc/type/map.py index 36faca394d..fdec3c3800 100644 --- a/dimos/robot/unitree_webrtc/type/map.py +++ b/dimos/robot/unitree_webrtc/type/map.py @@ -47,8 +47,8 @@ def __init__( # type: ignore[no-untyped-def] 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: From 8347f13773fcf759e74a1fe899ec073cb527a64d Mon Sep 17 00:00:00 2001 From: Paul Nechifor Date: Thu, 18 Dec 2025 07:44:57 +0200 Subject: [PATCH 23/42] minor fixes --- dimos/navigation/replanning_a_star/global_planner.py | 7 +++++++ dimos/navigation/replanning_a_star/local_planner.py | 8 ++++++-- dimos/navigation/replanning_a_star/module.py | 2 ++ dimos/navigation/replanning_a_star/path_distancer.py | 6 +++--- 4 files changed, 18 insertions(+), 5 deletions(-) diff --git a/dimos/navigation/replanning_a_star/global_planner.py b/dimos/navigation/replanning_a_star/global_planner.py index d205646688..226479a38f 100644 --- a/dimos/navigation/replanning_a_star/global_planner.py +++ b/dimos/navigation/replanning_a_star/global_planner.py @@ -14,6 +14,7 @@ from threading import Event, RLock, Thread, current_thread +from dimos_lcm.std_msgs import Bool # type: ignore[import-untyped] from reactivex import Subject from reactivex.disposable import CompositeDisposable @@ -39,6 +40,7 @@ class GlobalPlanner(Resource): path: Subject[Path] + goal_reached: Subject[Bool] _current_odom: PoseStamped | None = None _current_goal: PoseStamped | None = None @@ -84,6 +86,8 @@ def stop(self) -> None: if self._thread is not None: if 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: @@ -117,6 +121,9 @@ def cancel_goal(self, *, but_will_try_again: bool = False, arrived: bool = False 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() diff --git a/dimos/navigation/replanning_a_star/local_planner.py b/dimos/navigation/replanning_a_star/local_planner.py index cba74fd1db..f02ded89a9 100644 --- a/dimos/navigation/replanning_a_star/local_planner.py +++ b/dimos/navigation/replanning_a_star/local_planner.py @@ -110,6 +110,8 @@ def stop_planning(self) -> None: if self._thread is not None: if self._thread is not current_thread(): self._thread.join(2) + if self._thread.is_alive(): + logger.error("LocalPlanner thread did not stop in time.") self._thread = None self._reset_state() @@ -220,7 +222,8 @@ def _compute_path_following(self) -> Twist: if path_distancer.distance_to_goal(current_pos) < self._goal_tolerance: logger.info("Reached goal position, starting final rotation") - self._state = "final_rotation" + with self._lock: + self._state = "final_rotation" return self._compute_final_rotation() closest_index = path_distancer.find_closest_point_index(current_pos) @@ -231,7 +234,8 @@ def _compute_path_following(self) -> Twist: distance = np.linalg.norm(direction) if distance < 1e-6: - return Twist() # TODO: WOT??????????????????????????????????????????????????????????????????????????????? + # 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]) diff --git a/dimos/navigation/replanning_a_star/module.py b/dimos/navigation/replanning_a_star/module.py index 49befb4017..965e0c9668 100644 --- a/dimos/navigation/replanning_a_star/module.py +++ b/dimos/navigation/replanning_a_star/module.py @@ -67,6 +67,8 @@ def start(self) -> None: 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) diff --git a/dimos/navigation/replanning_a_star/path_distancer.py b/dimos/navigation/replanning_a_star/path_distancer.py index 05f57064b1..7d4f537530 100644 --- a/dimos/navigation/replanning_a_star/path_distancer.py +++ b/dimos/navigation/replanning_a_star/path_distancer.py @@ -27,11 +27,11 @@ class PathDistancer: 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_cummulative_distance_array(self._path) + 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 cummulative distances, find the + Given a path, and a precomputed array of cumulative distances, find the point which is `lookahead_dist` ahead of the current point. """ @@ -71,7 +71,7 @@ def find_closest_point_index(self, pos: NDArray[np.float64]) -> int: return int(np.argmin(distances)) -def _make_cummulative_distance_array(array: NDArray[np.float64]) -> NDArray[np.float64]: +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. From 485a69d4b209e34e886c50b0d6dbac2b7f23e09e Mon Sep 17 00:00:00 2001 From: Paul Nechifor Date: Thu, 18 Dec 2025 07:57:21 +0200 Subject: [PATCH 24/42] move local_costmap from map.py --- .../navigation/local_planner/local_planner.py | 21 +++++++++++---- dimos/navigation/replanning_a_star/module.py | 1 - .../replanning_a_star/position_tracker.py | 5 ++-- dimos/robot/unitree_webrtc/type/map.py | 26 +------------------ dimos/robot/unitree_webrtc/type/test_map.py | 1 - 5 files changed, 20 insertions(+), 34 deletions(-) diff --git a/dimos/navigation/local_planner/local_planner.py b/dimos/navigation/local_planner/local_planner.py index 79f136b48e..2c1899b516 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] = None # type: ignore[assignment] + lidar: In[LidarMessage] = None # type: ignore[assignment] odom: In[PoseStamped] = None # type: ignore[assignment] path: In[Path] = None # type: ignore[assignment] @@ -92,7 +95,7 @@ def __init__( # type: ignore[no-untyped-def] def start(self) -> None: super().start() - self._disposables.add(Disposable(self.local_costmap.subscribe(self._on_costmap))) + 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))) @@ -101,8 +104,16 @@ 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 diff --git a/dimos/navigation/replanning_a_star/module.py b/dimos/navigation/replanning_a_star/module.py index 965e0c9668..7ffd5d4fb1 100644 --- a/dimos/navigation/replanning_a_star/module.py +++ b/dimos/navigation/replanning_a_star/module.py @@ -31,7 +31,6 @@ class ReplanningAStarPlanner(Module, NavigationInterface): global_costmap: In[OccupancyGrid] goal_request: In[PoseStamped] target: In[PoseStamped] - local_costmap: In[OccupancyGrid] # TODO: Use this. goal_reached: Out[Bool] navigation_state: Out[String] # TODO: set it diff --git a/dimos/navigation/replanning_a_star/position_tracker.py b/dimos/navigation/replanning_a_star/position_tracker.py index 6d411a20c3..e756aee796 100644 --- a/dimos/navigation/replanning_a_star/position_tracker.py +++ b/dimos/navigation/replanning_a_star/position_tracker.py @@ -14,6 +14,7 @@ from threading import RLock import time +from typing import cast import numpy as np from numpy.typing import NDArray @@ -61,7 +62,7 @@ def _get_recent_positions(self) -> NDArray[np.float32]: cutoff = now - self._time_window if self._size == 0: - return np.empty((0, 2)) + return np.empty((0, 2), dtype=np.float32) if self._size < self._max_points: mask = self._timestamps[: self._size] >= cutoff @@ -70,7 +71,7 @@ def _get_recent_positions(self) -> NDArray[np.float32]: 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 pos[mask] + return cast(NDArray[np.float32], pos[mask]) def is_stuck(self) -> bool: with self._lock: diff --git a/dimos/robot/unitree_webrtc/type/map.py b/dimos/robot/unitree_webrtc/type/map.py index fdec3c3800..9a01e319bc 100644 --- a/dimos/robot/unitree_webrtc/type/map.py +++ b/dimos/robot/unitree_webrtc/type/map.py @@ -22,7 +22,6 @@ from dimos.core import DimosCluster, In, LCMTransport, Module, Out, rpc from dimos.core.global_config import GlobalConfig -from dimos.mapping.occupancy.gradient import gradient from dimos.mapping.pointclouds.accumulators.general import GeneralPointCloudAccumulator from dimos.mapping.pointclouds.accumulators.protocol import PointCloudAccumulator from dimos.mapping.pointclouds.occupancy import general_occupancy @@ -36,7 +35,6 @@ class Map(Module): lidar: In[LidarMessage] = None # type: ignore[assignment] global_map: Out[LidarMessage] = None # type: ignore[assignment] global_costmap: Out[OccupancyGrid] = None # type: ignore[assignment] - local_costmap: Out[OccupancyGrid] = None # type: ignore[assignment] _point_cloud_accumulator: PointCloudAccumulator _global_config: GlobalConfig @@ -95,32 +93,11 @@ def to_lidar_message(self) -> LidarMessage: ts=time.time(), ) - # Is this RPC? + # TODO: Why is this RPC? @rpc def add_frame(self, frame: LidarMessage) -> None: self._point_cloud_accumulator.add(frame.pointcloud) - # TODO: MOVE THIS - ################################################################################# - ################################################################################# - ################################################################################# - ################################################################################# - ################################################################################# - ################################################################################# - ################################################################################# - ################################################################################# - local_costmap = gradient( - general_occupancy( - frame, - resolution=self.cost_resolution, - min_height=0.15, - max_height=0.6, - ), - max_distance=0.25, - ) - - self.local_costmap.publish(local_costmap) - @property def o3d_geometry(self) -> o3d.geometry.PointCloud: return self._point_cloud_accumulator.get_point_cloud() @@ -152,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 bde2022a8f..d0479a782c 100644 --- a/dimos/robot/unitree_webrtc/type/test_map.py +++ b/dimos/robot/unitree_webrtc/type/test_map.py @@ -88,7 +88,6 @@ class MockStream: def publish(self, msg) -> None: pass # Do nothing - map_.local_costmap = MockStream() map_.global_costmap = MockStream() map_.global_map = MockStream() From 1215efaa2d5ae6822ad51316eb9da7e0770d6212 Mon Sep 17 00:00:00 2001 From: Paul Nechifor Date: Thu, 18 Dec 2025 08:17:02 +0200 Subject: [PATCH 25/42] linting --- dimos/agents2/skills/google_maps_skill_container.py | 2 -- dimos/models/__init__.py | 2 +- dimos/models/vl/__init__.py | 4 ++-- dimos/models/vl/test_vlm.py | 2 +- dimos/navigation/replanning_a_star/position_tracker.py | 2 +- dimos/robot/drone/test_drone.py | 4 ++-- 6 files changed, 7 insertions(+), 9 deletions(-) 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/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/navigation/replanning_a_star/position_tracker.py b/dimos/navigation/replanning_a_star/position_tracker.py index e756aee796..85b5fa22d4 100644 --- a/dimos/navigation/replanning_a_star/position_tracker.py +++ b/dimos/navigation/replanning_a_star/position_tracker.py @@ -71,7 +71,7 @@ def _get_recent_positions(self) -> NDArray[np.float32]: 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]) + return cast("NDArray[np.float32]", pos[mask]) def is_stuck(self) -> bool: with self._lock: 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], From 6868247963201802e1b353cb00ad753abb984c0c Mon Sep 17 00:00:00 2001 From: Paul Nechifor Date: Thu, 18 Dec 2025 08:21:34 +0200 Subject: [PATCH 26/42] fix test --- dimos/core/test_core.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dimos/core/test_core.py b/dimos/core/test_core.py index f9262acd55..17ee300a44 100644 --- a/dimos/core/test_core.py +++ b/dimos/core/test_core.py @@ -87,7 +87,7 @@ def test_classmethods() -> None: # Check that we have the expected RPC methods assert "navigate_to" in class_rpcs, "navigate_to should be in rpcs" assert "start" in class_rpcs, "start should be in rpcs" - assert len(class_rpcs) == 8 + assert len(class_rpcs) == 9 # Check that the values are callable assert callable(class_rpcs["navigate_to"]), "navigate_to should be callable" From 5af8bc9416d556c686c9da05ad93dd9a024eba62 Mon Sep 17 00:00:00 2001 From: Paul Nechifor Date: Thu, 18 Dec 2025 11:42:39 +0200 Subject: [PATCH 27/42] tweaks --- .../replanning_a_star/global_planner.py | 15 ++++++++++++--- .../navigation/replanning_a_star/local_planner.py | 9 ++------- 2 files changed, 14 insertions(+), 10 deletions(-) diff --git a/dimos/navigation/replanning_a_star/global_planner.py b/dimos/navigation/replanning_a_star/global_planner.py index 226479a38f..b5c468ae1c 100644 --- a/dimos/navigation/replanning_a_star/global_planner.py +++ b/dimos/navigation/replanning_a_star/global_planner.py @@ -60,6 +60,7 @@ class GlobalPlanner(Resource): 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) @@ -143,9 +144,17 @@ def _thread_entrypoint(self) -> None: while not self._stop_planner.is_set(): with self._lock: current_goal = self._current_goal - - if current_goal: - if self._position_tracker.is_stuck(): + current_odom = self._current_odom + + if current_goal and current_odom: + # Check if close enough to goal - accept as arrived + 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) + elif self._position_tracker.is_stuck(): logger.info("Robot is stuck. Replanning.") self._replan_path() diff --git a/dimos/navigation/replanning_a_star/local_planner.py b/dimos/navigation/replanning_a_star/local_planner.py index f02ded89a9..b6bff1cc91 100644 --- a/dimos/navigation/replanning_a_star/local_planner.py +++ b/dimos/navigation/replanning_a_star/local_planner.py @@ -14,7 +14,7 @@ import math import os -from threading import Event, RLock, Thread, current_thread +from threading import Event, RLock, Thread import time import traceback from typing import Literal, TypeAlias @@ -107,12 +107,7 @@ def stop_planning(self) -> None: self._stop_planning_event.set() with self._lock: - if self._thread is not None: - if self._thread is not current_thread(): - self._thread.join(2) - if self._thread.is_alive(): - logger.error("LocalPlanner thread did not stop in time.") - self._thread = None + self._thread = None self._reset_state() From cbfa7d3905fdcad3208de470fb9f8752cb38c756 Mon Sep 17 00:00:00 2001 From: Paul Nechifor Date: Fri, 19 Dec 2025 03:44:36 +0200 Subject: [PATCH 28/42] add pd control --- .../replanning_a_star/local_planner.py | 45 +++++++++++++++---- 1 file changed, 37 insertions(+), 8 deletions(-) diff --git a/dimos/navigation/replanning_a_star/local_planner.py b/dimos/navigation/replanning_a_star/local_planner.py index b6bff1cc91..8496d069c5 100644 --- a/dimos/navigation/replanning_a_star/local_planner.py +++ b/dimos/navigation/replanning_a_star/local_planner.py @@ -62,11 +62,15 @@ class LocalPlanner(Resource): _navigation_map: NavigationMap _speed: float = 1.0 - _k_angular: float = 2.0 + _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.2 _control_frequency: float = 20.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() @@ -78,6 +82,8 @@ def __init__(self, global_config: GlobalConfig, navigation_map: NavigationMap) - self._state = "idle" 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 @@ -180,6 +186,30 @@ def _loop(self) -> None: sleep_time = max(0.0, (1.0 / self._control_frequency) - elapsed) stop_event.wait(sleep_time) + 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) + + # 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 @@ -198,7 +228,7 @@ def _compute_initial_rotation(self) -> Twist: self._state = "path_following" return self._compute_path_following() - angular_velocity = np.clip(self._k_angular * yaw_error, -self._speed, self._speed) + angular_velocity = self._compute_angular_velocity(yaw_error, self._speed) return Twist( linear=Vector3(0.0, 0.0, 0.0), @@ -239,14 +269,14 @@ def _compute_path_following(self) -> Twist: # 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 = np.clip(self._k_angular * yaw_error, -self._speed, self._speed) + 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._k_angular * yaw_error + angular_velocity = self._compute_angular_velocity(yaw_error, self._speed) linear_velocity = self._speed * (1.0 - abs(yaw_error) / rotation_threshold) return Twist( @@ -272,10 +302,7 @@ def _compute_final_rotation(self) -> Twist: self._state = "arrived" return Twist() - max_angular_speed = self._speed - angular_velocity = np.clip( - self._k_angular * yaw_error, -max_angular_speed, max_angular_speed - ) + angular_velocity = self._compute_angular_velocity(yaw_error, self._speed) return Twist( linear=Vector3(0.0, 0.0, 0.0), @@ -289,6 +316,8 @@ def _reset_state(self) -> 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) -> Image: scale = 8 From f417bed9a9787a48c113a8fe7954c88c68868fec Mon Sep 17 00:00:00 2001 From: Paul Nechifor Date: Fri, 19 Dec 2025 04:22:43 +0200 Subject: [PATCH 29/42] improvements --- dimos/core/blueprints.py | 8 +------- dimos/mapping/occupancy/gradient.py | 25 +++++++++---------------- dimos/mapping/occupancy/path_mask.py | 2 +- 3 files changed, 11 insertions(+), 24 deletions(-) diff --git a/dimos/core/blueprints.py b/dimos/core/blueprints.py index 4df0300265..cdc0df919d 100644 --- a/dimos/core/blueprints.py +++ b/dimos/core/blueprints.py @@ -212,7 +212,7 @@ def _connect_transports(self, module_coordinator: ModuleCoordinator) -> None: "Transport", name=remapped_name, original_name=original_name, - topic=_get_topic(transport), + topic=str(getattr(transport, "topic", None)), type=f"{type.__module__}.{type.__qualname__}", module=module.__name__, transport=transport.__class__.__name__, @@ -362,9 +362,3 @@ def _eliminate_duplicates(blueprints: list[ModuleBlueprint]) -> list[ModuleBluep seen.add(bp.module) unique_blueprints.append(bp) return list(reversed(unique_blueprints)) - - -def _get_topic(transport: Any) -> str: - topic = getattr(transport, "topic", None) - topic = getattr(transport, "topic", topic) - return str(topic) diff --git a/dimos/mapping/occupancy/gradient.py b/dimos/mapping/occupancy/gradient.py index 45c889b921..aeafa53d3d 100644 --- a/dimos/mapping/occupancy/gradient.py +++ b/dimos/mapping/occupancy/gradient.py @@ -150,22 +150,15 @@ def voronoi_gradient( nearest_obstacle_cluster = obstacle_labels[indices[0], indices[1]] # Find Voronoi edges: cells where neighbors belong to different obstacle clusters - voronoi_edges = np.zeros_like(obstacle_map, dtype=bool) - - # Check 8-connectivity neighbors using roll for efficient comparison - for dy in [-1, 0, 1]: - for dx in [-1, 0, 1]: - if dy == 0 and dx == 0: - continue - # Shift and compare cluster labels - shifted = np.roll(np.roll(nearest_obstacle_cluster, dy, axis=0), dx, axis=1) - voronoi_edges |= nearest_obstacle_cluster != shifted - - # Remove edges at boundaries (roll wraps around, creating false edges) - voronoi_edges[0, :] = False - voronoi_edges[-1, :] = False - voronoi_edges[:, 0] = False - voronoi_edges[:, -1] = False + # 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 diff --git a/dimos/mapping/occupancy/path_mask.py b/dimos/mapping/occupancy/path_mask.py index 48c9b8ed2d..8dfbd94cec 100644 --- a/dimos/mapping/occupancy/path_mask.py +++ b/dimos/mapping/occupancy/path_mask.py @@ -86,7 +86,7 @@ def make_path_mask( occupied_count = np.sum(occupied_in_path) if occupied_count / total_points > 0.05: - raise Exception( + raise ValueError( f"More than 5% of path points are occupied: " f"{occupied_count}/{total_points} ({100 * occupied_count / total_points:.1f}%)" ) From 0bedaae0bc7bfd64bf1db02f12e42e9ca17bccda Mon Sep 17 00:00:00 2001 From: Paul Nechifor Date: Fri, 19 Dec 2025 04:24:37 +0200 Subject: [PATCH 30/42] unnecessary set --- dimos/navigation/global_planner/min_cost_astar.py | 10 +--------- 1 file changed, 1 insertion(+), 9 deletions(-) diff --git a/dimos/navigation/global_planner/min_cost_astar.py b/dimos/navigation/global_planner/min_cost_astar.py index 9ca81831be..1300a58fd2 100644 --- a/dimos/navigation/global_planner/min_cost_astar.py +++ b/dimos/navigation/global_planner/min_cost_astar.py @@ -167,16 +167,10 @@ def min_cost_astar( 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)) - # Track nodes already in open set to avoid duplicates - open_set_hash: set[tuple[int, int]] = {start_tuple} - while open_set: _, _, current = heapq.heappop(open_set) current_x, current_y = current - if current in open_set_hash: - open_set_hash.remove(current) - if current in closed_set: continue @@ -228,8 +222,6 @@ def min_cost_astar( priority_dist = tentative_dist + h_dist # Add the neighbor to the open set with its priority - if neighbor not in open_set_hash: - heapq.heappush(open_set, (priority_cost, priority_dist, neighbor)) - open_set_hash.add(neighbor) + heapq.heappush(open_set, (priority_cost, priority_dist, neighbor)) return None From 37c533c5f1fde7d757cd71d0ea7e2741b558f5ea Mon Sep 17 00:00:00 2001 From: Paul Nechifor Date: Fri, 19 Dec 2025 04:53:33 +0200 Subject: [PATCH 31/42] minor fixes --- dimos/mapping/occupancy/gradient.py | 16 ++++++++-------- dimos/mapping/occupancy/path_resampling.py | 2 +- .../replanning_a_star/global_planner.py | 2 +- .../replanning_a_star/local_planner.py | 4 ++-- 4 files changed, 12 insertions(+), 12 deletions(-) diff --git a/dimos/mapping/occupancy/gradient.py b/dimos/mapping/occupancy/gradient.py index aeafa53d3d..91e9bb83a5 100644 --- a/dimos/mapping/occupancy/gradient.py +++ b/dimos/mapping/occupancy/gradient.py @@ -19,7 +19,7 @@ def gradient( - ocupancy_grid: OccupancyGrid, obstacle_threshold: int = 50, max_distance: float = 2.0 + occupancy_grid: OccupancyGrid, obstacle_threshold: int = 50, max_distance: float = 2.0 ) -> OccupancyGrid: """Create a gradient OccupancyGrid for path planning. @@ -41,19 +41,19 @@ def gradient( """ # Remember which cells are unknown - unknown_mask = ocupancy_grid.grid == CostValues.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 = (ocupancy_grid.grid >= obstacle_threshold).astype(np.float32) + 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 * ocupancy_grid.resolution, 0, max_distance) # type: ignore[operator] + 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 @@ -72,10 +72,10 @@ def gradient( # Create new OccupancyGrid with gradient gradient_grid = OccupancyGrid( grid=gradient_data, - resolution=ocupancy_grid.resolution, - origin=ocupancy_grid.origin, - frame_id=ocupancy_grid.frame_id, - ts=ocupancy_grid.ts, + resolution=occupancy_grid.resolution, + origin=occupancy_grid.origin, + frame_id=occupancy_grid.frame_id, + ts=occupancy_grid.ts, ) return gradient_grid diff --git a/dimos/mapping/occupancy/path_resampling.py b/dimos/mapping/occupancy/path_resampling.py index 0ae6a1a089..f33d6c7cb9 100644 --- a/dimos/mapping/occupancy/path_resampling.py +++ b/dimos/mapping/occupancy/path_resampling.py @@ -37,7 +37,7 @@ def _add_orientations_to_path(path: Path, goal_orientation: Quaternion) -> None: Path with orientations added to all poses """ if not path.poses or len(path.poses) < 2: - return None + return # Calculate orientations for all poses except the last one for i in range(len(path.poses) - 1): diff --git a/dimos/navigation/replanning_a_star/global_planner.py b/dimos/navigation/replanning_a_star/global_planner.py index b5c468ae1c..32b354b0fb 100644 --- a/dimos/navigation/replanning_a_star/global_planner.py +++ b/dimos/navigation/replanning_a_star/global_planner.py @@ -86,10 +86,10 @@ def stop(self) -> None: self._stop_planner.set() if self._thread is not None: if self._thread is not current_thread(): + self._thread = None 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: diff --git a/dimos/navigation/replanning_a_star/local_planner.py b/dimos/navigation/replanning_a_star/local_planner.py index 8496d069c5..cd5a0635d8 100644 --- a/dimos/navigation/replanning_a_star/local_planner.py +++ b/dimos/navigation/replanning_a_star/local_planner.py @@ -51,8 +51,8 @@ class LocalPlanner(Resource): _thread: Thread | None = None _path: Path | None = None - _path_clearance: PathClearance | None - _path_distancer: PathDistancer | None + _path_clearance: PathClearance | None = None + _path_distancer: PathDistancer | None = None _current_odom: PoseStamped | None = None _pose_index: int _lock: RLock From 3c9d97446542491a107858290bebe9a7f6fdaef9 Mon Sep 17 00:00:00 2001 From: Paul Nechifor Date: Fri, 19 Dec 2025 05:49:19 +0200 Subject: [PATCH 32/42] get unique state --- .../replanning_a_star/local_planner.py | 17 ++++++++++++----- 1 file changed, 12 insertions(+), 5 deletions(-) diff --git a/dimos/navigation/replanning_a_star/local_planner.py b/dimos/navigation/replanning_a_star/local_planner.py index cd5a0635d8..1799d15144 100644 --- a/dimos/navigation/replanning_a_star/local_planner.py +++ b/dimos/navigation/replanning_a_star/local_planner.py @@ -58,6 +58,7 @@ class LocalPlanner(Resource): _lock: RLock _stop_planning_event: Event _state: PlannerState + _state_unique_id: int _global_config: GlobalConfig _navigation_map: NavigationMap @@ -80,6 +81,7 @@ def __init__(self, global_config: GlobalConfig, navigation_map: NavigationMap) - 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 @@ -140,13 +142,18 @@ def _thread_entrypoint(self) -> None: 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 - self._state = "initial_rotation" + self._change_state("initial_rotation") if path is None or path_clearance is None: raise RuntimeError("No path set for local planner.") @@ -225,7 +232,7 @@ def _compute_initial_rotation(self) -> Twist: if abs(yaw_error) < self._orientation_tolerance: with self._lock: - self._state = "path_following" + self._change_state("path_following") return self._compute_path_following() angular_velocity = self._compute_angular_velocity(yaw_error, self._speed) @@ -248,7 +255,7 @@ def _compute_path_following(self) -> Twist: if path_distancer.distance_to_goal(current_pos) < self._goal_tolerance: logger.info("Reached goal position, starting final rotation") with self._lock: - self._state = "final_rotation" + self._change_state("final_rotation") return self._compute_final_rotation() closest_index = path_distancer.find_closest_point_index(current_pos) @@ -299,7 +306,7 @@ def _compute_final_rotation(self) -> Twist: if abs(yaw_error) < self._orientation_tolerance: logger.info("Final rotation complete, goal reached") with self._lock: - self._state = "arrived" + self._change_state("arrived") return Twist() angular_velocity = self._compute_angular_velocity(yaw_error, self._speed) @@ -311,7 +318,7 @@ def _compute_final_rotation(self) -> Twist: def _reset_state(self) -> None: with self._lock: - self._state = "idle" + self._change_state("idle") self._path = None self._path_clearance = None self._path_distancer = None From 662c8c062ce29644577d4a5c2526bf4a35681a1e Mon Sep 17 00:00:00 2001 From: Paul Nechifor Date: Fri, 19 Dec 2025 07:10:06 +0200 Subject: [PATCH 33/42] move the time check to the global planner --- .../replanning_a_star/global_planner.py | 63 ++++++++++++------- .../replanning_a_star/local_planner.py | 43 ++++++++----- .../replanning_a_star/position_tracker.py | 16 +---- 3 files changed, 74 insertions(+), 48 deletions(-) diff --git a/dimos/navigation/replanning_a_star/global_planner.py b/dimos/navigation/replanning_a_star/global_planner.py index 32b354b0fb..9fab3ddcb2 100644 --- a/dimos/navigation/replanning_a_star/global_planner.py +++ b/dimos/navigation/replanning_a_star/global_planner.py @@ -13,6 +13,7 @@ # 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 @@ -46,9 +47,11 @@ class GlobalPlanner(Resource): _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 _lock: RLock @@ -57,17 +60,20 @@ class GlobalPlanner(Resource): _safe_goal_tolerance: float = 4.0 _replan_goal_tolerance: float = 0.5 _max_replan_attempts: int = 10 + _stuck_time_window: float = 8.0 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._position_tracker = PositionTracker(self._stuck_time_window) self._disposables = CompositeDisposable() - self._lock = RLock() self._stop_planner = Event() + self._lock = RLock() + self._replan_attempt = 0 def start(self) -> None: self._local_planner.start() @@ -77,19 +83,18 @@ def start(self) -> None: self._stop_planner.clear() self._thread = Thread(target=self._thread_entrypoint, daemon=True) self._thread.start() - self._replan_attempt = 0 def stop(self) -> None: self.cancel_goal() self._local_planner.stop() self._disposables.dispose() self._stop_planner.set() - if self._thread is not None: - if self._thread is not current_thread(): - self._thread = None - self._thread.join(2) - if self._thread.is_alive(): - logger.error("GlobalPlanner thread did not stop in time.") + + 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: @@ -141,24 +146,40 @@ def debug_navigation(self) -> Subject[Image]: return self._local_planner.debug_navigation def _thread_entrypoint(self) -> None: + """Monitor if the robot is stuck.""" + + last_id = -1 + last_time = time.perf_counter() + while not self._stop_planner.is_set(): + self._stop_planner.wait(0.1) + with self._lock: current_goal = self._current_goal current_odom = self._current_odom - if current_goal and current_odom: - # Check if close enough to goal - accept as arrived - 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) - elif self._position_tracker.is_stuck(): - logger.info("Robot is stuck. Replanning.") - self._replan_path() + if not current_goal or not current_odom: + continue - self._stop_planner.wait(0.1) + 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 + + _, new_id = self._local_planner.get_unique_state() + + if new_id != last_id: + last_id = new_id + last_time = time.perf_counter() + continue + + if ( + time.perf_counter() - last_time > self._stuck_time_window + and self._position_tracker.is_stuck() + ): + logger.info("Robot is stuck. Replanning.") + self._replan_path() + last_time = time.perf_counter() def _on_stopped_navigating(self, stop_message: StopMessage) -> None: self.path.on_next(Path()) diff --git a/dimos/navigation/replanning_a_star/local_planner.py b/dimos/navigation/replanning_a_star/local_planner.py index 1799d15144..d8c455075c 100644 --- a/dimos/navigation/replanning_a_star/local_planner.py +++ b/dimos/navigation/replanning_a_star/local_planner.py @@ -54,6 +54,7 @@ class LocalPlanner(Resource): _path_clearance: PathClearance | None = None _path_distancer: PathDistancer | None = None _current_odom: PoseStamped | None = None + _pose_index: int _lock: RLock _stop_planning_event: Event @@ -68,8 +69,9 @@ class LocalPlanner(Resource): _max_angular_accel: float = 2.0 _goal_tolerance: float = 0.3 _orientation_tolerance: float = 0.2 - _control_frequency: float = 20.0 + _control_frequency: float = 10.0 _rotation_threshold: float = 90 + _prev_yaw_error: float _prev_angular_velocity: float @@ -77,6 +79,7 @@ def __init__(self, global_config: GlobalConfig, navigation_map: NavigationMap) - self.cmd_vel = Subject() self.stopped_navigating = Subject() self.debug_navigation = Subject() + self._pose_index = 0 self._lock = RLock() self._stop_planning_event = Event() @@ -84,6 +87,7 @@ def __init__(self, global_config: GlobalConfig, navigation_map: NavigationMap) - 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 @@ -124,13 +128,17 @@ def get_state(self) -> NavigationState: state = self._state match state: - case "idle": + 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() @@ -165,7 +173,9 @@ def _loop(self) -> None: 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)) + self.debug_navigation.on_next( + self._make_debug_navigation_image(path, path_clearance) + ) if path_clearance.is_obstacle_ahead(): self.stopped_navigating.on_next("obstacle_found") @@ -326,7 +336,7 @@ def _reset_state(self) -> None: self._prev_yaw_error = 0.0 self._prev_angular_velocity = 0.0 - def _make_debug_navigation_image(self, path: Path) -> Image: + def _make_debug_navigation_image(self, path: Path, path_clearance: PathClearance) -> Image: scale = 8 image = visualize_path( self._navigation_map.gradient_costmap, @@ -338,18 +348,22 @@ def _make_debug_navigation_image(self, path: Path) -> Image: ) image.data = np.flipud(image.data) - if self._path_clearance is not None: - mask = self._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 + # 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 - if self._current_odom is not None: + 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( - self._current_odom.position + current_odom.position ) x = int(grid_pos.x * scale) y = image.data.shape[0] - 1 - int(grid_pos.y * scale) @@ -360,4 +374,5 @@ def _make_debug_navigation_image(self, path: Path) -> Image: 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/position_tracker.py b/dimos/navigation/replanning_a_star/position_tracker.py index 85b5fa22d4..1f94c113b2 100644 --- a/dimos/navigation/replanning_a_star/position_tracker.py +++ b/dimos/navigation/replanning_a_star/position_tracker.py @@ -33,11 +33,10 @@ class PositionTracker: _positions: NDArray[np.float32] _index: int _size: int - _last_check: float - def __init__(self) -> None: + def __init__(self, time_window: float) -> None: self._lock = RLock() - self._time_window = 10.0 + self._time_window = time_window self._threshold = 0.5 self._max_points = int(_max_points_per_second * self._time_window) self.reset_data() @@ -48,7 +47,6 @@ def reset_data(self) -> None: self._positions = np.zeros((self._max_points, 2), dtype=np.float32) self._index = 0 self._size = 0 - self._last_check = time.perf_counter() def add_position(self, pose: PoseStamped) -> None: with self._lock: @@ -58,8 +56,7 @@ def add_position(self, pose: PoseStamped) -> None: self._size = min(self._size + 1, self._max_points) def _get_recent_positions(self) -> NDArray[np.float32]: - now = time.time() - cutoff = now - self._time_window + cutoff = time.time() - self._time_window if self._size == 0: return np.empty((0, 2), dtype=np.float32) @@ -75,13 +72,6 @@ def _get_recent_positions(self) -> NDArray[np.float32]: def is_stuck(self) -> bool: with self._lock: - last_check = self._last_check - - if time.perf_counter() - last_check < self._time_window: - return False - - with self._lock: - self._last_check = time.perf_counter() recent = self._get_recent_positions() if len(recent) == 0: From 7895deae77c6bb7d55029036cb33ceea4bc0ae48 Mon Sep 17 00:00:00 2001 From: Paul Nechifor Date: Fri, 19 Dec 2025 07:37:35 +0200 Subject: [PATCH 34/42] reduce speed --- dimos/navigation/replanning_a_star/local_planner.py | 10 +++------- 1 file changed, 3 insertions(+), 7 deletions(-) diff --git a/dimos/navigation/replanning_a_star/local_planner.py b/dimos/navigation/replanning_a_star/local_planner.py index d8c455075c..cdb6ba6919 100644 --- a/dimos/navigation/replanning_a_star/local_planner.py +++ b/dimos/navigation/replanning_a_star/local_planner.py @@ -63,7 +63,7 @@ class LocalPlanner(Resource): _global_config: GlobalConfig _navigation_map: NavigationMap - _speed: float = 1.0 + _speed: float = 0.55 _k_angular: float = 0.5 _k_derivative: float = 0.15 _max_angular_accel: float = 2.0 @@ -353,18 +353,14 @@ def _make_debug_navigation_image(self, path: Path, path_clearance: PathClearance 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 + 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 - ) + 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 From 610fce50cc76fedd60d3522514c400d742ad30d2 Mon Sep 17 00:00:00 2001 From: Paul Nechifor Date: Fri, 19 Dec 2025 07:39:22 +0200 Subject: [PATCH 35/42] remove assertion which does not work in CI --- dimos/navigation/global_planner/test_astar.py | 1 - 1 file changed, 1 deletion(-) diff --git a/dimos/navigation/global_planner/test_astar.py b/dimos/navigation/global_planner/test_astar.py index fd5784a3af..6fec9220b9 100644 --- a/dimos/navigation/global_planner/test_astar.py +++ b/dimos/navigation/global_planner/test_astar.py @@ -93,7 +93,6 @@ def test_astar_python_and_cpp(costmap) -> None: times_better = elapsed_time_python / elapsed_time_cpp print(f"astar C++ is {times_better:.2f} times faster than Python") - assert times_better > 5, "there's an issue C++ if less than 5x faster" # Assert that both implementations return almost identical points. np.testing.assert_allclose( From 2cbf19bfcd8a9aa29e91f76e5f6899ab0c0f8ddf Mon Sep 17 00:00:00 2001 From: Paul Nechifor Date: Sat, 20 Dec 2025 07:29:05 +0200 Subject: [PATCH 36/42] add minimum velocity --- .../replanning_a_star/local_planner.py | 42 ++++++++++++++++++- .../replanning_a_star/position_tracker.py | 2 +- 2 files changed, 41 insertions(+), 3 deletions(-) diff --git a/dimos/navigation/replanning_a_star/local_planner.py b/dimos/navigation/replanning_a_star/local_planner.py index cdb6ba6919..4701008a8a 100644 --- a/dimos/navigation/replanning_a_star/local_planner.py +++ b/dimos/navigation/replanning_a_star/local_planner.py @@ -64,11 +64,13 @@ class LocalPlanner(Resource): _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.2 + _orientation_tolerance: float = 0.35 _control_frequency: float = 10.0 _rotation_threshold: float = 90 @@ -161,11 +163,28 @@ def _loop(self) -> None: with self._lock: path = self._path path_clearance = self._path_clearance - self._change_state("initial_rotation") + 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 + 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: + with self._lock: + self._change_state("path_following") + else: + with self._lock: + self._change_state("initial_rotation") + else: + with self._lock: + self._change_state("initial_rotation") + while not stop_event.is_set(): start_time = time.perf_counter() @@ -178,6 +197,7 @@ def _loop(self) -> None: ) if path_clearance.is_obstacle_ahead(): + logger.info("Obstacle detected ahead, stopping local planner.") self.stopped_navigating.on_next("obstacle_found") break @@ -197,12 +217,24 @@ def _loop(self) -> None: cmd_vel = None if cmd_vel is not None: + print(cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.angular.z) 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 @@ -221,6 +253,9 @@ def _compute_angular_velocity(self, yaw_error: float, max_speed: float) -> float # 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 @@ -240,6 +275,8 @@ def _compute_initial_rotation(self) -> Twist: robot_yaw = current_odom.orientation.euler[2] yaw_error = normalize_angle(first_yaw - robot_yaw) + print("yaw_error", yaw_error) + if abs(yaw_error) < self._orientation_tolerance: with self._lock: self._change_state("path_following") @@ -295,6 +332,7 @@ def _compute_path_following(self) -> Twist: # 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), diff --git a/dimos/navigation/replanning_a_star/position_tracker.py b/dimos/navigation/replanning_a_star/position_tracker.py index 1f94c113b2..fb1fd508a5 100644 --- a/dimos/navigation/replanning_a_star/position_tracker.py +++ b/dimos/navigation/replanning_a_star/position_tracker.py @@ -37,7 +37,7 @@ class PositionTracker: def __init__(self, time_window: float) -> None: self._lock = RLock() self._time_window = time_window - self._threshold = 0.5 + self._threshold = 0.4 self._max_points = int(_max_points_per_second * self._time_window) self.reset_data() From 62e75997050863c22d2dcbf445dbc26795cfb0a1 Mon Sep 17 00:00:00 2001 From: Paul Nechifor Date: Sat, 20 Dec 2025 07:34:42 +0200 Subject: [PATCH 37/42] fix --- data/.lfs/make_path_mask_full.png.tar.gz | 4 ++-- data/.lfs/make_path_mask_two_meters.png.tar.gz | 4 ++-- dimos/mapping/occupancy/test_path_mask.py | 3 +-- 3 files changed, 5 insertions(+), 6 deletions(-) diff --git a/data/.lfs/make_path_mask_full.png.tar.gz b/data/.lfs/make_path_mask_full.png.tar.gz index f4093a2080..0e9336aaea 100644 --- a/data/.lfs/make_path_mask_full.png.tar.gz +++ b/data/.lfs/make_path_mask_full.png.tar.gz @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:dbfedcccd0f284bc66156d30d664d392456fd03d0438a5fa4e05c23af1eb698c -size 11231 +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 index 3ac1f4bbf4..7fa9e767b8 100644 --- a/data/.lfs/make_path_mask_two_meters.png.tar.gz +++ b/data/.lfs/make_path_mask_two_meters.png.tar.gz @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:24f618bb9f07685d9f21663ace62dec70d9b79e7ea1ef61f07a30e3475f0d0a6 -size 11408 +oid sha256:da608d410f4a1afee0965abfac814bc05267bdde31b0d3a9622c39515ee4f813 +size 11395 diff --git a/dimos/mapping/occupancy/test_path_mask.py b/dimos/mapping/occupancy/test_path_mask.py index bf5cc7d3ac..16a519755b 100644 --- a/dimos/mapping/occupancy/test_path_mask.py +++ b/dimos/mapping/occupancy/test_path_mask.py @@ -37,13 +37,12 @@ def test_make_path_mask(occupancy_gradient, pose_index, max_length, expected_ima 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) + 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] - actual.save(f"data/{expected_image}") np.testing.assert_array_equal(actual.data, expected.data) From 24ee6dd8e7cd5fbe74810b932d355eacc9fd6e5e Mon Sep 17 00:00:00 2001 From: Paul Nechifor Date: Sat, 20 Dec 2025 07:48:26 +0200 Subject: [PATCH 38/42] remove duplicate --- dimos/navigation/global_planner/astar.py | 1 - 1 file changed, 1 deletion(-) diff --git a/dimos/navigation/global_planner/astar.py b/dimos/navigation/global_planner/astar.py index c9159e40cd..1593004ab2 100644 --- a/dimos/navigation/global_planner/astar.py +++ b/dimos/navigation/global_planner/astar.py @@ -14,7 +14,6 @@ from dimos.msgs.geometry_msgs import VectorLike from dimos.msgs.nav_msgs import OccupancyGrid, Path -from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid 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 435b1312d9240bbffd67398e78be8da33556daf3 Mon Sep 17 00:00:00 2001 From: Paul Nechifor Date: Sat, 20 Dec 2025 08:08:33 +0200 Subject: [PATCH 39/42] check if veers off track --- .../replanning_a_star/global_planner.py | 15 ++++++++++++++- .../navigation/replanning_a_star/local_planner.py | 12 ++++++++++++ .../replanning_a_star/path_distancer.py | 4 ++++ 3 files changed, 30 insertions(+), 1 deletion(-) diff --git a/dimos/navigation/replanning_a_star/global_planner.py b/dimos/navigation/replanning_a_star/global_planner.py index 9fab3ddcb2..56d429f242 100644 --- a/dimos/navigation/replanning_a_star/global_planner.py +++ b/dimos/navigation/replanning_a_star/global_planner.py @@ -61,6 +61,7 @@ class GlobalPlanner(Resource): _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() @@ -146,7 +147,7 @@ def debug_navigation(self) -> Subject[Image]: return self._local_planner.debug_navigation def _thread_entrypoint(self) -> None: - """Monitor if the robot is stuck.""" + """Monitor if the robot is stuck or veers off track.""" last_id = -1 last_time = time.perf_counter() @@ -166,6 +167,18 @@ def _thread_entrypoint(self) -> None: 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_time = time.perf_counter() + continue + _, new_id = self._local_planner.get_unique_state() if new_id != last_id: diff --git a/dimos/navigation/replanning_a_star/local_planner.py b/dimos/navigation/replanning_a_star/local_planner.py index 4701008a8a..922b73b807 100644 --- a/dimos/navigation/replanning_a_star/local_planner.py +++ b/dimos/navigation/replanning_a_star/local_planner.py @@ -289,6 +289,18 @@ def _compute_initial_rotation(self) -> Twist: 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 diff --git a/dimos/navigation/replanning_a_star/path_distancer.py b/dimos/navigation/replanning_a_star/path_distancer.py index 7d4f537530..9f0e623c64 100644 --- a/dimos/navigation/replanning_a_star/path_distancer.py +++ b/dimos/navigation/replanning_a_star/path_distancer.py @@ -65,6 +65,10 @@ def find_lookahead_point(self, start_idx: int) -> NDArray[np.float64]: 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) From 95148bae7ed613a35175ee20399d156dfc2f3c6a Mon Sep 17 00:00:00 2001 From: Paul Nechifor Date: Sun, 21 Dec 2025 04:04:03 +0200 Subject: [PATCH 40/42] safety issues --- .../replanning_a_star/global_planner.py | 2 +- .../replanning_a_star/local_planner.py | 26 +++++++++--------- .../replanning_a_star/path_clearance.py | 27 ++++++++++++------- .../replanning_a_star/position_tracker.py | 2 +- 4 files changed, 32 insertions(+), 25 deletions(-) diff --git a/dimos/navigation/replanning_a_star/global_planner.py b/dimos/navigation/replanning_a_star/global_planner.py index 56d429f242..15c4dc33d9 100644 --- a/dimos/navigation/replanning_a_star/global_planner.py +++ b/dimos/navigation/replanning_a_star/global_planner.py @@ -285,7 +285,7 @@ def _find_wide_path(self, goal: Vector3, robot_pos: Vector3) -> Path | None: 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: + if path and path.poses: logger.info(f"Found path {size}x robot width.") return path diff --git a/dimos/navigation/replanning_a_star/local_planner.py b/dimos/navigation/replanning_a_star/local_planner.py index 922b73b807..fb84198681 100644 --- a/dimos/navigation/replanning_a_star/local_planner.py +++ b/dimos/navigation/replanning_a_star/local_planner.py @@ -168,28 +168,25 @@ def _loop(self) -> None: 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 + # Determine initial state: skip initial_rotation if already aligned. + new_state = "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: - with self._lock: - self._change_state("path_following") - else: - with self._lock: - self._change_state("initial_rotation") - else: - with self._lock: - self._change_state("initial_rotation") + new_state = "path_following" + + with self._lock: + self._change_state(new_state) while not stop_event.is_set(): start_time = time.perf_counter() - path_clearance.update_costmap(self._navigation_map.binary_costmap) - path_clearance.update_pose_index(self._pose_index) + 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( @@ -318,7 +315,10 @@ def _compute_path_following(self) -> Twist: return self._compute_final_rotation() closest_index = path_distancer.find_closest_point_index(current_pos) - self._pose_index = closest_index + + with self._lock: + self._pose_index = closest_index + lookahead_point = path_distancer.find_lookahead_point(closest_index) direction = lookahead_point - current_pos diff --git a/dimos/navigation/replanning_a_star/path_clearance.py b/dimos/navigation/replanning_a_star/path_clearance.py index 2251659840..52897de7fc 100644 --- a/dimos/navigation/replanning_a_star/path_clearance.py +++ b/dimos/navigation/replanning_a_star/path_clearance.py @@ -52,34 +52,41 @@ def update_pose_index(self, index: int) -> None: @property def mask(self) -> NDArray[np.bool_]: - assert self._costmap is not None + 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 self._costmap.grid.shape == self._last_used_shape - and self._pose_distance(self._last_used_pose, self._pose_index) - < self._max_distance_cache + 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=self._costmap, + occupancy_grid=costmap, path=self._path, robot_width=self._global_config.robot_width, - pose_index=self._pose_index, + pose_index=pose_index, max_length=self._path_lookup_distance, ) - self._last_used_shape = self._costmap.grid.shape - self._last_used_pose = self._pose_index + self._last_used_shape = costmap.grid.shape + self._last_used_pose = pose_index return self._last_mask def is_obstacle_ahead(self) -> bool: - if self._costmap is None: + with self._lock: + costmap = self._costmap + + if costmap is None: return True - return bool(np.any(self._costmap.grid[self.mask] == CostValues.OCCUPIED)) + + 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 diff --git a/dimos/navigation/replanning_a_star/position_tracker.py b/dimos/navigation/replanning_a_star/position_tracker.py index fb1fd508a5..66b069684f 100644 --- a/dimos/navigation/replanning_a_star/position_tracker.py +++ b/dimos/navigation/replanning_a_star/position_tracker.py @@ -50,7 +50,7 @@ def reset_data(self) -> None: def add_position(self, pose: PoseStamped) -> None: with self._lock: - self._timestamps[self._index] = pose.ts + 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) From 4655f41dbbe46b9a289635cd58f7da12ba3fbb9c Mon Sep 17 00:00:00 2001 From: Paul Nechifor Date: Sun, 21 Dec 2025 04:16:36 +0200 Subject: [PATCH 41/42] fix threading issue --- .../replanning_a_star/global_planner.py | 53 +++++++++++++++---- .../replanning_a_star/local_planner.py | 2 +- 2 files changed, 43 insertions(+), 12 deletions(-) diff --git a/dimos/navigation/replanning_a_star/global_planner.py b/dimos/navigation/replanning_a_star/global_planner.py index 15c4dc33d9..efe4f814b3 100644 --- a/dimos/navigation/replanning_a_star/global_planner.py +++ b/dimos/navigation/replanning_a_star/global_planner.py @@ -54,6 +54,8 @@ class GlobalPlanner(Resource): _position_tracker: PositionTracker _disposables: CompositeDisposable _stop_planner: Event + _replan_event: Event + _replan_reason: StopMessage | None _lock: RLock _replan_attempt: int @@ -73,6 +75,8 @@ def __init__(self, global_config: GlobalConfig) -> None: 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 @@ -90,6 +94,7 @@ def stop(self) -> None: 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) @@ -147,13 +152,29 @@ def debug_navigation(self) -> Subject[Image]: return self._local_planner.debug_navigation def _thread_entrypoint(self) -> None: - """Monitor if the robot is stuck or veers off track.""" + """Monitor if the robot is stuck, veers off track, or stopped navigating.""" last_id = -1 - last_time = time.perf_counter() + last_stuck_check = time.perf_counter() while not self._stop_planner.is_set(): - self._stop_planner.wait(0.1) + # 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 @@ -176,32 +197,42 @@ def _thread_entrypoint(self) -> None: threshold=self._max_path_deviation, ) self._replan_path() - last_time = time.perf_counter() + 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_time = time.perf_counter() + last_stuck_check = time.perf_counter() continue if ( - time.perf_counter() - last_time > self._stuck_time_window + 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_time = time.perf_counter() + 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 == "obstacle_found": - logger.info("Replanning path due to obstacle found.") - self._replan_path() - elif stop_message == "arrived": + + 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() diff --git a/dimos/navigation/replanning_a_star/local_planner.py b/dimos/navigation/replanning_a_star/local_planner.py index fb84198681..3fb30d1454 100644 --- a/dimos/navigation/replanning_a_star/local_planner.py +++ b/dimos/navigation/replanning_a_star/local_planner.py @@ -169,7 +169,7 @@ def _loop(self) -> None: raise RuntimeError("No path set for local planner.") # Determine initial state: skip initial_rotation if already aligned. - new_state = "initial_rotation" + 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] From d1ec605a55f0079c841728ca83ab9e23cd7023ce Mon Sep 17 00:00:00 2001 From: Paul Nechifor Date: Mon, 22 Dec 2025 14:41:58 +0200 Subject: [PATCH 42/42] remove prints --- dimos/navigation/replanning_a_star/local_planner.py | 3 --- 1 file changed, 3 deletions(-) diff --git a/dimos/navigation/replanning_a_star/local_planner.py b/dimos/navigation/replanning_a_star/local_planner.py index 3fb30d1454..f31c5e2e4a 100644 --- a/dimos/navigation/replanning_a_star/local_planner.py +++ b/dimos/navigation/replanning_a_star/local_planner.py @@ -214,7 +214,6 @@ def _loop(self) -> None: cmd_vel = None if cmd_vel is not None: - print(cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.angular.z) self.cmd_vel.on_next(cmd_vel) elapsed = time.perf_counter() - start_time @@ -272,8 +271,6 @@ def _compute_initial_rotation(self) -> Twist: robot_yaw = current_odom.orientation.euler[2] yaw_error = normalize_angle(first_yaw - robot_yaw) - print("yaw_error", yaw_error) - if abs(yaw_error) < self._orientation_tolerance: with self._lock: self._change_state("path_following")