04 / 06 · RRT* · global planning · numpy only
Throw random points at the map, grow a tree toward them, and keep rewiring it until the route through the pillars is close to the shortest one. Then fly it.
How it works
01 · Sample and extend
Pick a random point in the box around start and goal (8% of the time, the goal itself). Find the nearest node and extend it up to 0.8 m toward the point, if that edge clears every pillar padded by 0.5 m, checked every 0.1 m.
This is a wall the potential field cannot get past; for a global planner it is routine.
02 · Choose a parent, rewire
A new node takes the cheapest parent it can reach within 1.6 m. Then each neighbour checks whether going through the new node would be shorter, and switches if so.
When a node switches, the saving flows down its whole subtree, a step most tutorial versions skip. The orange flashes are rewires.
03 · Keep refining
After the first path to the goal appears, the tree keeps growing for 1,500 more iterations (4,000 at most), and the best path keeps getting shorter. The goal candidate is re-picked over all nodes that can see it, so an older one improved by rewiring is never missed.
04 · Smooth and fly
A line-of-sight prune and 120 random shortcuts, each re-checked against the pillars, straighten the path. A waypoint follower flies it with velocity setpoints and slows before sharp turns.
The slowdown came from Gazebo: a 90° turn at 1.5 m/s overshot the 0.5 m padding and clipped a pillar.
treebest path · rewireflown
Try it
Click or tap to set a goal: a fresh tree grows from where the drone is, then it flies the smoothed path. A 2D re-implementation of rrtstar_core.py with the same parameters; 1.2 m pillars stay flyable because the cruise is at 2 m.
Numbers are the defaults in rrtstar_core.py. c(x) is the length of the tree path from the start to node x; every edge is checked against the same free-space test every 0.1 m.
free space
x is free ⇔ for every pillar: |xxy − cxy| ≥ r + 0.5 or alt(x) ≥ top + 0.5
each iteration
then fly it
| scenario | result | follow | flight | min clearance | re-plans |
|---|---|---|---|---|---|
| default course, fixed (1.5 m/s) | reached | 8.5 s | 11.6 m | 0.50 m | 0 |
| default course, lidar (1.5 m/s) | reached | 9.3 s | 12.8 m | 0.52 m | 0 |
| forest, fixed (1.0 m/s) | reached | 20.6 s | 16.4 m | 0.52 m | 0 |
| forest, lidar (1.0 m/s) | reached | 35.0 s | 45.7 m | 0.13 m | 9, all successful |
The forest-with-lidar run is the cost of exploring what it cannot see yet: the first path crosses an area later found blocked, pillars behind the wall appear one by one, and each one invalidates the path and triggers a re-plan. Clearance cannot be guaranteed for surfaces not yet observed. A grow-only lidar map also keeps every projection error forever, so it uses full-attitude projection, an altitude band and a yaw-rate gate.
RViz shows the whole search tree, which is the most useful view of RRT*: thin blue lines are the tree, the green line is the planned path.
Python · numpy · ROS 2 Humble · PX4 v1.14 · Gazebo Garden · 2D lidar