Deep dive

Go2 Non-ROS Navigation

The Go2 navigation stack runs entirely without ROS. It uses a column-carving voxel map strategy: each new LiDAR frame replaces the corresponding region of the global map entirely, ensuring the map always reflects the latest observations.

Data Flow

Pipeline Steps

1. LiDAR Frame — GO2Connection

We don’t connect to the LiDAR directly — instead we use Unitree’s WebRTC client (via legion’s webrtc driver), which streams a heavily preprocessed 5cm voxel grid rather than raw point cloud data. This allows us to support stock, unjailbroken Go2 Air and Pro models out of the box.

2. Global Voxel Map — VoxelGridMapper

The VoxelGridMapper maintains a sparse 3D occupancy grid using Open3D’s VoxelBlockGrid backed by a hash map. Each voxel is a 5cm cube by default.

Voxel hash map provides O(1) insert/erase/lookup, so this is efficient even with millions of voxels. The grid runs on CUDA by default for speed, with CPU fallback.

Each incoming LiDAR frame is spliced into the global map via column carving. We consider any previously mapped voxels in the space of a received LiDAR frame stale, by erasing entire Z-columns in the footprint, we guarantee:

• No ghost obstacles from previous passes
• Dynamic objects (people, doors) get cleared automatically
• The latest observation always wins

We don’t have proper loop closure and stable odometry, we trust the data go2 odom reports, which is surprisingly stable but does drift eventually, You will reliably map and nav through very large spaces (500sqm in our tests) but you won’t go down the street to a super market.

Configuration

Parameter	Default	Description
voxel_size	0.05	Voxel cube size in meters
block_count	2,000,000	Max voxels in hash map
device	CUDA:0	Compute device (CUDA:0 or CPU:0)
carve_columns	true	Enable column carving (disable for append-only mapping)
publish_interval	0	Seconds between map publishes (0 = every frame)

3. Global Costmap — CostMapper

The CostMapper converts the 3D voxel map into a 2D occupancy grid. The default algorithm (height_cost) maps rate of change of Z, with some smoothing.

algo settings are in occupancy.py and can be configured per robot

Configuration

skip

@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

Cost	Meaning
0	Flat, easy to traverse
50	Moderate slope (~7.5cm rise per cell in case of go2)
100	Steep or impassable (≥15cm rise per cell in case of go2)
-1	Unknown (no observations)

4. Navigation Costmap — ReplanningAStarPlanner

The planner will process the terrain gradient and compute it’s own algo-relevant costmap, prioritizing safe free paths, while be willing to path aggressively through tight spaces if it has to

We run the planner in a constant loop so it will dynamically react to obstacles encountered.

5. All Layers Combined

All visualization layers shown together

Patrolling

The patrolling system drives the robot to systematically cover a known area. It is exposed as an agent skill. An LLM agent can call start_patrol and stop_patrol to control it. Note that the area has to be explored first.

How it works

1. Visitation tracking — As the robot moves, a visitation grid (aligned to the costmap) marks cells around the robot’s position as visited. This gives the system a running picture of where the robot has and hasn’t been. This expires over time, and has to be visited again.

2. Goal selection — A patrol router picks the next goal. The default strategy is coverage: it samples a handful of candidate points from unvisited, obstacle-free cells, plans a path to each one, and picks the candidate whose path would cover the most new ground. Candidates are weighted by a Voronoi skeleton so goals are more likely to be spread evenly across the map, rather than clustering in large open areas.

3. Navigation loop — The module sends each goal to the planner and waits for a goal_reached signal before requesting the next one. If no valid goal is available (e.g. the map hasn’t loaded yet), it retries after a short delay.

4. Stopping — When patrol is stopped, the module cancels in-progress navigation by publishing the robot’s current pose as the goal, then re-enables the planner’s normal replanning behavior.

Patrol router strategies

Router	Behavior
coverage	Maximizes new-cell coverage per goal. Uses Voronoi weighting for even spatial distribution.
random	Picks a random unvisited, obstacle-free cell.
frontier	Targets the boundary between known and unknown space, useful for exploration-style patrol.

Safety

Goal candidates are filtered through a safe mask — the free-space region eroded by the robot’s clearance radius — so the robot is never sent to a position too close to walls or obstacles. The planner’s safe-goal clearance is also tightened while patrolling to ensure the robot can rotate in place at every goal.

Router comparison

Coverage	Frontier	Random

Sample patrol trace (26 min)

Blueprint Composition

The navigation stack is composed in the unitree_go2 blueprint:

skip fold output=assets/go2_blueprint.svg

from dimos.core.coordination.blueprints import autoconnect
from dimos.core.introspection.svg import to_svg
from dimos.mapping.costmapper import CostMapper
from dimos.mapping.voxels import VoxelGridMapper
from dimos.navigation.frontier_exploration.wavefront_frontier_goal_selector import (
    WavefrontFrontierExplorer,
)
from dimos.navigation.replanning_a_star.module import ReplanningAStarPlanner
from dimos.robot.unitree.go2.blueprints.basic.unitree_go2_basic import unitree_go2_basic

unitree_go2 = autoconnect(
    unitree_go2_basic,
    VoxelGridMapper.blueprint(),
    CostMapper.blueprint(),
    ReplanningAStarPlanner.blueprint(),
    WavefrontFrontierExplorer.blueprint(),
).global_config(n_workers=6, robot_model="unitree_go2")

to_svg(unitree_go2, "assets/go2_blueprint.svg")