melodic noetic   Show EOL distros: 

Overview

The base_local_planner package provides a controller that drives a mobile base in the plane. This controller serves to connect the path planner to the robot. Using a map, the planner creates a kinematic trajectory for the robot to get from a start to a goal location. Along the way, the planner creates, at least locally around the robot, a value function, represented as a grid map. This value function encodes the costs of traversing through the grid cells. The controller's job is to use this value function to determine dx,dy,dtheta velocities to send to the robot.

The basic idea of both the Trajectory Rollout and Dynamic Window Approach (DWA) algorithms is as follows:

1. Discretely sample in the robot's control space (dx,dy,dtheta)
2. For each sampled velocity, perform forward simulation from the robot's current state to predict what would happen if the sampled velocity were applied for some (short) period of time.
3. Evaluate (score) each trajectory resulting from the forward simulation, using a metric that incorporates characteristics such as: proximity to obstacles, proximity to the goal, proximity to the global path, and speed. Discard illegal trajectories (those that collide with obstacles).
4. Pick the highest-scoring trajectory and send the associated velocity to the mobile base.
5. Rinse and repeat.

DWA differs from Trajectory Rollout in how the robot's control space is sampled. Trajectory Rollout samples from the set of achievable velocities over the entire forward simulation period given the acceleration limits of the robot, while DWA samples from the set of achievable velocities for just one simulation step given the acceleration limits of the robot. This means that DWA is a more efficient algorithm because it samples a smaller space, but may be outperformed by Trajectory Rollout for robots with low acceleration limits because DWA does not forward simulate constant accelerations. In practice however, we find DWA and Trajectory Rollout to perform comparably in all our tests and recommend use of DWA for its efficiency gains.

Useful references:

• Brian P. Gerkey and Kurt Konolige. "Planning and Control in Unstructured Terrain ". Discussion of the Trajectory Rollout algorithm in use on the LAGR robot.

• D. Fox, W. Burgard, and S. Thrun. "The dynamic window approach to collision avoidance". The Dynamic Window Approach to local control.

• Alonzo Kelly. "An Intelligent Predictive Controller for Autonomous Vehicles". An previous system that takes a similar approach to control.

Map Grid

In order to score trajectories efficiently, the Map Grid is used. For each control cycle, a grid is created around the robot (the size of the local costmap), and the global path is mapped onto this area. This means certain of the grid cells will be marked with distance 0 to a path point, and distance 0 to the goal. A propagation algorithm then efficiently marks all other cells with their manhattan distance to the closest of the points marked with zero.

This map grid is then used in the scoring of trajectories.

The goal of the global path may often lie outside the small area covered by the map_grid, when scoring trajectories for proximity to goal, what is considered is the "local goal", meaning the first path point which is inside the area having a consecutive point outside the area. The size of the area is determined by move_base.

Oscillation Suppression

Oscillation occur when in either of the x, y, or theta dimensions, positive and negative values are chosen consecutively. To prevent oscillations, when the robot moves in any direction, for the next cycles the opposite direction is marked invalid, until the robot has moved beyond a certain distance from the position where the flag was set.

Generic Local Planning

New in navigation 1.10.0

The groovy release of ROS includes a new implementation of the dwa_local_planner package. The implementation attempts to be more modular, to allow easier creation of custom local planners while reusing a lot of code. The code base of base_local_planner has been extended with several new headers and classes.

The principle of local planning is the search for a suitable local plan in every control cycle. For that purpose, a number of candidate trajectories is generated. For a generated trajectory, it is checked whether it collides with an obstacle. If not, a rating is given to compare several trajectories picking the best.

Obviously, depending on the robot shape (and actuator shape) and the domain, this principle can be instantiated in very different ways. There are many special ways to generate trajectories, and ways to search the space of potential trajectories for a most suitable one.

The interfaces and classes below capture the generic local planning principles allowing many instantiations. It should be possible to create custom local planners using the dwa_local_planner as template and just adding own cost functions or trajectory generators.

TrajectorySampleGenerator

This interface describes a Generator which may generate a finite or infinte number of trajectories, returning a new one on each invocation of nextTrajectory().

The SimpleTrajectoryGenerator class can generate trajectories described in the overview, using either the trajectory rollout or the DWA principle.

TrajectoryCostFunction

This interface contains most importantly scoreTrajectory(Trajectory &traj), which takes a trajectory and returns a score. A negative score means the trajectory is invalid. For positive value, the semantics are that a trajectory with a lower score is preferrable to one with a higher score with respect to this cost function.

Each cost function also has a scale by which its impact can be altered in comparison to the other cost functions.

The base_local_planner package ships with some cost functions used on the PR2, described below.

SimpleScoredSamplingPlanner

This is a simple implementation of a trajectory search, taking a TrajectorySampleGenerator and a list of TrajectoryCostFunction. It will invoke nextTrajectory() until the generator stops generating trajectories. For each, trajectory, it will loop over the list of cost functions, adding up their positive values or aborting the scoring if one cost function returns a negative value.

Using the scale of the cost functions the result is then the Trajectory with the best weighted sum of positive cost function results.

Helper classes

LocalPlannerUtil

This helper interface provides functionality that should be common to all localplanners in the move_base context. It manages the current global plan, the current motion limits, and the current costmap (local map of sensed obstacles).

OdometryHelperRos

This class provides odometry information for ROS based robots.

LatchedStopRotateController

Ideally a local planner will make a robot stop exactly where it should. In practice however, due to sensor noise and actuator uncertainty, it may happen that the robot approaches the target spot but moves on. This can lead to undesired robot behavior of oscilatting on a spot.

The LatchedStopRotateController is a Controller that can be used as soon as the robot is close enough to the goal. The Controller will then just perform a full stop and a rotation on the spot towards the goal orientation, regardless of whether the robot position after the full stop leads the robot outside the goal position tolerance.

Cost Functions

ObstacleCostFunction

This cost function rates a trajectory based on the perceived obstacles. It either returns negative costs if the trajectory passes through an obstacle, or zero else.

MapGridCostFunction

This cost function class rates a trajectory based on how closely the trajectory follows a global path or approaches a goal point. This attempts to optimize calculation speed by using the same precomputed map of distances to a path or goal point for all trajectories.

In the dwa_local_planner, this cost function is instantiated multiple times, for different purposes. To keep the trajectory close to the path, to make the robot advance towards a local goal, and also to make the robot front ("nose") point towards a local goal. This cost function is a heuristics that can give bad results or even fail with unsuitable parameters.

OscillationCostFunction

This cost function class helps reducing certain oscillations, be returning negative costs for switches of motion directions if the last previous switch happened below a certain distance. While this effectively prevents these oscillations, but might also prevent good solutions if used with unsuitable parameters.

PreferForwardCostFunction

This cost function class was designed with robots like the PR2 in mind having good sensor coverage only in front of the robot (tilting laser). The costs function prefers motions in the front direction, penalizing backwards and strafing motions. On other robots or in other domains this may be very undesirable behavior.

TrajectoryPlannerROS

The base_local_planner::TrajectoryPlannerROS object is a wrapper for a base_local_planner::TrajectoryPlanner object that exposes its functionality as a C++ ROS Wrapper. It operates within a ROS namespace (assumed to be name from here on) specified on initialization. It adheres to the nav_core::BaseLocalPlanner interface found in the nav_core package.

Example creation of a base_local_planner::TrajectoryPlannerROS object:

   1 #include <tf/transform_listener.h>
   2 #include <costmap_2d/costmap_2d_ros.h>
   3 #include <base_local_planner/trajectory_planner_ros.h>
   4 
   5 ...
   6 
   7 tf::TransformListener tf(ros::Duration(10));
   8 costmap_2d::Costmap2DROS costmap("my_costmap", tf);
   9 
  10 base_local_planner::TrajectoryPlannerROS tp;
  11 tp.initialize("my_trajectory_planner", &tf, &costmap);

API Stability

• The C++ API is stable
• The ROS API is stable

Published Topics

• The portion of the global plan that the local planner is currently attempting to follow. Used primarily for visualization purposes.

• The local plan or trajectory that scored the highest on the last cycle. Used primarily for visualization purposes.

• The cost grid used for planning. Used for visualization purposes. See the publish_cost_grid_pc parameter for enabling/disabling this visualization. New in navigation 1.4.0

Subscribed Topics

• Odometry information that gives the local planner the current speed of the robot. The velocity information in this message is assumed to be in the same coordinate frame as the robot_base_frame of the costmap contained within the TrajectoryPlannerROS object. See the costmap_2d package for information about the robot_base_frame parameter.

Parameters

There are a large number of ROS Parameters that can be set to customize the behavior of the base_local_planner::TrajectoryPlannerROS wrapper. These parameters are grouped into several categories: robot configuration, goal tolerance, forward simulation, trajectory scoring, oscillation prevention, and global plan.

Robot Configuration Parameters

~<name>/acc_lim_x (double, default: 2.5)

• The x acceleration limit of the robot in meters/sec^2

• The y acceleration limit of the robot in meters/sec^2

• The rotational acceleration limit of the robot in radians/sec^2

• The maximum forward velocity allowed for the base in meters/sec

• The minimum forward velocity allowed for the base in meters/sec. It is useful to specify this to guarantee that velocity commands sent to a mobile base are high enough to allow the base to overcome friction.

• The maximum rotational velocity allowed for the base in radians/sec

• The minimum rotational velocity allowed for the base in radians/sec

• The minimum rotational velocity allowed for the base while performing in-place rotations in radians/sec

• DEPRECATED (use escape_vel):Speed used for backing up during escapes in meters/sec. Note that it must be negative in order for the robot to actually reverse. A positive speed will cause the robot to move forward while attempting to escape.

• Speed used for driving during escapes in meters/sec. Note that it must be negative in order for the robot to actually reverse. A positive speed will cause the robot to move forward while attempting to escape. New in navigation 1.3.1

• Determines whether velocity commands are generated for a holonomic or non-holonomic robot. For holonomic robots, strafing velocity commands may be issued to the base. For non-holonomic robots, no strafing velocity commands will be issued.

The following parameters are only used if holonomic_robot is set to true:

~<name>/y_vels (list, default: [-0.3, -0.1, 0.1, 0.3])

• The strafing velocities that a holonomic robot will consider in meters/sec

Goal Tolerance Parameters

~<name>/yaw_goal_tolerance (double, default: 0.05)

• The tolerance in radians for the controller in yaw/rotation when achieving its goal

• The tolerance in meters for the controller in the x & y distance when achieving a goal

• If goal tolerance is latched, if the robot ever reaches the goal xy location it will simply rotate in place, even if it ends up outside the goal tolerance while it is doing so. - New in navigation 1.3.1

Forward Simulation Parameters

~<name>/sim_time (double, default: 1.0)

• The amount of time to forward-simulate trajectories in seconds

• The step size, in meters, to take between points on a given trajectory

• The step size, in radians, to take between angular samples on a given trajectory. - New in navigation 1.3.1

• The number of samples to use when exploring the x velocity space

• The number of samples to use when exploring the theta velocity space

• The frequency at which this controller will be called in Hz. Uses searchParam to read the parameter from parent namespaces if not set in the namespace of the controller. For use with move_base, this means that you only need to set its "controller_frequency" parameter and can safely leave this one unset. - New in navigation 1.3.1

Trajectory Scoring Parameters

The cost function used to score each trajectory is in the following form:

cost = 
  pdist_scale * (distance to path from the endpoint of the trajectory in map cells or meters depending on the meter_scoring parameter) 
  + gdist_scale * (distance to local goal from the endpoint of the trajectory in map cells or meters depending on the meter_scoring parameter) 
  + occdist_scale * (maximum obstacle cost along the trajectory in obstacle cost (0-254))

~<name>/meter_scoring (bool, default: false)

• Whether the gdist_scale and pdist_scale parameters should assume that goal_distance and path_distance are expressed in units of meters or cells. Cells are assumed by default. New in navigation 1.3.1

• The weighting for how much the controller should stay close to the path it was given, maximal possible value is 5.0

• The weighting for how much the controller should attempt to reach its local goal, also controls speed, maximal possible value is 5.0

• The weighting for how much the controller should attempt to avoid obstacles

• How far to look ahead in meters when scoring different in-place-rotation trajectories

• Whether to score based on the robot's heading to the path or its distance from the path

• How far to look ahead in time in seconds along the simulated trajectory when using heading scoring

• Whether to use the Dynamic Window Approach (DWA)_ or whether to use Trajectory Rollout (NOTE: In our experience DWA worked as well as Trajectory Rollout and is computationally less expensive. It is possible that robots with extremely poor acceleration limits could gain from running Trajectory Rollout, but we recommend trying DWA first.)

• Whether or not to publish the cost grid that the planner will use when planning. When true, a sensor_msgs/PointCloud2 will be available on the ~<name>/cost_cloud topic. Each point cloud represents the cost grid and has a field for each individual scoring function component as well as the overall cost for each cell, taking the scoring parameters into account. New in navigation 1.4.0

• The frame to set for the cost_cloud. Should be set to the same frame as the local costmap's global frame. New in navigation 1.4.0

Oscillation Prevention Parameters

~<name>/oscillation_reset_dist (double, default: 0.05)

• How far the robot must travel in meters before oscillation flags are reset

Global Plan Parameters

~<name>/prune_plan (bool, default: true)

• Defines whether or not to eat up the plan as the robot moves along the path. If set to true, points will fall off the end of the plan once the robot moves 1 meter past them.

TrajectoryPlanner

The base_local_planner::TrajectoryPlanner provides implementations of the DWA and Trajectory Rollout algorithms described earlier. In order to use the base_local_planner::TrajectoryPlanner with ROS, please use the TrajectoryPlannerROS wrapper. It is not recommended to use the base_local_planner::TrajectoryPlanner on its own.

API Stability

The C++ API is stable. However, it is recommended that you use the TrajectoryPlannerROS wrapper instead of using the base_local_planner::TrajectoryPlanner on its own.