[ros-users] Incremental heuristic search in move_base

[Mike Phillips]

Hi Simon,
The SBPL package has an implementation of AD* (anytime D*).  If you
were to set the initial epsilon (initial weight on the heuristic) to
1.0 the planner will run exactly like D* Lite.  If you are interested
in the anytime functionality AD* works by running multiple iterations
weighted A*, and decrementing the weight until it gets to 1.0 (optimal
search).  This allows the planner to find a suboptimal solution
quickly, improve it as time allows, and if there is enough time
produce the optimal solution.

If you want to use SBPL with the navigation stack there is a package
called sbpl_lattice_planner which can be used as a global planner for
the navigation stack.  The node is basically a wrapper around SBPL to
integrate it into the nav stack.  The default parameters run ARA* (an
anytime A* planner) but you can change it to run AD* with an initial
epsilon of 1.0 to make it run D* Lite.  This package runs the planners
on a 3D state space (x,y,theta) to produce smoother paths, handle
non-holonomic constraints, and deal with non-circular footprints.
This comes at a cost planning times.  The package in its current form
doesn't support planning in a 2D state space, but SBPL does.  Since
sbpl_lattice_planner is just a wrapper some relatively simple changes
can make the package use SBPL's 2D state space.
Specifically if you just change the type of field env_ from
EnvironmentNAVXYTHETALAT* to EnvironmentNAV2D*, change how it is
instantiated similarly, and then fix whatever doesn't compile (small
things like need a subset of the parameters for most function calls
due to one less dimension).

The implementation of D* uses dynamic memory allocation for the states
in order to be general to planning in cases where the state space is
high-dimensional and you wouldn't want to construct all of the state a
priori.  Therefore for the 2D case, this will cause some small slow
down since we already have the 2D costmap allocated and allocating the
whole state space in advance would probably be a little faster.

Let me know if you have any questions about SBPL, or getting it to
work with the nav stack for your application.

-Mike-

> Hi Eitan,
>
> Here is the story. I have a need of Incremental heuristic search in Base
> Global Planner for long range navigation in an unknown area, so I tried to
> modify your A* in move_base to D* Lite<http://idm-lab.org/bib/abstracts/papers/aaai02b.pdf>.
> However, it is really difficult to arrange the order of the re-explored
> cells in the priority buffer because you are using the threshold curT to
> control the behavior of the priority queue. And also, some re-explored cells
> have to be updated twice by the D* Lite during the re-planning. So I take
> use of your code to realize a new way: 1) Keep the potarr during one long
> range move; 2) Swap the start and goal cells before the propagation, make
> your A* as a backward version; 3)Do a limited re-propagation when
> re-planning is necessary. I also create a PointCloud2 message for the
> explored potential area, and add this patch
> <https://code.ros.org/trac/ros-pkg/ticket/4610>to visualize the message.
> Here <http://www.youtube.com/watch?v=jTt4IrsOqEU> is a screencast of my
> experiment.
>
> At the moment, this method is not stable yet, some times the robot will be
> stopped by the incomplete potential area. Do you think it is a good way to
> do the Incremental replanning? Or it is still possible to realize the D*
> Lite based on your A*?
>
> Cheers,
> Simon