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 (release v0.2.1)
*
(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
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.
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:
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 byMPC_ACC_UP_MAX. One
flight dived onto a roof the lidar had resolved 2.3 m below it. - PX4’s
EKF2_GPS_DELAYdefaults 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:
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
- 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. - 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? - What do you regression-test on the controller side between runs, beyond
“the mission completed”?
Happy to answer questions about any part of it.