Navigation Stack · Unitree Go2/Go2W · ROS 2 Humble · MuJoCo simulation + real hardware
Last updated: 2026-08-06
Overview
A modular navigation stack for the Go2/Go2W platform. All configurations launch
from a single entry point; localization, mapping, navigation, and exploration
components are swappable without changing the safety or operator layers.
A dedicated safety node owns the final command path to the platform.
In every demonstration on this page, navigation goals are placed manually by the
operator in real time (RViz 2D goal pose, or a clicked 3D point for the multilevel
scene). No goal sequence is pre-scripted or scheduled; the videos show live
goal-driven navigation.
One launch entry point resolves a thin preset (which localization / mapping /
navigation / exploration components to run) and supervises the whole session:
readiness checks, TF validation, and artifact verification on every run.
Manual controller input latches autonomy off and cancels active navigation goals.
Figure 1. Product stack topology. Accented components are selected per
preset; every command path converges on a single mux and a single safety gate, and
manual input latches autonomy off rather than sharing priority with it.
A web operator console provides login, live camera / map / robot state, and a
browser e-stop. It has been exercised against the real robot: map reset,
exploration start/stop, and live 2D map / 3D voxel views in a real session.
Web operator console (simulation view): live RGB / depth feeds, safety
and control state, compute telemetry, localization, occupancy map, and per-topic
rate checks against the stack contract.
Current limitations
Parts of the frontend are not yet routed through the unified backend API.
2. Real-robot LiDAR occupancy navigation
Mid360 → Fast-LIO → 2D occupancy grid → Nav2 on the real Go2W, with goal-driven
navigation from RViz. Several hardware-only issues were found and fixed during
bring-up (estimator rate collapse, incorrect robot footprint parameters).
Figure 2. Real-robot LiDAR configuration. The entire autonomy pipeline
currently runs on the workstation; only raw sensor packets and gated velocity
commands cross the Ethernet boundary. Moving this compute onboard is
next-direction 4 (the onboard environment is ROS 1 today).
Real Go2W session (2026-07-30): live LiDAR point cloud, occupancy map,
and an operator-placed navigation goal in RViz (left); the robot and onboard camera
view (right). The goal is clicked live during the session, not pre-programmed.
Current limitations
Validated only with the LiDAR driver and compute on the workstation; the
onboard Orin NX currently runs a ROS 1 environment, while this stack is
ROS 2, so onboard hosting needs a compute migration first.
SLAM divergence is not auto-detected yet; longer runs require operator supervision.
3. RTAB-Map navigation instance (simulation)
The localization/mapping pair swapped from Fast-LIO + LiDAR occupancy to
RTAB-Map + RGB-D, with the navigation and safety layers unchanged — a direct
validation that stack components are genuinely replaceable.
Figure 3. Perception-head swap. The RGB-D head replaces Fast-LIO +
OctoMap with RTAB-Map publishing the same map and odometry contract; Nav2, the
command mux, and the safety gate are byte-for-byte the same configuration as the
LiDAR stack.
RTAB-Map instance (simulation): occupancy grid built from the RGB-D
camera (RViz, right) consistent with the LiDAR navigation scan (green);
MuJoCo world on the left.
Current limitations
Simulation only. The real D455 is currently display-only: no extrinsic calibration, not wired into navigation, and bandwidth-limited on its current USB connection.
4. SCAN-Planner integration (simulation)
SCAN-Planner (3D local planner, B-spline trajectory replanning) integrated
through the same command/safety chain as Nav2. Closed-loop goal reaching works
on flat ground; a purpose-built two-level test scene (stairs + ramp) probes
multilevel navigation.
Preliminary conclusion: on flat ground SCAN qualitatively outperforms the MPPI
baseline and is the leading candidate there. As the video below shows, the
planner does produce 3D paths through the two-level scene — but the current
controller is not capable of executing the ascent. The gap is in execution
(controller / locomotion), not in planning.
Figure 4. Local navigation backends compared. Nav2 plans on inflated 2D
costmaps and samples SE(2) controls; SCAN-Planner maintains its own 3D occupancy and
optimises smooth B-spline trajectories tracked by its upstream controller. Both emit
cmd_vel_auto into the identical mux/safety chain, and the
current stair/ramp failure sits downstream of both, in locomotion.
Two-level scene (stairs + ramp), simulation: SCAN-Planner builds a 3D map
and plans 3D paths toward an elevated goal clicked live by the operator in RViz
(Publish Point, right); the current controller cannot execute the climb
(MuJoCo, left).
Current limitations
Planned 3D paths are not yet executable: the bundled closed-loop controller
cannot follow them up stairs or ramps.
Comparison against Nav2/MPPI is qualitative; no standardized benchmark yet.
Not yet profiled on any embedded platform.
Next directions
Optimise SLAM stability.
Optimise the Go2/Go2W controller in simulation using reinforcement learning.
Deploy SCAN-Planner in the OPS lab: navigate from the pit, up the stairs,
to the ground floor.
Move compute onboard — next 4 weeks (the current onboard environment is
ROS 1; this stack is ROS 2).
Deploy vision-only SLAM on the real robot using the D455.
Adopt open simulation benchmarks and develop complex navigation terrains
to stress-test the robot in simulation.
