# px4-ros2-drone-nav: map-free 3D navigation with a persistent D\* Lite planner and CUDA MPPI (ROS 2 Jazzy, PX4 SITL, Gazebo Harmonic)

**URL:** <https://discourse.openrobotics.org/t/px4-ros2-drone-nav-map-free-3d-navigation-with-a-persistent-d-lite-planner-and-cuda-mppi-ros-2-jazzy-px4-sitl-gazebo-harmonic/58190>\
**Category:** Projects\
**Tags:** ros2, gazebo, navigation, simulation, wg-aerial-robotics\
**Created:** [September 16, 2026, 3:39pm UTC](https://discourse.openrobotics.org/t/px4-ros2-drone-nav-map-free-3d-navigation-with-a-persistent-d-lite-planner-and-cuda-mppi-ros-2-jazzy-px4-sitl-gazebo-harmonic/58190 "2026-09-16T15:39:33Z")\
**Posts on this page:** 1\
**Page:** 1

<div class="post-metadata">

**Author:** ![formiat](https://sea2.discourse-cdn.com/flex022/user_avatar/discourse.openrobotics.org/formiat/32/35476_2.png) [@formiat](https://discourse.openrobotics.org/u/formiat)\
**Post date:** [September 16, 2026, 3:39pm UTC](https://discourse.openrobotics.org/t/px4-ros2-drone-nav-map-free-3d-navigation-with-a-persistent-d-lite-planner-and-cuda-mppi-ros-2-jazzy-px4-sitl-gazebo-harmonic/58190/1 "2026-09-16T15:39:33Z")

</div>

Hello everyone,

I have been building an open-source navigation stack for a quadrotor that is  
given a start and a goal inside a multi-storey urban world and no map at all.  
It builds its own obstacle memory from a simulated 3D lidar while it flies,  
plans over that memory in full 3D, and executes through PX4 in offboard mode.  
It is MIT licensed, simulation-first, and explicitly not validated for real  
aircraft.

Repository: [GitHub - formiat/px4-ros2-drone-nav · GitHub](https://github.com/formiat/px4-ros2-drone-nav) (release `v0.2.1`)

![08_forum_interior_replanning_640](https://us1.discourse-cdn.com/flex022/uploads/ros/original/3X/6/d/6df97d75bc6c5e2bed72db4221fb424d97630409.gif)\*  
(GIF: Gazebo on the left, RViz on the right; pink is what the drone  
remembers, blue is the current lidar scan, orange is the route it is committed  
to.)\*

Full flight, about three minutes: [https://www.youtube.com/watch?v=rKXcERqb9Ho](https://www.youtube.com/watch?v=rKXcERqb9Ho)

The location is the DARPA Subterranean Challenge “Urban Circuit Practice 01”  
world published by Open Robotics on Gazebo Fuel (CC BY 4.0), imported with its  
collision geometry.

## The stack

ROS 2 Jazzy, PX4 SITL v1.17 over uXRCE-DDS, Gazebo Harmonic, CUDA for the  
optimizer. One ament package, `drone_city_nav`, plus the Gazebo assets, the  
PX4 orchestration scripts and the container tooling.

```plaintext
Gazebo GPU lidar (360 x 181 beams, 10 Hz) + PX4 local position
  -> obstacle_memory_3d_node: timestamp-aligned ray integration into a
     revisioned unknown/free/occupied Occupancy3D, base snapshot + dirty chunks
  -> production_mppi_node
       persistent sparse D* Lite over an adaptive 26-connected 3D lattice
       -> certified route geometry with a speed profile
       -> CUDA MPPI local horizon at 50 Hz
       -> exact swept-footprint validation against raw occupancy
       -> atomic execution plan -> timestamped MppiTrajectoryHorizon
  -> mppi_offboard_node -> PX4 trajectory setpoints

```

Gazebo contacts run on an independent path: a contact involving the drone  
reaches a `collision_crash_node`, which publishes a typed destruction event and  
latches the offboard into a disarm lifecycle. That is how a crash is detected  
in the mission check rather than inferred from the trajectory.

## The part I would most like to argue about: unknown space is free

In this stack `Free` and `Unknown` have identical traversability and identical  
base cost. Only confirmed `Occupied` cells and the physical swept hull of the  
vehicle are hard constraints. There is no strict-known switch, no frontier stop  
policy, no inflation, and no preference for flying through what has already  
been seen. The planner commits to a route through space nobody has looked at.

What keeps that safe is not a penalty but one inequality, applied as the speed  
law everywhere:

```plaintext
speed * total_latency + stopping_distance + physical_margin
  <= guaranteed_lidar_detection_range

```

Total latency is the configured maximum age of timestamp-aligned lidar evidence  
plus control reaction latency; the stopping distance comes from a jerk-limited  
model using the weaker of the guaranteed horizontal and vertical decelerations;  
the guaranteed range is bound by a config test to the sensor model and to the  
obstacle memory’s own range. With 14 m of guaranteed range, 0.6 s of evidence  
age, 0.1 s of reaction and a 2 m physical margin the contract admits about  
5.6 m/s in level flight. The vehicle flies at the speed from which it can stop  
inside the volume the sensor is guaranteed to have resolved, so entering  
unobserved space is not a gamble, it is a bounded one.

The price is visible in the numbers below as route availability: when a wall  
does appear a few metres ahead, the route is invalidated and the replacement  
search takes time, and during that time the vehicle brakes. I think that is the  
right trade, but I would like to hear how others bound it.

## Validated flights

`make sim-urban-point-to-point-headless`, five consecutive flights on one  
commit with nothing changed between them (the release series, reproduced in  
`CHANGELOG.md`):

| Flight | Path | Duration | Mean speed | Crash | Route availability | Planner p95 |
| --- | --- | --- | --- | --- | --- | --- |
| r288 | 399.9 m | 137.8 s | 2.90 m/s | none | 95.1 % | 154 ms |
| r289 | 384.6 m | 148.8 s | 2.58 m/s | none | 93.1 % | 152 ms |
| r290 | 401.5 m | 134.4 s | 2.99 m/s | none | 92.0 % | 157 ms |
| r291 | 371.5 m | 123.9 s | 3.00 m/s | none | 91.2 % | 154 ms |
| r292 | 417.2 m | 134.7 s | 3.10 m/s | none | 88.9 % | 155 ms |

Mean speed is path length over the time from mission readiness to the  
successful result, so every hold, stop and replan counts against it. Route  
availability is the share of flight time with a certified route to the goal.  
The workstation is 16 threads with an RTX 3060.

## Regression-testing the assumptions, not only the outcome

The part of this project I would recommend to anyone doing offboard control in  
simulation: every headless flight records the trajectory setpoints, PX4’s local  
position and the true Gazebo pose, and the mission check then holds four  
measurements against the assumptions the navigation laws stand on. Lateral  
tracking error at p99, the deceleration the airframe actually delivers when it  
arrests a descent, the position estimate against the true pose with the clocks  
aligned on the speed profile, and the age of the lidar evidence at the planning  
tick. A law that quietly stops being true now fails a check on the next flight  
instead of showing up as a crash three weeks later.

Two findings from that loop that may be useful to other PX4 and Gazebo users:

- The stopping laws assumed a 3 m/s^2 descent arrest. Measured on PX4’s local  
position, the x500 arrests a descent at 1.4 to 2.2 m/s^2, because arresting a  
descent is bound by the thrust above hover and not by `MPC_ACC_UP_MAX`. One  
flight dived onto a roof the lidar had resolved 2.3 m below it.
- PX4’s `EKF2_GPS_DELAY` defaults to 110 ms, which is right for a real  
receiver. The Gazebo bridge stamps the simulated navsat sample at the moment  
it receives it, so the filter subtracted a delay that was not there and  
placed the vehicle where it would have moved to since: the position estimate  
ran 0.11 s ahead of the true pose along the motion, 0.35 m at 3 m/s. Setting  
it to zero in SITL removed that lead, and the lateral tracking error at p99  
fell from 0.13 to 0.29 m down to 0.03 to 0.18 m. If you fly offboard in  
Gazebo and your vehicle seems to cut corners, this is worth checking.

## Running it

On a Linux host with Docker and the NVIDIA container runtime, one script  
prepares a fresh clone (dev image, PX4 SITL build, workspace, versioned  
environment assets) and starts the flight:

```bash
git clone https://github.com/formiat/px4-ros2-drone-nav.git
cd px4-ros2-drone-nav
./scripts/bootstrap.sh

```

`--headless` runs the same flight without the GUI and with the mission check;  
`--no-run` only prepares. Every step is skipped when its result exists. The  
first run downloads several gigabytes and builds for tens of minutes.

## Limitations

- Route availability is 89 to 95 percent against the 97 percent the mission  
check wants; the remainder is replacement search after a wall appeared in  
unobserved space.
- Localization is PX4’s EKF and sits 0.19 to 0.25 m from the true pose across  
the track, and the obstacle memory is built in the estimate’s frame at  
observation time. Nothing corrects the estimate against the map yet;  
lidar-inertial localization without GNSS and magnetometer is the next  
milestone.
- Everything is tuned for this simulation: the x500 airframe, PX4 SITL v1.17,  
Gazebo Harmonic, ROS 2 Jazzy. Multi-vehicle cooperative traffic and  
interception scenarios exist in the repository but were last flown before the  
September navigation changes.
- Simulation-oriented research code. Do not put it on a physical aircraft  
without a separate safety review and failsafe design.

## Questions for the forum

1. How do you decide how fast to fly into space you have not observed yet? I  
use a guaranteed detection range and an evidence-age budget; I would like to  
hear other formulations, especially ones that survive a sensor with blind  
cones.
2. For those using `nav2_mppi_controller`: the local optimizer here is a  
separate CUDA implementation with a 3D route and a raw swept-footprint  
validation after the update. Is anyone running Nav2’s MPPI on an aerial  
platform in full 3D, and where does it stop being a good fit?
3. What do you regression-test on the controller side between runs, beyond  
“the mission completed”?

Happy to answer questions about any part of it.