freespace planning algorithms

Role

This package is for development of path planning algorithms in free space.

Implemented algorithms

• Hybrid A* and RRT* (includes RRT and informed RRT*)

Please see rrtstar.md for a note on the implementation for informed-RRT*.

NOTE: As for RRT*, one can choose whether update after feasible solution found in RRT*. If not doing so, the algorithm is the almost (but exactly because of rewiring procedure) same as vanilla RRT. If you choose update, then you have option if the sampling after feasible solution found is “informed”. If set true, then the algorithm is equivalent to informed RRT\* of Gammell et al. 2014.

Algorithm selection

There is a trade-off between algorithm speed and resulting solution quality. When we sort the algorithms by the spectrum of (high quality solution/ slow) -> (low quality solution / fast) it would be A* -> informed RRT* -> RRT. Note that in almost all case informed RRT* is better than RRT* for solution quality given the same computational time budget. So, RRT* is omitted in the comparison.

Some selection criteria would be:

• If obstacle geometry is complex: -> avoid RRT and RRT*. The resulting path could be too messy.
• If goal location is far from the start: -> avoid A*. Take too long time because it based on grid discretization.

Guide to implement a new algorithm

• All planning algorithm class in this package must inherit AbstractPlanningAlgorithm class. If necessary, please overwrite the virtual functions.
• All algorithms must use nav_msgs::OccupancyGrid-typed costmap. Thus, AbstractPlanningAlgorithm class mainly implements the collision checking using the costmap, grid-based indexing, and coordinate transformation related to costmap.
• All algorithms must take both PlannerCommonParam-typed and algorithm-specific- type structs as inputs of the constructor. For example, AstarSearch class’s constructor takes both PlannerCommonParam and AstarParam.

Running the standalone tests and visualization

Building the package with ros-test and run tests:

colcon build --packages-select autoware_freespace_planning_algorithms
colcon test --packages-select autoware_freespace_planning_algorithms

Inside the test, simulation results are stored in /tmp/fpalgos-{algorithm_type}-case{scenario_number} as a rosbag. Loading these resulting files, by using test/debug_plot.py, one can create plots visualizing the path and obstacles as shown in the figures below. The created figures are then again saved in /tmp.

A* (single curvature case)

informed RRT* with 200 msec time budget

RRT* without update (almost same as RRT)

The black cells, green box, and red box, respectively, indicate obstacles, start configuration, and goal configuration. The sequence of the blue boxes indicate the solution path.

Extension to Python module (only A* supported)

There is an implementation of the extension to the python module. You can try A* search via Python by setting follows:

• parameters,
• costmap,
• start pose,
• goal pose.

Then, you can get

• success or failure,
• searched trajectory.

The example code is scripts/example/example.py. Note that you need to build this package and source the setup shell script in advance.

License notice

Files src/reeds_shepp.cpp and include/astar_search/reeds_shepp.h are fetched from pyReedsShepp. Note that the implementation in pyReedsShepp is also heavily based on the code in ompl.

File truncated at 100 lines see the full file

Changelog for package autoware_freespace_planning_algorithms

0.53.0 (2026-09-29)

• Merge remote-tracking branch 'origin/main' into prepare-0.53.0-changelog

• perf(autoware_freespace_planning_algorithms): update the RRT* goal link incrementally when no rewire occurs (#13245)

  * perf(autoware_freespace_planning_algorithms): update the RRT* goal link incrementally when no rewire occurs extend() rescans every reached node after each extension once a solution exists, but the scan can only change the answer when a reconnect rewired the tree or a new node reached the goal. Keep the full rescan for the reconnect case and otherwise compare just the newly reached node against the current best. Deterministic traces at 100, 600, and 2,000 extensions are bit-identical to the baseline (tree hashes and solution costs equal) on two platforms, including reconnect-heavy runs. Goal-cost evaluations drop 500,001 -> 1 in a 500k-reached-node scaling fixture (~100x on that phase). Co-authored-by: Maxim Kern <<maximkern@college.harvard.edu>>

  * test(autoware_freespace_planning_algorithms): verify the RRT* goal cost against all reached nodes Address review feedback: the informed test self-seeds from std::random_device and never checks the solution cost, so a broken goal link would produce a feasible but suboptimal path undetected. Add a seed hook to CSpace so tests can make the sampling sequence deterministic, and a test that runs 3,000 seeded extensions with periodic branch-and-bound pruning, asserting after every extension that the maintained goal cost equals the best cost among the nodes that reached the goal. The reference is computed from the public node list, so no internals are exposed. The test passes with the incremental goal update and on the baseline implementation, and fails when the incremental comparison is deliberately broken to skip improvements from newly reached nodes. Co-authored-by: Maxim Kern <<maximkern@college.harvard.edu>> ---------Co-authored-by: Maxim Kern <<maximkern@college.harvard.edu>>

• perf(autoware_freespace_planning_algorithms): reset only graph nodes touched by the preceding search (#13230)

  * perf(autoware_freespace_planning_algorithms): reset only graph nodes touched by the preceding search Track the node ids that transition out of the default state during a search and, when the map dimensions are unchanged, reset exactly those nodes instead of reassigning the entire graph vector on every plan. A dimension change still takes the full-reassignment path. An 8-plan interaction trace (repeated goals, multi-goal, blocked-map failure and recovery, same-size map replacement, dimension change) produces hash-identical waypoints on two platforms. In a focused benchmark (150x150 map, 8 headings, one touched node) the reset path median drops from 497 ms to 2.9 ms per 1,000 repetitions. Co-authored-by: Maxim Kern <<maximkern@college.harvard.edu>>

  * test(autoware_freespace_planning_algorithms): assert repeated plans on one instance are identical Address review feedback: the suite never compared a plan against a reference, so a leak of search state between plans would keep the tests green. Plan goal A, then goal B, then goal A again on a single instance and require the two A results to be identical, waypoint by waypoint. Any node the touched-node reset failed to restore would change the second result. The test passes on this branch and on the baseline implementation, and fails when the touched-node recording in expandNodes is deliberately removed. Co-authored-by: Maxim Kern <<maximkern@college.harvard.edu>> ---------Co-authored-by: Maxim Kern <<maximkern@college.harvard.edu>>

• perf(autoware_freespace_planning_algorithms): skip equal-distance duplicate pushes in the collision-free distance map (#13241) Only push a neighbor when its tentative distance strictly improves the stored value. Equal-distance re-pushes reprocess identical entries without changing any output of the Dijkstra pass. Every output equals the independently derived octile metric on an open grid, and full-map output checksums are unchanged. Heap pushes on a 101x101 open grid drop from 1,801,700 to 1,045,700 and the isolated phase median improves ~38%. Co-authored-by: Maxim Kern <<maximkern@college.harvard.edu>> Co-authored-by: Kosuke Takeuchi <<kosuke.tnp@gmail.com>>

• chore(pre-commit): update clang-format to v22.1.5 (#13126)

  • chore(pre-commit): update clang-format to v22.1.5

  * style(pre-commit): autofix ---------

• Contributors: Mete Fatih Cırıt, Ryohsuke Mitsudome, owenbales

0.52.0 (2026-06-30)

• Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base

• fix(clang-tidy): re-enable bugprone-assignment-in-if-condition (#12552)

File truncated at 100 lines see the full file