diff --git a/data/.lfs/unitree_go2_bigoffice.tar.gz b/data/.lfs/unitree_go2_bigoffice.tar.gz new file mode 100644 index 0000000000..6582702479 --- /dev/null +++ b/data/.lfs/unitree_go2_bigoffice.tar.gz @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:3a009674153f7ee1f98219af69dc7a92d063f2581bfd9b0aa019762c9235895c +size 2312982327 diff --git a/dimos/core/global_config.py b/dimos/core/global_config.py index f4df745f08..6fca91c8d3 100644 --- a/dimos/core/global_config.py +++ b/dimos/core/global_config.py @@ -68,5 +68,5 @@ def mujoco_start_pos_float(self) -> tuple[float, float]: @cached_property def mujoco_camera_position_float(self) -> tuple[float, ...]: if self.mujoco_camera_position is None: - return [-0.906, 0.008, 1.101, 4.931, 89.749, -46.378] + return (-0.906, 0.008, 1.101, 4.931, 89.749, -46.378) return tuple(_get_all_numbers(self.mujoco_camera_position)) diff --git a/dimos/mapping/costmapper.py b/dimos/mapping/costmapper.py new file mode 100644 index 0000000000..98a6d6e66e --- /dev/null +++ b/dimos/mapping/costmapper.py @@ -0,0 +1,67 @@ +# 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 dataclasses import asdict, dataclass, field + +from reactivex import operators as ops + +from dimos.core import In, Module, Out, rpc +from dimos.core.module import ModuleConfig +from dimos.mapping.pointclouds.occupancy import ( + OCCUPANCY_ALGOS, + HeightCostConfig, + OccupancyConfig, +) +from dimos.msgs.nav_msgs import OccupancyGrid +from dimos.robot.unitree_webrtc.type.lidar import LidarMessage +from dimos.utils.reactive import backpressure + + +@dataclass +class Config(ModuleConfig): + algo: str = "height_cost" + config: OccupancyConfig = field(default_factory=HeightCostConfig) + + +class CostMapper(Module): + default_config = Config + config: Config + + global_map: In[LidarMessage] + global_costmap: Out[OccupancyGrid] + + @rpc + def start(self) -> None: + super().start() + + self._disposables.add( + backpressure( + self.global_map.observable() # type: ignore[no-untyped-call] + ) + .pipe(ops.map(self._calculate_costmap)) + .subscribe( + self.global_costmap.publish, + ) + ) + + @rpc + def stop(self) -> None: + super().stop() + + def _calculate_costmap(self, msg: LidarMessage) -> OccupancyGrid: + fn = OCCUPANCY_ALGOS[self.config.algo] + return fn(msg, **asdict(self.config.config)) + + +cost_mapper = CostMapper.blueprint diff --git a/dimos/mapping/pointclouds/occupancy.py b/dimos/mapping/pointclouds/occupancy.py index 6b4180ab03..9d9519c702 100644 --- a/dimos/mapping/pointclouds/occupancy.py +++ b/dimos/mapping/pointclouds/occupancy.py @@ -14,7 +14,8 @@ from __future__ import annotations -from typing import TYPE_CHECKING +from dataclasses import dataclass +from typing import TYPE_CHECKING, Any, Protocol, TypeVar import numpy as np from scipy import ndimage # type: ignore[import-untyped] @@ -23,18 +24,50 @@ from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid if TYPE_CHECKING: + from collections.abc import Callable + from dimos.msgs.sensor_msgs import PointCloud2 -def height_cost_occupancy( - cloud: PointCloud2, - resolution: float = 0.05, - can_pass_under: float = 0.6, - can_climb: float = 0.15, - ignore_noise: float = 0.05, - smoothing: float = 1.0, - frame_id: str | None = None, -) -> OccupancyGrid: +@dataclass(frozen=True) +class OccupancyConfig: + """Base config for all occupancy grid generators.""" + + resolution: float = 0.05 + frame_id: str | None = None + + +ConfigT = TypeVar("ConfigT", bound=OccupancyConfig, covariant=True) + + +class OccupancyFn(Protocol[ConfigT]): + """Protocol for pointcloud-to-occupancy conversion functions. + + Functions matching this protocol take a PointCloud2 and config kwargs, + returning an OccupancyGrid. Call with: fn(cloud, resolution=0.1, ...) + """ + + @property + def config_class(self) -> type[ConfigT]: ... + + def __call__(self, cloud: PointCloud2, **kwargs: Any) -> OccupancyGrid: ... + + +# Populated after function definitions below +OCCUPANCY_ALGOS: dict[str, Callable[..., OccupancyGrid]] = {} + + +@dataclass(frozen=True) +class HeightCostConfig(OccupancyConfig): + """Config for height-cost based occupancy (terrain slope analysis).""" + + can_pass_under: float = 0.6 + can_climb: float = 0.15 + ignore_noise: float = 0.05 + smoothing: float = 1.0 + + +def height_cost_occupancy(cloud: PointCloud2, **kwargs: Any) -> OccupancyGrid: """Create a costmap based on terrain slope (rate of change of height). Costs are assigned based on the gradient magnitude of the terrain height. @@ -43,21 +76,22 @@ def height_cost_occupancy( Args: cloud: PointCloud2 message containing 3D points - resolution: Grid resolution in meters/cell (default: 0.05) - can_pass_under: Max height to consider - points above are ignored (default: 0.6) - can_climb: Height change in meters that maps to cost 100 (default: 0.15) - smoothing: Gaussian smoothing sigma in cells for filling gaps (default: 1.0) - frame_id: Reference frame for the grid (default: uses cloud's frame_id) + **kwargs: HeightCostConfig fields - resolution, can_pass_under, can_climb, + ignore_noise, smoothing, frame_id Returns: OccupancyGrid with costs 0-100 based on terrain slope, -1 for unknown """ + cfg = HeightCostConfig(**kwargs) points = cloud.as_numpy() ts = cloud.ts if hasattr(cloud, "ts") and cloud.ts is not None else 0.0 if len(points) == 0: return OccupancyGrid( - width=1, height=1, resolution=resolution, frame_id=frame_id or cloud.frame_id + width=1, + height=1, + resolution=cfg.resolution, + frame_id=cfg.frame_id or cloud.frame_id, ) # Find bounds of the point cloud in X-Y plane (use all points) @@ -74,8 +108,8 @@ def height_cost_occupancy( max_y += padding # Calculate grid dimensions - width = int(np.ceil((max_x - min_x) / resolution)) - height = int(np.ceil((max_y - min_y) / resolution)) + width = int(np.ceil((max_x - min_x) / cfg.resolution)) + height = int(np.ceil((max_y - min_y) / cfg.resolution)) # Create origin pose origin = Pose() @@ -90,8 +124,8 @@ def height_cost_occupancy( max_height_map = np.full((height, width), np.nan, dtype=np.float32) # Convert point XY to grid indices - grid_x = np.round((points[:, 0] - min_x) / resolution).astype(np.int32) - grid_y = np.round((points[:, 1] - min_y) / resolution).astype(np.int32) + grid_x = np.round((points[:, 0] - min_x) / cfg.resolution).astype(np.int32) + grid_y = np.round((points[:, 1] - min_y) / cfg.resolution).astype(np.int32) # Clip to grid bounds grid_x = np.clip(grid_x, 0, width - 1) @@ -105,21 +139,21 @@ def height_cost_occupancy( # If gap between min and max > can_pass_under, robot can pass under - use min (ground) # Otherwise use max (solid obstacle) height_gap = max_height_map - min_height_map - height_map = np.where(height_gap > can_pass_under, min_height_map, max_height_map) + height_map = np.where(height_gap > cfg.can_pass_under, min_height_map, max_height_map) # Track which cells have observations observed_mask = ~np.isnan(height_map) # Step 3: Apply smoothing to fill gaps while preserving unknown space - if smoothing > 0 and np.any(observed_mask): + if cfg.smoothing > 0 and np.any(observed_mask): # Use a weighted smoothing approach that only interpolates from known cells # Create a weight map (1 for observed, 0 for unknown) weights = observed_mask.astype(np.float32) height_map_filled = np.where(observed_mask, height_map, 0.0) # Smooth both height values and weights - smoothed_heights = ndimage.gaussian_filter(height_map_filled, sigma=smoothing) - smoothed_weights = ndimage.gaussian_filter(weights, sigma=smoothing) + smoothed_heights = ndimage.gaussian_filter(height_map_filled, sigma=cfg.smoothing) + smoothed_weights = ndimage.gaussian_filter(weights, sigma=cfg.smoothing) # Avoid division by zero (use np.divide with where to prevent warning) valid_smooth = smoothed_weights > 0.01 @@ -139,22 +173,22 @@ def height_cost_occupancy( height_for_grad = np.where(observed_mask, height_map, 0.0) # Calculate gradients (Sobel gives gradient in pixels, scale by resolution) - grad_x = ndimage.sobel(height_for_grad, axis=1) / (8.0 * resolution) - grad_y = ndimage.sobel(height_for_grad, axis=0) / (8.0 * resolution) + grad_x = ndimage.sobel(height_for_grad, axis=1) / (8.0 * cfg.resolution) + grad_y = ndimage.sobel(height_for_grad, axis=0) / (8.0 * cfg.resolution) # Gradient magnitude = height change per meter gradient_magnitude = np.sqrt(grad_x**2 + grad_y**2) # Map gradient to cost: can_climb height change over one cell maps to cost 100 # gradient_magnitude is in m/m, so multiply by resolution to get height change per cell - height_change_per_cell = gradient_magnitude * resolution + height_change_per_cell = gradient_magnitude * cfg.resolution # Ignore height changes below noise threshold (lidar floor noise) height_change_per_cell = np.where( - height_change_per_cell < ignore_noise, 0.0, height_change_per_cell + height_change_per_cell < cfg.ignore_noise, 0.0, height_change_per_cell ) - cost_float = (height_change_per_cell / can_climb) * 100.0 + cost_float = (height_change_per_cell / cfg.can_climb) * 100.0 cost_float = np.clip(cost_float, 0, 100) # Erode observed mask - only trust gradients where all neighbors are observed @@ -169,65 +203,58 @@ def height_cost_occupancy( return OccupancyGrid( grid=cost, - resolution=resolution, + resolution=cfg.resolution, origin=origin, - frame_id=frame_id or cloud.frame_id, + frame_id=cfg.frame_id or cloud.frame_id, ts=ts, ) -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: +@dataclass(frozen=True) +class GeneralOccupancyConfig(OccupancyConfig): + """Config for general obstacle-based occupancy.""" + + min_height: float = 0.1 + max_height: float = 2.0 + mark_free_radius: float = 0.4 + + +# can remove, just needs pulling out of unitree type/map.py +def general_occupancy(cloud: PointCloud2, **kwargs: Any) -> 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. + **kwargs: GeneralOccupancyConfig fields - resolution, min_height, max_height, + frame_id, mark_free_radius Returns: OccupancyGrid with occupied cells where points were projected """ - - # Get points as numpy array + cfg = GeneralOccupancyConfig(**kwargs) 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 + width=1, + height=1, + resolution=cfg.resolution, + frame_id=cfg.frame_id or cloud.frame_id, ) # Filter points by height for obstacles - obstacle_mask = (points[:, 2] >= min_height) & (points[:, 2] <= max_height) + obstacle_mask = (points[:, 2] >= cfg.min_height) & (points[:, 2] <= cfg.max_height) obstacle_points = points[obstacle_mask] # Get points below min_height for marking as free space - ground_mask = points[:, 2] < min_height + ground_mask = points[:, 2] < cfg.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 - ) + min_x = np.min(points[:, 0]) + max_x = np.max(points[:, 0]) + min_y = np.min(points[:, 1]) + max_y = np.max(points[:, 1]) # Add some padding around the bounds padding = 1.0 # 1 meter padding @@ -237,8 +264,8 @@ def general_occupancy( max_y += padding # Calculate grid dimensions - width = int(np.ceil((max_x - min_x) / resolution)) - height = int(np.ceil((max_y - min_y) / resolution)) + width = int(np.ceil((max_x - min_x) / cfg.resolution)) + height = int(np.ceil((max_y - min_y) / cfg.resolution)) # Create origin pose (bottom-left corner of the grid) origin = Pose() @@ -252,8 +279,8 @@ def general_occupancy( # 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) + ground_x = ((ground_points[:, 0] - min_x) / cfg.resolution).astype(np.int32) + ground_y = ((ground_points[:, 1] - min_y) / cfg.resolution).astype(np.int32) # Clip indices to grid bounds ground_x = np.clip(ground_x, 0, width - 1) @@ -264,8 +291,8 @@ def general_occupancy( # 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) + obs_x = ((obstacle_points[:, 0] - min_x) / cfg.resolution).astype(np.int32) + obs_y = ((obstacle_points[:, 1] - min_y) / cfg.resolution).astype(np.int32) # Clip indices to grid bounds obs_x = np.clip(obs_x, 0, width - 1) @@ -275,12 +302,12 @@ def general_occupancy( grid[obs_y, obs_x] = 100 # Lethal obstacle # Apply mark_free_radius to expand free space areas - if mark_free_radius > 0: + if cfg.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)) + free_radius_cells = int(np.ceil(cfg.mark_free_radius / cfg.resolution)) # Create circular kernel y, x = np.ogrid[ @@ -299,52 +326,60 @@ def general_occupancy( # 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( + return OccupancyGrid( grid=grid, - resolution=resolution, + resolution=cfg.resolution, origin=origin, - frame_id=frame_id or cloud.frame_id, + frame_id=cfg.frame_id or cloud.frame_id, ts=ts, ) - return occupancy_grid +@dataclass(frozen=True) +class SimpleOccupancyConfig(OccupancyConfig): + """Config for simple occupancy with morphological closing.""" + + min_height: float = 0.1 + max_height: float = 2.0 + closing_iterations: int = 1 + closing_connectivity: int = 2 + + +def simple_occupancy(cloud: PointCloud2, **kwargs: Any) -> OccupancyGrid: + """Create a simple occupancy grid with morphological closing. + + Args: + cloud: PointCloud2 message containing 3D points + **kwargs: SimpleOccupancyConfig fields - resolution, min_height, max_height, + frame_id, closing_iterations, closing_connectivity -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: + Returns: + OccupancyGrid with occupied/free cells + """ + cfg = SimpleOccupancyConfig(**kwargs) points = cloud.as_numpy() if len(points) == 0: return OccupancyGrid( - width=1, height=1, resolution=resolution, frame_id=frame_id or cloud.frame_id + width=1, + height=1, + resolution=cfg.resolution, + frame_id=cfg.frame_id or cloud.frame_id, ) # Filter points by height for obstacles - obstacle_mask = (points[:, 2] >= min_height) & (points[:, 2] <= max_height) + obstacle_mask = (points[:, 2] >= cfg.min_height) & (points[:, 2] <= cfg.max_height) obstacle_points = points[obstacle_mask] # Get points below min_height for marking as free space - ground_mask = points[:, 2] < min_height + ground_mask = points[:, 2] < cfg.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 - ) + min_x = np.min(points[:, 0]) + max_x = np.max(points[:, 0]) + min_y = np.min(points[:, 1]) + max_y = np.max(points[:, 1]) # Add some padding around the bounds padding = 1.0 # 1 meter padding @@ -354,8 +389,8 @@ def simple_occupancy( max_y += padding # Calculate grid dimensions - width = int(np.ceil((max_x - min_x) / resolution)) - height = int(np.ceil((max_y - min_y) / resolution)) + width = int(np.ceil((max_x - min_x) / cfg.resolution)) + height = int(np.ceil((max_y - min_y) / cfg.resolution)) # Create origin pose (bottom-left corner of the grid) origin = Pose() @@ -369,8 +404,8 @@ def simple_occupancy( # 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) + ground_x = np.round((ground_points[:, 0] - min_x) / cfg.resolution).astype(np.int32) + ground_y = np.round((ground_points[:, 1] - min_y) / cfg.resolution).astype(np.int32) # Clip indices to grid bounds ground_x = np.clip(ground_x, 0, width - 1) @@ -381,8 +416,8 @@ def simple_occupancy( # 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) + obs_x = np.round((obstacle_points[:, 0] - min_x) / cfg.resolution).astype(np.int32) + obs_y = np.round((obstacle_points[:, 1] - min_y) / cfg.resolution).astype(np.int32) # Clip indices to grid bounds obs_x = np.clip(obs_x, 0, width - 1) @@ -393,12 +428,12 @@ def simple_occupancy( # Fill small gaps in occupied regions using morphological closing occupied_mask = grid == 100 - if np.any(occupied_mask) and closing_iterations > 0: + if np.any(occupied_mask) and cfg.closing_iterations > 0: # connectivity=1 gives 4-connectivity, connectivity=2 gives 8-connectivity - structure = ndimage.generate_binary_structure(2, closing_connectivity) + structure = ndimage.generate_binary_structure(2, cfg.closing_connectivity) # Closing = dilation then erosion - fills small holes closed_mask = ndimage.binary_closing( - occupied_mask, structure=structure, iterations=closing_iterations + occupied_mask, structure=structure, iterations=cfg.closing_iterations ) # Fill gaps (both unknown and free space) grid[closed_mask] = 100 @@ -407,12 +442,20 @@ def simple_occupancy( # 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( + return OccupancyGrid( grid=grid, - resolution=resolution, + resolution=cfg.resolution, origin=origin, - frame_id=frame_id or cloud.frame_id, + frame_id=cfg.frame_id or cloud.frame_id, ts=ts, ) - return occupancy_grid + +# Populate algorithm registry +OCCUPANCY_ALGOS.update( + { + "height_cost": height_cost_occupancy, + "general": general_occupancy, + "simple": simple_occupancy, + } +) diff --git a/dimos/mapping/test_voxels.py b/dimos/mapping/test_voxels.py index bbb63b1c10..321c2e2ce3 100644 --- a/dimos/mapping/test_voxels.py +++ b/dimos/mapping/test_voxels.py @@ -39,7 +39,6 @@ def mapper() -> Generator[VoxelGridMapper, None, None]: class Go2MapperMoment(Go2Moment): global_map: OutputMoment[PointCloud2] = OutputMoment(LCMTransport("/global_map", PointCloud2)) - costmap: OutputMoment[OccupancyGrid] = OutputMoment(LCMTransport("/costmap", OccupancyGrid)) MomentFactory = Callable[[float, bool], Go2MapperMoment] @@ -111,7 +110,6 @@ def test_carving( mapper.add_frame(lidar_frame2) moment2.global_map.set(mapper.get_global_pointcloud2()) - moment2.costmap.set(mapper.get_global_occupancygrid()) moment2.publish() count_carving = mapper.size() diff --git a/dimos/mapping/voxels.py b/dimos/mapping/voxels.py index b5d1d235ff..2e509228cd 100644 --- a/dimos/mapping/voxels.py +++ b/dimos/mapping/voxels.py @@ -13,35 +13,27 @@ # limitations under the License. from collections.abc import Callable -from dataclasses import dataclass, field +from dataclasses import dataclass import functools import time import numpy as np import open3d as o3d # type: ignore[import-untyped] import open3d.core as o3c # type: ignore[import-untyped] -from reactivex import interval +from reactivex import interval, operators as ops from reactivex.disposable import Disposable +from reactivex.subject import Subject from dimos.core import DimosCluster, In, LCMTransport, Module, Out, rpc from dimos.core.global_config import GlobalConfig from dimos.core.module import ModuleConfig -from dimos.mapping.pointclouds.occupancy import height_cost_occupancy -from dimos.msgs.nav_msgs import OccupancyGrid from dimos.msgs.sensor_msgs import PointCloud2 from dimos.robot.unitree.connection.go2 import Go2ConnectionProtocol from dimos.robot.unitree_webrtc.type.lidar import LidarMessage -from dimos.spec.map import Global3DMap, GlobalCostmap +from dimos.spec.map import Global3DMap from dimos.utils.decorators import simple_mcache from dimos.utils.metrics import timed - - -@dataclass -class CostmapConfig: - publish: bool = True - resolution: float = 0.05 - can_pass_under: float = 0.6 - can_climb: float = 0.15 +from dimos.utils.reactive import backpressure @dataclass @@ -52,7 +44,6 @@ class Config(ModuleConfig): block_count: int = 2_000_000 device: str = "CUDA:0" carve_columns: bool = True - costmap: CostmapConfig = field(default_factory=CostmapConfig) class VoxelGridMapper(Module): @@ -61,7 +52,6 @@ class VoxelGridMapper(Module): lidar: In[LidarMessage] global_map: Out[LidarMessage] - global_costmap: Out[OccupancyGrid] def __init__(self, **kwargs: object) -> None: super().__init__(**kwargs) @@ -91,15 +81,24 @@ def __init__(self, **kwargs: object) -> None: def start(self) -> None: super().start() + # Subject to trigger publishing, with backpressure to drop if busy + self._publish_trigger: Subject[None] = Subject() + self._disposables.add( + backpressure(self._publish_trigger) + .pipe(ops.map(lambda _: self.publish_global_map())) + .subscribe() + ) + lidar_unsub = self.lidar.subscribe(self._on_frame) self._disposables.add(Disposable(lidar_unsub)) # If publish_interval > 0, publish on timer; otherwise publish on each frame if self.config.publish_interval > 0: - disposable = interval(self.config.publish_interval).subscribe( - lambda _: self.publish_global_map() + self._disposables.add( + interval(self.config.publish_interval).subscribe( + lambda _: self._publish_trigger.on_next(None) + ) ) - self._disposables.add(disposable) @rpc def stop(self) -> None: @@ -108,12 +107,10 @@ def stop(self) -> None: def _on_frame(self, frame: LidarMessage) -> None: self.add_frame(frame) if self.config.publish_interval <= 0: - self.publish_global_map() + self._publish_trigger.on_next(None) def publish_global_map(self) -> None: self.global_map.publish(self.get_global_pointcloud2()) - if self.config.costmap.publish: - self.global_costmap.publish(self.get_global_occupancygrid()) def size(self) -> int: return self._voxel_hashmap.size() # type: ignore[no-any-return] @@ -140,7 +137,6 @@ def add_frame(self, frame: PointCloud2) -> None: self.get_global_pointcloud.invalidate_cache(self) # type: ignore[attr-defined] self.get_global_pointcloud2.invalidate_cache(self) # type: ignore[attr-defined] - self.get_global_occupancygrid.invalidate_cache(self) # type: ignore[attr-defined] def _carve_and_insert(self, new_keys: o3c.Tensor) -> None: """Column carving: remove all existing voxels sharing (X,Y) with new_keys, then insert.""" @@ -201,15 +197,6 @@ def get_global_pointcloud(self) -> o3d.t.geometry.PointCloud: out.point["positions"] = pts return out - @simple_mcache - def get_global_occupancygrid(self) -> OccupancyGrid: - return height_cost_occupancy( - self.get_global_pointcloud2(), - resolution=self.config.costmap.resolution, - can_pass_under=self.config.costmap.can_pass_under, - can_climb=self.config.costmap.can_climb, - ) - # @timed() def splice_cylinder( diff --git a/dimos/navigation/replanning_a_star/global_planner.py b/dimos/navigation/replanning_a_star/global_planner.py index efe4f814b3..b7d34ebd89 100644 --- a/dimos/navigation/replanning_a_star/global_planner.py +++ b/dimos/navigation/replanning_a_star/global_planner.py @@ -311,7 +311,8 @@ def _plan_path(self) -> None: 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] + # sizes_to_try: list[float] = [2.2, 1.7, 1.3, 1] + sizes_to_try: list[float] = [1.1] for size in sizes_to_try: costmap = self._navigation_map.make_gradient_costmap(size) diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index fd969f6d38..85f4271083 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -16,13 +16,10 @@ # The blueprints are defined as import strings so as not to trigger unnecessary imports. all_blueprints = { - "unitree-go2": "dimos.robot.unitree_webrtc.unitree_go2_blueprints:standard", - "unitree-go2-newmapper": "dimos.robot.unitree_webrtc.unitree_go2_blueprints:newmapper", + "unitree-go2": "dimos.robot.unitree_webrtc.unitree_go2_blueprints:nav", + "unitree-go2-nav": "dimos.robot.unitree_webrtc.unitree_go2_blueprints:nav", "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", + "unitree-go2-spatial": "dimos.robot.unitree_webrtc.unitree_go2_blueprints:spatial", "unitree-go2-agentic": "dimos.robot.unitree_webrtc.unitree_go2_blueprints:agentic", "unitree-go2-agentic-ollama": "dimos.robot.unitree_webrtc.unitree_go2_blueprints:agentic_ollama", "unitree-go2-agentic-huggingface": "dimos.robot.unitree_webrtc.unitree_go2_blueprints:agentic_huggingface", diff --git a/dimos/robot/unitree/connection/go2.py b/dimos/robot/unitree/connection/go2.py index 5fda73fba7..4760995a12 100644 --- a/dimos/robot/unitree/connection/go2.py +++ b/dimos/robot/unitree/connection/go2.py @@ -37,7 +37,7 @@ from dimos.utils.data import get_data from dimos.utils.decorators.decorators import simple_mcache from dimos.utils.logging_config import setup_logger -from dimos.utils.testing import TimedSensorReplay +from dimos.utils.testing import TimedSensorReplay, TimedSensorStorage logger = setup_logger(level=logging.INFO) @@ -75,7 +75,7 @@ def _camera_info_static() -> CameraInfo: class ReplayConnection(UnitreeWebRTCConnection): - dir_name = "unitree_go2_office_walk2" + dir_name = "unitree_go2_bigoffice" # we don't want UnitreeWebRTCConnection to init def __init__( # type: ignore[no-untyped-def] @@ -163,6 +163,17 @@ def __init__( # type: ignore[no-untyped-def] Module.__init__(self, *args, **kwargs) + @rpc + def record(self, recording_name: str) -> None: + lidar_store: TimedSensorStorage = TimedSensorStorage(f"{recording_name}/lidar") # type: ignore[type-arg] + lidar_store.save_stream(self.connection.lidar_stream()).subscribe(lambda x: x) # type: ignore[arg-type, attr-defined] + + odom_store: TimedSensorStorage = TimedSensorStorage(f"{recording_name}/odom") # type: ignore[type-arg] + odom_store.save_stream(self.connection.odom_stream()).subscribe(lambda x: x) # type: ignore[arg-type, attr-defined] + + video_store: TimedSensorStorage = TimedSensorStorage(f"{recording_name}/video") # type: ignore[type-arg] + video_store.save_stream(self.connection.video_stream()).subscribe(lambda x: x) # type: ignore[arg-type, attr-defined] + @rpc def start(self) -> None: super().start() @@ -181,6 +192,7 @@ def start(self) -> None: self._camera_info_thread.start() self.standup() + # self.record("go2_bigoffice") @rpc def stop(self) -> None: diff --git a/dimos/robot/unitree_webrtc/unitree_go2_blueprints.py b/dimos/robot/unitree_webrtc/unitree_go2_blueprints.py index fedd90c59c..d0da2bbdf9 100644 --- a/dimos/robot/unitree_webrtc/unitree_go2_blueprints.py +++ b/dimos/robot/unitree_webrtc/unitree_go2_blueprints.py @@ -28,6 +28,7 @@ from dimos.constants import DEFAULT_CAPACITY_COLOR_IMAGE from dimos.core.blueprints import autoconnect from dimos.core.transport import JpegLcmTransport, JpegShmTransport, LCMTransport, pSHMTransport +from dimos.mapping.costmapper import cost_mapper from dimos.mapping.voxels import voxel_mapper from dimos.msgs.geometry_msgs import PoseStamped from dimos.msgs.sensor_msgs import Image @@ -75,52 +76,33 @@ linux = autoconnect(foxglove_bridge()) basic = autoconnect( - linux if platform.system() == "Linux" else mac, - websocket_vis(), go2_connection(), - mapper(voxel_size=0.5, global_publish_interval=2.5), - astar_planner(), - holonomic_local_planner(), - behavior_tree_navigator(), - wavefront_frontier_explorer(), -).global_config(n_dask_workers=4, robot_model="unitree_go2") - - -newmapper = autoconnect( linux if platform.system() == "Linux" else mac, - go2_connection(), - # these values are defaults but leaving here for clarity - # - # no publish interval - publishes immediately on each lidar frame - # voxel size same as input - voxel_mapper(voxel_size=0.05, publish_interval=0), + websocket_vis(), ).global_config(n_dask_workers=4, robot_model="unitree_go2") - -standard = autoconnect( +nav = autoconnect( basic, + voxel_mapper(voxel_size=0.05), + cost_mapper(), + replanning_a_star_planner(), + wavefront_frontier_explorer(), +).global_config(n_dask_workers=6, robot_model="unitree_go2") + +spatial = autoconnect( + nav, spatial_memory(), - object_tracking(frame_id="camera_link"), utilization(), ).global_config(n_dask_workers=8) -test_new_nav = autoconnect( - go2_connection(), - voxel_mapper(voxel_size=0.05, publish_interval=0), - replanning_a_star_planner(), - wavefront_frontier_explorer(), - websocket_vis(), - foxglove_bridge(), -).global_config(n_dask_workers=4, robot_model="unitree_go2") - -standard_with_jpeglcm = standard.transports( +with_jpeglcm = nav.transports( { ("color_image", Image): JpegLcmTransport("/color_image", Image), } ) -standard_with_jpegshm = autoconnect( - standard.transports( +with_jpegshm = autoconnect( + nav.transports( { ("color_image", Image): JpegShmTransport("/color_image", quality=75), } @@ -141,13 +123,13 @@ ) agentic = autoconnect( - standard, + spatial, llm_agent(), _common_agentic, ) agentic_ollama = autoconnect( - standard, + spatial, llm_agent( model="qwen3:8b", provider=Provider.OLLAMA, # type: ignore[attr-defined] @@ -158,7 +140,7 @@ ) agentic_huggingface = autoconnect( - standard, + spatial, llm_agent( model="Qwen/Qwen2.5-1.5B-Instruct", provider=Provider.HUGGINGFACE, # type: ignore[attr-defined]