Skip to content

The freespace planning algorithms package#

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 node with test problems and visualization#

Building the package with ros-test and run it by rosrun:

catkin build astar_search --catkin-make-args run_tests
rosrun astar_search astar_search-test

Please make sure that roscore is running before running the test.

Inside the test, simulation results were stored in /tmp/result_multi0.txt.

Note that the postfix multi or single correspond to multiple curvatures and single curvature cases. 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 with the name like /tmp/result_multi0.png.

sample output figure

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.

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. Both pyReedsShepp and ompl are distributed under 3-clause BSD license.