Overview

This package is a ROS wrapper around the sbpl_dynamic_planner that adheres to the nav_core::BaseGlobalPlanner interface specified in nav_core. This allows the planner to be used as the global planner for move_base or parallel_move_base. The planner will generate a path from the robot's current position to a desired goal pose (x,y,theta). Paths are generated by combining a series of "motion primitives" which are short, kinematically feasible motions, and "wait-in-place" operations. Planning is therefore done in x,y,theta,time dimensions, resulting in smooth time-parameterized paths. Taking robot orientation into account is especially important if the robot is not assumed to be circular or has nonholonomic constraints. In dynamic environments timing is very important which another reason why monitoring the time to turn is important. The planner includes the time dimension in a way that keeps the state space small (so it plans fast) while producing intelligent looking plans in the presence of moving obstacles such as ducking into a doorway to avoid an oncoming person and then proceeding after they have passed. The planner uses a modified version of ARA* (Anytime Repairing A*) in order to produce a sub-optimal plan quickly and improve toward being optimal as time allows. This is particularly important in dynamic environments.

Important: This planner's performance is highly dependent on the quality of the predicted dynamic obstacle trajectories! The basic tracker in this stack lidar_tracking is not robust especially once the robot is moving. Users are encouraged to provide better trackers which publish the appropriate dynamic_obs_msgs. In simulation, this stack provides fake_tracking which polls the simulator for the locations of other robots and works reasonably well.

Also while using move_base with this planner will run, it will not run the global planner regularly which is more important when the environment is changing quickly. Therefore, it is recommended that you use parallel_move_base which is a modified version of move_base where the global and local planners each run on at the same time in parallel. Each of them are run on their own user-specified frequency.

How to use

This global planner can be used with move_base simply by setting the "base_global_planner" parameter to "SBPLDynEnvGlobalPlanner". Additionally, at the very least the path to a motion primitive file must be specified shown below in the list of parameters. Also a node that publishes dynamic_obs_msgs is needed in order to plan with respect to moving obstacles.

Some launch files to run the planner in simulation are in the demo_sbpl_dynamic_env package. Also, there is a launch file in this package "launch/pr2_dynamic_planner.launch" which runs the planner on the PR2 robot and can be easily modified for other platforms. Note: the real robot launch file uses lidar_tracking to track and predict dynamic obstacle trajectories. This node is not robust!

ROS API

Subscribed Topics

dynamic_obstacles (dynamic_obs_msgs/DynamicObstacles)

• This message provides the predicted future trajectories of the moving obstacles. While the message has a very general full covariance matrix for each point in the predicted trajectories, this planner assumes shared variance so it just uses the average of the variances for x and y.

move_base/status (actionlib_msgs/GoalStatusArray)

• Since the default path followers don't follow time-parameterized paths, when the path has a point where the robot must wait in place, the planner only returns the path up until that wait. It checks the goal status until the robot has reached the "sub-goal". Then it feeds the next part of the path to move_base by telling it there is a "new goal". Hopefully this is a temporary hack.

Published Topics

~/SBPLDynEnvGlobalPlanner/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.

visualization_marker_array (visualization_msgs/MarkerArray)

• Text visualization markers are published to display where the robot will have to wait on the path (if any).

move_base_simple/goal (geometry_msgs/PoseStamped)

• Since the default path followers don't follow time-parameterized paths, when the path has a point where the robot must wait in place, the planner only returns the path up until that wait. It checks the goal status until the robot has reached the "sub-goal". Then it feeds the next part of the path to move_base by telling it there is a "new goal". Hopefully this is a temporary hack.

Parameters

~/SBPLDynEnvGlobalPlanner/allocated_time (double, default: 0.5)

• 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).

~/SBPLDynEnvGlobalPlanner/initial_epsilon (double, default: 5.0)

• 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).

~/SBPLDynEnvGlobalPlanner/decrease_epsilon (double, default: 1.0)

• The amount by which epsilon is decreased after each solution is found.

~/SBPLDynEnvGlobalPlanner/primitive_filename (string, default: "")

• 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.

~/SBPLDynEnvGlobalPlanner/time_resolution (double, default: 0.001)

• This parameter sets the time granularity when doing collision checks against moving obstacles.

~/SBPLDynEnvGlobalPlanner/temporal_padding (double, default: 1.5)

• The minimum amount of time allowed between when a moving obstacle occupies a cell and when the robot can (and vice versa).

~/SBPLDynEnvGlobalPlanner/nominalvel_mpersecs (double, default: 0.4)

• The linear velocity of the robot in meters/sec

~/SBPLDynEnvGlobalPlanner/timetoturn45degsinplace_secs (double, default: 0.6)

• The time it takes the robot to turn 45 degrees in place in seconds.

~/SBPLDynEnvGlobalPlanner/remove_dynObs_from_costmap (bool, default: true)

• This should be set to true if your moving obstacles are also present in the static costmap. Then the planner will attempt to remove them in order to make sure they are only treated as moving obstacles.

~/SBPLDynEnvGlobalPlanner/dyn_obs_pad_costmap_removal (double, default: 0.2)

• When removing dynamic obstacles from the costmap the planner will use the radius of the obstacle plus this parameter. (This parameter only matters if remove_dynObs_from_costmap is true).

Customizing your Motion Primitives

Please refer to the sbpl_dynamic_planner documentation for pre-made motion primitives for the PR2 (and other robots) as well as instructions on how to generate your own custom motions.