sbpl_lattice_planner is a ROS wrapper for the SBPL lattice environment and adheres to the nav_core::BaseGlobalPlanner interface specified in nav_core. The lattice planner can therefore be used as the global planner for move_base. The planner will generate a path from the robot's current position to a desired goal pose. Paths are generated by combining a series of "motion primitives" which are short, kinematically feasible motions. Planning is therefore done in x, y, and theta dimensions, resulting in smooth paths that take robot orientation into account, which is especially important if the robot is not assumed to be circular or has nonholonomic constraints. Plans can be found using the ARA* planner or AD* planner.
How to use
This global planner can be used with move_base simply by setting the "base_global_planner" parameter to "SBPLLatticePlanner". Additionally, at the very least the path to a motion primitive file must be specified shown below in the list of parameters.
Example in Stage
The package contains a launch file for testing the sbpl_lattice_planner as the global planner for move_base using stage for 2D simulation:
roslaunch sbpl_lattice_planner move_base_sbpl_fake_localization_2.5cm.launch
Published Topics~/SBPLLatticePlanner/plan (nav_msgs/Path)
- The last plan computed by SBPL, published every time the planner computes a new path, and used primarily for visualization purposes.
- Statistics from the last planning request. Stats include: time taken to get to the first and final solutions, number of state expansions taken to get the first and final solutions, the epsilon (bound on the sub-optimality of the solution) of the first and final solutions, and the size of the final solution.
Parameters~/SBPLLatticePlanner/planner_type (string, default: "ARAPlanner")
- Specifies which planner to use. It can either be "ARAPlanner" for ARA* or "ADPlanner" for AD*.
- The amount of time given to the planner to find a solution. If there is still time remaining after the planner finds its sub-optimal initial solution (specified by "initial_epsilon"), the planner will use up remaining time improving solution until it is optimal or until time runs out (which ever comes first).
- The value the heuristic is scaled by for the first search. This value must be greater or equal to 1. The larger this value is the faster the search tends to find a solution (likely sub-optimal if epsilon is larger than 1). After the first search, the planner will continue to reduce the epsilon value until it is 1 (optimal search).
- The type of environment being searched. Currently, XYThetaLattice is the only supported environment.
- The direction the search is done in. If you are using AD* you should use backward search for fast replanning times. For ARA* it doesn't matter too much which direction you use.
- The path to a motion primitive file. This MUST be specified by the user for the planner to work. There is an example motion primitive file that can be used in matlab/mprim/pr2.mprim in the SBPL package. If you want to generate your own motion primitive file to match the kinematics of your robot or your map resolution, there is are several genmprim*.m scripts in matlab/mprim/ in the SBPL package to help you.
- The parameter only matters if you are using AD*. If at least this many map cells have changed since the last plan was generated, the planner will not reuse previous search information and instead plan from scratch.
- The linear velocity of the robot in meters/sec
- The time it takes the robot to turn 45 degrees in place in seconds.
- The cost of an obstacle in the planner's version of the costmap. All other values in the costmap are scaled accordingly. The obstacle cost is the largest in the costmap2d so by setting this parameter to something below its obstacle thresh, we can get obstacle padding that is less harsh and more reasonable. This will make the planner more likely to choose to go through more narrow areas such as doorways.
Customizing your Motion Primitives
Please refer to the SBPL documentation for pre-made motion primitives for the PR2 (and other robots) as well as instructions on how to generate your own custom motions.