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.46.0 (2025-06-20)

0.45.0 (2025-05-22)

0.44.2 (2025-06-10)

0.44.1 (2025-05-01)

0.44.0 (2025-04-18)

0.43.0 (2025-03-21)

• Merge remote-tracking branch 'origin/main' into chore/bump-version-0.43
• chore: rename from [autoware.universe]{.title-ref} to [autoware_universe]{.title-ref} (#10306)
• Contributors: Hayato Mizushima, Yutaka Kondo

0.42.0 (2025-03-03)

• Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base
• feat(autoware_utils): replace autoware_universe_utils with autoware_utils (#10191)
• Contributors: Fumiya Watanabe, 心刚

0.41.2 (2025-02-19)

• chore: bump version to 0.41.1 (#10088)
• Contributors: Ryohsuke Mitsudome

0.41.1 (2025-02-10)

0.41.0 (2025-01-29)

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

• fix(autoware_freespace_planning_algorithms): fix bugprone-errors (#9670)

  • fix: bugprone-error

  * fix: bugprone-error ---------

• build(autoware_freespace_planning_algorithms): increase test timeout to 2 mins (#9639)

• Contributors: Fumiya Watanabe, M. Fatih Cırıt, kobayu858

0.40.0 (2024-12-12)

• Merge branch 'main' into release-0.40.0

• Revert "chore(package.xml): bump version to 0.39.0 (#9587)" This reverts commit c9f0f2688c57b0f657f5c1f28f036a970682e7f5.

• fix: fix ticket links in CHANGELOG.rst (#9588)

• chore(package.xml): bump version to 0.39.0 (#9587)

  • chore(package.xml): bump version to 0.39.0
  • fix: fix ticket links in CHANGELOG.rst

  * fix: remove unnecessary diff ---------Co-authored-by: Yutaka Kondo <<yutaka.kondo@youtalk.jp>>

• fix: fix ticket links in CHANGELOG.rst (#9588)

• fix(cpplint): include what you use - planning (#9570)

• refactor: correct spelling (#9528)

• fix(autoware_freespace_planner, autoware_freespace_planning_algorithms): modify freespace planner to use node clock instead of system clock (#9152)

  • Modified the autoware_freespace_planner and autoware_freespace_planning_algorithms packages to use the node clock instead of rclcpp detached clock. This allows the module to make use of sim time. Previously during simulation the parking trajectory would have system time in trajectory header messages causing downstream issues like non-clearance of trajectory buffers in motion planning based on elapsed time.
  • style(pre-commit): autofix
  • Updated the freespace planner instantiation call in the path planning modules

File truncated at 100 lines see the full file