Public Member Functions | Protected Member Functions | Protected Attributes
teb_local_planner::TebOptimalPlanner Class Reference

This class optimizes an internal Timed Elastic Band trajectory using the g2o-framework. More...

#include <optimal_planner.h>

Inheritance diagram for teb_local_planner::TebOptimalPlanner:
Inheritance graph
[legend]

List of all members.

Public Member Functions

void initialize (const TebConfig &cfg, ObstContainer *obstacles=NULL, RobotFootprintModelPtr robot_model=boost::make_shared< PointRobotFootprint >(), TebVisualizationPtr visual=TebVisualizationPtr(), const ViaPointContainer *via_points=NULL)
 Initializes the optimal planner.
 TebOptimalPlanner ()
 Default constructor.
 TebOptimalPlanner (const TebConfig &cfg, ObstContainer *obstacles=NULL, RobotFootprintModelPtr robot_model=boost::make_shared< PointRobotFootprint >(), TebVisualizationPtr visual=TebVisualizationPtr(), const ViaPointContainer *via_points=NULL)
 Construct and initialize the TEB optimal planner.
virtual ~TebOptimalPlanner ()
 Destruct the optimal planner.
Plan a trajectory
virtual bool plan (const std::vector< geometry_msgs::PoseStamped > &initial_plan, const geometry_msgs::Twist *start_vel=NULL, bool free_goal_vel=false)
 Plan a trajectory based on an initial reference plan.
virtual bool plan (const tf::Pose &start, const tf::Pose &goal, const geometry_msgs::Twist *start_vel=NULL, bool free_goal_vel=false)
 Plan a trajectory between a given start and goal pose (tf::Pose version)
virtual bool plan (const PoseSE2 &start, const PoseSE2 &goal, const geometry_msgs::Twist *start_vel=NULL, bool free_goal_vel=false)
 Plan a trajectory between a given start and goal pose.
virtual bool getVelocityCommand (double &vx, double &vy, double &omega) const
 Get the velocity command from a previously optimized plan to control the robot at the current sampling interval.
bool optimizeTEB (int iterations_innerloop, int iterations_outerloop, bool compute_cost_afterwards=false, double obst_cost_scale=1.0, double viapoint_cost_scale=1.0, bool alternative_time_cost=false)
 Optimize a previously initialized trajectory (actual TEB optimization loop).
Desired initial and final velocity
void setVelocityStart (const geometry_msgs::Twist &vel_start)
 Set the initial velocity at the trajectory's start pose (e.g. the robot's velocity) [twist overload].
void setVelocityGoal (const geometry_msgs::Twist &vel_goal)
 Set the desired final velocity at the trajectory's goal pose.
void setVelocityGoalFree ()
 Set the desired final velocity at the trajectory's goal pose to be the maximum velocity limit.
Take obstacles into account
void setObstVector (ObstContainer *obst_vector)
 Assign a new set of obstacles.
const ObstContainer & getObstVector () const
 Access the internal obstacle container.
Take via-points into account
void setViaPoints (const ViaPointContainer *via_points)
 Assign a new set of via-points.
const ViaPointContainer & getViaPoints () const
 Access the internal via-point container.
Visualization
void setVisualization (TebVisualizationPtr visualization)
 Register a TebVisualization class to enable visiualization routines (e.g. publish the local plan and pose sequence)
virtual void visualize ()
 Publish the local plan and pose sequence via ros topics (e.g. subscribe with rviz).

Protected Member Functions

boost::shared_ptr
< g2o::SparseOptimizer > 		initOptimizer ()
 Initialize and configure the g2o sparse optimizer.
Hyper-Graph creation and optimization
bool buildGraph (double weight_multiplier=1.0)
 Build the hyper-graph representing the TEB optimization problem.
bool optimizeGraph (int no_iterations, bool clear_after=true)
 Optimize the previously constructed hyper-graph to deform / optimize the TEB.
void clearGraph ()
 Clear an existing internal hyper-graph.
void AddTEBVertices ()
 Add all relevant vertices to the hyper-graph as optimizable variables.
void AddEdgesVelocity ()
 Add all edges (local cost functions) for limiting the translational and angular velocity.
void AddEdgesAcceleration ()
 Add all edges (local cost functions) for limiting the translational and angular acceleration.
void AddEdgesTimeOptimal ()
 Add all edges (local cost functions) for minimizing the transition time (resp. minimize time differences)
void AddEdgesObstacles (double weight_multiplier=1.0)
 Add all edges (local cost functions) related to keeping a distance from static obstacles.
void AddEdgesObstaclesLegacy (double weight_multiplier=1.0)
 Add all edges (local cost functions) related to keeping a distance from static obstacles (legacy association strategy)
void AddEdgesViaPoints ()
 Add all edges (local cost functions) related to minimizing the distance to via-points.
void AddEdgesDynamicObstacles ()
 Add all edges (local cost functions) related to keeping a distance from dynamic (moving) obstacles.
void AddEdgesKinematicsDiffDrive ()
 Add all edges (local cost functions) for satisfying kinematic constraints of a differential drive robot.
void AddEdgesKinematicsCarlike ()
 Add all edges (local cost functions) for satisfying kinematic constraints of a carlike robot.

Protected Attributes

const TebConfig * cfg_
 Config class that stores and manages all related parameters.
double cost_
 Store cost value of the current hyper-graph.
bool initialized_
 Keeps track about the correct initialization of this class.
ObstContainer * obstacles_
 Store obstacles that are relevant for planning.
bool optimized_
 This variable is true as long as the last optimization has been completed successful.
boost::shared_ptr
< g2o::SparseOptimizer > 		optimizer_
 g2o optimizer for trajectory optimization
RobotFootprintModelPtr robot_model_
 Robot model.
TimedElasticBand teb_
 Actual trajectory object.
std::pair< bool,
geometry_msgs::Twist > 		vel_goal_
 Store the final velocity at the goal pose.
std::pair< bool,
geometry_msgs::Twist > 		vel_start_
 Store the initial velocity at the start pose.
const ViaPointContainer * via_points_
 Store via points for planning.
TebVisualizationPtr visualization_
 Instance of the visualization class.

Utility methods and more

virtual void clearPlanner ()
 Reset the planner by clearing the internal graph and trajectory.
TimedElasticBand & teb ()
 Access the internal TimedElasticBand trajectory.
const TimedElasticBand & teb () const
 Access the internal TimedElasticBand trajectory (read-only).
boost::shared_ptr
< g2o::SparseOptimizer > 		optimizer ()
 Access the internal g2o optimizer.
boost::shared_ptr< const
g2o::SparseOptimizer > 		optimizer () const
 Access the internal g2o optimizer (read-only).
bool isOptimized () const
 Check if last optimization was successful.
void computeCurrentCost (double obst_cost_scale=1.0, double viapoint_cost_scale=1.0, bool alternative_time_cost=false)
 Compute the cost vector of a given optimization problen (hyper-graph must exist).
virtual void computeCurrentCost (std::vector< double > &cost, double obst_cost_scale=1.0, double viapoint_cost_scale=1.0, bool alternative_time_cost=false)
double getCurrentCost () const
 Access the cost vector.
void extractVelocity (const PoseSE2 &pose1, const PoseSE2 &pose2, double dt, double &vx, double &vy, double &omega) const
 Extract the velocity from consecutive poses and a time difference (including strafing velocity for holonomic robots)
void getVelocityProfile (std::vector< geometry_msgs::Twist > &velocity_profile) const
 Compute the velocity profile of the trajectory.
void getFullTrajectory (std::vector< TrajectoryPointMsg > &trajectory) const
 Return the complete trajectory including poses, velocity profiles and temporal information.
virtual bool isTrajectoryFeasible (base_local_planner::CostmapModel *costmap_model, const std::vector< geometry_msgs::Point > &footprint_spec, double inscribed_radius=0.0, double circumscribed_radius=0.0, int look_ahead_idx=-1)
 Check whether the planned trajectory is feasible or not.
virtual bool isHorizonReductionAppropriate (const std::vector< geometry_msgs::PoseStamped > &initial_plan) const
 Check if the planner suggests a shorter horizon (e.g. to resolve problems)
static void registerG2OTypes ()
 Register the vertices and edges defined for the TEB to the g2o::Factory.

Detailed Description

This class optimizes an internal Timed Elastic Band trajectory using the g2o-framework.

For an introduction and further details about the TEB optimization problem refer to:

Todo:
: Call buildGraph() only if the teb structure has been modified to speed up hot-starting from previous solutions.

Definition at line 105 of file optimal_planner.h.

Constructor & Destructor Documentation

teb_local_planner::TebOptimalPlanner::TebOptimalPlanner ( )

Default constructor.

Definition at line 49 of file optimal_planner.cpp.

teb_local_planner::TebOptimalPlanner::TebOptimalPlanner ( const TebConfig &  cfg,
ObstContainer *  obstacles = NULL,
RobotFootprintModelPtr  robot_model = boost::make_shared<PointRobotFootprint>(),
TebVisualizationPtr  visual = TebVisualizationPtr(),
const ViaPointContainer *  via_points = NULL 
)

Construct and initialize the TEB optimal planner.

Parameters:
cfgConst reference to the TebConfig class for internal parameters
obstaclesContainer storing all relevant obstacles (see Obstacle)
robot_modelShared pointer to the robot shape model used for optimization (optional)
visualShared pointer to the TebVisualization class (optional)
via_pointsContainer storing via-points (optional)

Definition at line 53 of file optimal_planner.cpp.

teb_local_planner::TebOptimalPlanner::~TebOptimalPlanner ( ) [virtual]

Destruct the optimal planner.

Definition at line 58 of file optimal_planner.cpp.

Member Function Documentation

void teb_local_planner::TebOptimalPlanner::AddEdgesAcceleration ( ) [protected]

Add all edges (local cost functions) for limiting the translational and angular acceleration.

See also:
EdgeAcceleration
EdgeAccelerationStart
EdgeAccelerationGoal
buildGraph
optimizeGraph

Definition at line 726 of file optimal_planner.cpp.

void teb_local_planner::TebOptimalPlanner::AddEdgesDynamicObstacles ( ) [protected]

Add all edges (local cost functions) related to keeping a distance from dynamic (moving) obstacles.

Warning:
experimental
See also:
EdgeDynamicObstacle
buildGraph
optimizeGraph

Definition at line 612 of file optimal_planner.cpp.

void teb_local_planner::TebOptimalPlanner::AddEdgesKinematicsCarlike ( ) [protected]

Add all edges (local cost functions) for satisfying kinematic constraints of a carlike robot.

Warning:
do not combine with AddEdgesKinematicsDiffDrive()
See also:
AddEdgesKinematicsDiffDrive
buildGraph
optimizeGraph

Definition at line 874 of file optimal_planner.cpp.

void teb_local_planner::TebOptimalPlanner::AddEdgesKinematicsDiffDrive ( ) [protected]

Add all edges (local cost functions) for satisfying kinematic constraints of a differential drive robot.

Warning:
do not combine with AddEdgesKinematicsCarlike()
See also:
AddEdgesKinematicsCarlike
buildGraph
optimizeGraph

Definition at line 852 of file optimal_planner.cpp.

void teb_local_planner::TebOptimalPlanner::AddEdgesObstacles ( double  weight_multiplier = 1.0) [protected]

Add all edges (local cost functions) related to keeping a distance from static obstacles.

Warning:
do not combine with AddEdgesInflatedObstacles <<<<<<< HEAD =======
See also:
EdgeObstacle
buildGraph
optimizeGraph
Parameters:
weight_multiplierSpecify an additional weight multipler (in addition to the the config weight)

Definition at line 388 of file optimal_planner.cpp.

void teb_local_planner::TebOptimalPlanner::AddEdgesObstaclesLegacy ( double  weight_multiplier = 1.0) [protected]

Add all edges (local cost functions) related to keeping a distance from static obstacles (legacy association strategy)

Warning:
do not combine with AddEdgesInflatedObstacles
See also:
EdgeObstacle
buildGraph
optimizeGraph
Parameters:
weight_multiplierSpecify an additional weight multipler (in addition to the the config weight)

Definition at line 517 of file optimal_planner.cpp.

void teb_local_planner::TebOptimalPlanner::AddEdgesTimeOptimal ( ) [protected]

Add all edges (local cost functions) for minimizing the transition time (resp. minimize time differences)

See also:
EdgeTimeOptimal
buildGraph
optimizeGraph

Definition at line 832 of file optimal_planner.cpp.

void teb_local_planner::TebOptimalPlanner::AddEdgesVelocity ( ) [protected]

Add all edges (local cost functions) for limiting the translational and angular velocity.

See also:
EdgeVelocity
buildGraph
optimizeGraph

Definition at line 675 of file optimal_planner.cpp.

void teb_local_planner::TebOptimalPlanner::AddEdgesViaPoints ( ) [protected]

Add all edges (local cost functions) related to minimizing the distance to via-points.

See also:
EdgeViaPoint
buildGraph
optimizeGraph

Definition at line 639 of file optimal_planner.cpp.

void teb_local_planner::TebOptimalPlanner::AddTEBVertices ( ) [protected]

Add all relevant vertices to the hyper-graph as optimizable variables.

Vertices (if unfixed) represent the variables that will be optimized.
In case of the Timed-Elastic-Band poses and time differences form the vertices of the hyper-graph.
The order of insertion of vertices (to the graph) is important for efficiency, since it affect the sparsity pattern of the underlying hessian computed for optimization.

See also:
VertexPose
VertexTimeDiff
buildGraph
optimizeGraph

Definition at line 370 of file optimal_planner.cpp.

bool teb_local_planner::TebOptimalPlanner::buildGraph ( double  weight_multiplier = 1.0) [protected]

Build the hyper-graph representing the TEB optimization problem.

This method creates the optimization problem according to the hyper-graph formulation.
For more details refer to the literature cited in the TebOptimalPlanner class description.

See also:
optimizeGraph
clearGraph
Parameters:
weight_multiplierSpecify a weight multipler for selected weights in optimizeGraph This might be used for weight adapation strategies. Currently, only obstacle collision weights are considered.
Returns:
true, if the graph was created successfully, false otherwise.

Definition at line 293 of file optimal_planner.cpp.

void teb_local_planner::TebOptimalPlanner::clearGraph ( ) [protected]

Clear an existing internal hyper-graph.

See also:
buildGraph
optimizeGraph

Definition at line 360 of file optimal_planner.cpp.

virtual void teb_local_planner::TebOptimalPlanner::clearPlanner ( ) [inline, virtual]

Reset the planner by clearing the internal graph and trajectory.

Implements teb_local_planner::PlannerInterface.

Definition at line 339 of file optimal_planner.h.

void teb_local_planner::TebOptimalPlanner::computeCurrentCost ( double  obst_cost_scale = 1.0,
double  viapoint_cost_scale = 1.0,
bool  alternative_time_cost = false 
)

Compute the cost vector of a given optimization problen (hyper-graph must exist).

Use this method to obtain information about the current edge errors / costs (local cost functions).
The vector of cost values is composed according to the different edge types (time_optimal, obstacles, ...).
Refer to the method declaration for the detailed composition.
The cost for the edges that minimize time differences (EdgeTimeOptimal) corresponds to the sum of all single squared time differneces: $ \sum_i \Delta T_i^2 $. Sometimes, the user may want to get a value that is proportional or identical to the actual trajectory transition time $ \sum_i \Delta T_i $.
Set alternative_time_cost to true in order to get the cost calculated using the latter equation, but check the implemented definition, if the value is scaled to match the magnitude of other cost values.

Todo:

Remove the scaling term for the alternative time cost.

Can we use the last error (chi2) calculated from g2o instead of calculating it by ourself?

See also:
getCurrentCost
optimizeTEB
Parameters:
obst_cost_scaleSpecify extra scaling for obstacle costs.
viapoint_cost_scaleSpecify extra scaling for via points.
alternative_time_costReplace the cost for the time optimal objective by the actual (weighted) transition time.
Returns:
TebCostVec containing the cost values

Definition at line 897 of file optimal_planner.cpp.

virtual void teb_local_planner::TebOptimalPlanner::computeCurrentCost ( std::vector< double > &  cost,
double  obst_cost_scale = 1.0,
double  viapoint_cost_scale = 1.0,
bool  alternative_time_cost = false 
) [inline, virtual]

Compute and return the cost of the current optimization graph (supports multiple trajectories)

Parameters:
[out]costcurrent cost value for each trajectory [for a planner with just a single trajectory: size=1, vector will not be cleared]
obst_cost_scaleSpecify extra scaling for obstacle costs
viapoint_cost_scaleSpecify extra scaling for via points.
alternative_time_costReplace the cost for the time optimal objective by the actual (weighted) transition time

Definition at line 417 of file optimal_planner.h.

void teb_local_planner::TebOptimalPlanner::extractVelocity ( const PoseSE2 &  pose1,
const PoseSE2 &  pose2,
double  dt,
double &  vx,
double &  vy,
double &  omega 
) const [inline]

Extract the velocity from consecutive poses and a time difference (including strafing velocity for holonomic robots)

The velocity is extracted using finite differences. The direction of the translational velocity is also determined.

Parameters:
pose1pose at time k
pose2consecutive pose at time k+1
dtactual time difference between k and k+1 (must be >0 !!!)
[out]vxtranslational velocity
[out]vystrafing velocity which can be nonzero for holonomic robots
[out]omegarotational velocity

Definition at line 999 of file optimal_planner.cpp.

double teb_local_planner::TebOptimalPlanner::getCurrentCost ( ) const [inline]

Access the cost vector.

The accumulated cost value previously calculated using computeCurrentCost or by calling optimizeTEB with enabled cost flag.

Returns:
const reference to the TebCostVec.

Definition at line 430 of file optimal_planner.h.

void teb_local_planner::TebOptimalPlanner::getFullTrajectory ( std::vector< TrajectoryPointMsg > &  trajectory) const

Return the complete trajectory including poses, velocity profiles and temporal information.

It is useful for evaluation and debugging purposes or for open-loop control. Since the velocity obtained using difference quotients is the mean velocity between consecutive poses, the velocity at each pose at time stamp k is obtained by taking the average between both velocities. The velocity of the first pose is v_start (provided initial value) and the last one is v_goal (usually zero, if free_goal_vel is off). See getVelocityProfile() for the list of velocities between consecutive points.

Todo:
The acceleration profile is not added at the moment.
Parameters:
[out]trajectorythe resulting trajectory

Definition at line 1090 of file optimal_planner.cpp.

const ObstContainer& teb_local_planner::TebOptimalPlanner::getObstVector ( ) const [inline]

Access the internal obstacle container.

Returns:
Const reference to the obstacle container

Definition at line 288 of file optimal_planner.h.

bool teb_local_planner::TebOptimalPlanner::getVelocityCommand ( double &  vx,
double &  vy,
double &  omega 
) const [virtual]

Get the velocity command from a previously optimized plan to control the robot at the current sampling interval.

Warning:
Call plan() first and check if the generated plan is feasible.
Parameters:
[out]vxtranslational velocity [m/s]
[out]vystrafing velocity which can be nonzero for holonomic robots[m/s]
[out]omegarotational velocity [rad/s]
Returns:
true if command is valid, false otherwise

Implements teb_local_planner::PlannerInterface.

Definition at line 1037 of file optimal_planner.cpp.

void teb_local_planner::TebOptimalPlanner::getVelocityProfile ( std::vector< geometry_msgs::Twist > &  velocity_profile) const

Compute the velocity profile of the trajectory.

This method computes the translational and rotational velocity for the complete planned trajectory. The first velocity is the one that is provided as initial velocity (fixed). Velocities at index k=2...end-1 are related to the transition from pose_{k-1} to pose_k. The last velocity is the final velocity (fixed). The number of Twist objects is therefore sizePoses()+1; In summary: v[0] = v_start, v[1,...end-1] = +-(pose_{k+1}-pose{k})/dt, v(end) = v_goal It can be used for evaluation and debugging purposes or for open-loop control. For computing the velocity required for controlling the robot to the next step refer to getVelocityCommand().

Parameters:
[out]velocity_profilevelocity profile will be written to this vector (after clearing any existing content) with the size=no_poses+1

Definition at line 1063 of file optimal_planner.cpp.

const ViaPointContainer& teb_local_planner::TebOptimalPlanner::getViaPoints ( ) const [inline]

Access the internal via-point container.

Returns:
Const reference to the via-point container

Definition at line 307 of file optimal_planner.h.

void teb_local_planner::TebOptimalPlanner::initialize ( const TebConfig &  cfg,
ObstContainer *  obstacles = NULL,
RobotFootprintModelPtr  robot_model = boost::make_shared<PointRobotFootprint>(),
TebVisualizationPtr  visual = TebVisualizationPtr(),
const ViaPointContainer *  via_points = NULL 
)

Initializes the optimal planner.

Parameters:
cfgConst reference to the TebConfig class for internal parameters
obstaclesContainer storing all relevant obstacles (see Obstacle)
robot_modelShared pointer to the robot shape model used for optimization (optional)
visualShared pointer to the TebVisualization class (optional)
via_pointsContainer storing via-points (optional)

Definition at line 68 of file optimal_planner.cpp.

boost::shared_ptr< g2o::SparseOptimizer > teb_local_planner::TebOptimalPlanner::initOptimizer ( ) [protected]

Initialize and configure the g2o sparse optimizer.

Returns:
shared pointer to the g2o::SparseOptimizer instance

Definition at line 145 of file optimal_planner.cpp.

bool teb_local_planner::TebOptimalPlanner::isHorizonReductionAppropriate ( const std::vector< geometry_msgs::PoseStamped > &  initial_plan) const [virtual]

Check if the planner suggests a shorter horizon (e.g. to resolve problems)

This method is intendend to be called after determining that a trajectory provided by the planner is infeasible. In some cases a reduction of the horizon length might resolve problems. E.g. if a planned trajectory cut corners. Implemented cases for returning true (remaining length must be larger than 2m to trigger any case):

  • Goal orientation - start orientation > 90°
  • Goal heading - start orientation > 90°
  • The planned trajectory is at least 30° shorter than the initial plan (accumulated euclidean distances)
  • Distance between consecutive poses > 0.9*min_obstacle_dist
    Parameters:
    initial_planThe intial and transformed plan (part of the local map and pruned up to the robot position)
    Returns:
    true, if the planner suggests a shorter horizon, false otherwise.

Reimplemented from teb_local_planner::PlannerInterface.

Definition at line 1172 of file optimal_planner.cpp.

bool teb_local_planner::TebOptimalPlanner::isOptimized ( ) const [inline]

Check if last optimization was successful.

Returns:
true if the last optimization returned without errors, otherwise false (also if no optimization has been called before).

Definition at line 384 of file optimal_planner.h.

bool teb_local_planner::TebOptimalPlanner::isTrajectoryFeasible ( base_local_planner::CostmapModel *  costmap_model,
const std::vector< geometry_msgs::Point > &  footprint_spec,
double  inscribed_radius = 0.0,
double  circumscribed_radius = 0.0,
int  look_ahead_idx = -1 
) [virtual]

Check whether the planned trajectory is feasible or not.

This method currently checks only that the trajectory, or a part of the trajectory is collision free. Obstacles are here represented as costmap instead of the internal ObstacleContainer.

Parameters:
costmap_modelPointer to the costmap model
footprint_specThe specification of the footprint of the robot in world coordinates
inscribed_radiusThe radius of the inscribed circle of the robot
circumscribed_radiusThe radius of the circumscribed circle of the robot
look_ahead_idxNumber of poses along the trajectory that should be verified, if -1, the complete trajectory will be checked.
Returns:
true, if the robot footprint along the first part of the trajectory intersects with any obstacle in the costmap, false otherwise.

Implements teb_local_planner::PlannerInterface.

Definition at line 1143 of file optimal_planner.cpp.

bool teb_local_planner::TebOptimalPlanner::optimizeGraph ( int  no_iterations,
bool  clear_after = true 
) [protected]

Optimize the previously constructed hyper-graph to deform / optimize the TEB.

This method invokes the g2o framework to solve the optimization problem considering dedicated sparsity patterns.
The current implementation calls a non-constrained sparse Levenberg-Marquardt algorithm. Constraints are considered by utilizing penalty approximations. Refer to the literature cited in the TebOptimalPlanner class description.

See also:
buildGraph
clearGraph
Parameters:
no_iterationsNumber of solver iterations
clear_afterClear the graph after optimization.
Returns:
true, if optimization terminates successfully, false otherwise.

Definition at line 328 of file optimal_planner.cpp.

boost::shared_ptr<g2o::SparseOptimizer> teb_local_planner::TebOptimalPlanner::optimizer ( ) [inline]

Access the internal g2o optimizer.

Warning:
In general, the underlying optimizer must not be modified directly. Use with care...
Returns:
const shared pointer to the g2o sparse optimizer

Definition at line 371 of file optimal_planner.h.

boost::shared_ptr<const g2o::SparseOptimizer> teb_local_planner::TebOptimalPlanner::optimizer ( ) const [inline]

Access the internal g2o optimizer (read-only).

Returns:
const shared pointer to the g2o sparse optimizer

Definition at line 377 of file optimal_planner.h.

bool teb_local_planner::TebOptimalPlanner::optimizeTEB ( int  iterations_innerloop,
int  iterations_outerloop,
bool  compute_cost_afterwards = false,
double  obst_cost_scale = 1.0,
double  viapoint_cost_scale = 1.0,
bool  alternative_time_cost = false 
)

Optimize a previously initialized trajectory (actual TEB optimization loop).

optimizeTEB implements the main optimization loop.
It consist of two nested loops:

  • The outer loop resizes the trajectory according to the temporal resolution by invoking TimedElasticBand::autoResize(). Afterwards the internal method optimizeGraph() is called that constitutes the innerloop.
  • The inner loop calls the solver (g2o framework, resp. sparse Levenberg-Marquardt) and iterates a specified number of optimization calls (iterations_innerloop).

The outer loop is repeated iterations_outerloop times.
The ratio of inner and outer loop iterations significantly defines the contraction behavior and convergence rate of the trajectory optimization. Based on our experiences, 2-6 innerloop iterations are sufficient.
The number of outer loop iterations should be determined by considering the maximum CPU time required to match the control rate.
Optionally, the cost vector can be calculated by specifying compute_cost_afterwards, see computeCurrentCost().

Remarks:
This method is usually called from a plan() method
Parameters:
iterations_innerloopNumber of iterations for the actual solver loop
iterations_outerloopSpecifies how often the trajectory should be resized followed by the inner solver loop.
compute_cost_afterwardsif true Calculate the cost vector according to computeCurrentCost(), the vector can be accessed afterwards using getCurrentCost().
obst_cost_scaleSpecify extra scaling for obstacle costs (only used if compute_cost_afterwards is true)
viapoint_cost_scaleSpecify extra scaling for via-point costs (only used if compute_cost_afterwards is true)
alternative_time_costReplace the cost for the time optimal objective by the actual (weighted) transition time (only used if compute_cost_afterwards is true).
Returns:
true if the optimization terminates successfully, false otherwise

Definition at line 166 of file optimal_planner.cpp.

bool teb_local_planner::TebOptimalPlanner::plan ( const std::vector< geometry_msgs::PoseStamped > &  initial_plan,
const geometry_msgs::Twist *  start_vel = NULL,
bool  free_goal_vel = false 
) [virtual]

Plan a trajectory based on an initial reference plan.

Call this method to create and optimize a trajectory that is initialized according to an initial reference plan (given as a container of poses).
The method supports hot-starting from previous solutions, if avaiable:

  • If no trajectory exist yet, a new trajectory is initialized based on the initial plan, see TimedElasticBand::initTEBtoGoal
  • If a previous solution is avaiable, update the trajectory based on the initial plan, see bool TimedElasticBand::updateAndPruneTEB
  • Afterwards optimize the recently initialized or updated trajectory by calling optimizeTEB() and invoking g2o
    Parameters:
    initial_planvector of geometry_msgs::PoseStamped
    start_velCurrent start velocity (e.g. the velocity of the robot, only linear.x, linear.y (holonomic) and angular.z are used)
    free_goal_velif true, a nonzero final velocity at the goal pose is allowed, otherwise the final velocity will be zero (default: false)
    Returns:
    true if planning was successful, false otherwise

Implements teb_local_planner::PlannerInterface.

Definition at line 222 of file optimal_planner.cpp.

bool teb_local_planner::TebOptimalPlanner::plan ( const tf::Pose &  start,
const tf::Pose &  goal,
const geometry_msgs::Twist *  start_vel = NULL,
bool  free_goal_vel = false 
) [virtual]

Plan a trajectory between a given start and goal pose (tf::Pose version)

Call this method to create and optimize a trajectory that is initialized between a given start and goal pose.
The method supports hot-starting from previous solutions, if avaiable:

  • If no trajectory exist yet, a new trajectory is initialized between start and goal poses, see TimedElasticBand::initTEBtoGoal
  • If a previous solution is avaiable, update the trajectory
    See also:
    bool TimedElasticBand::updateAndPruneTEB
  • Afterwards optimize the recently initialized or updated trajectory by calling optimizeTEB() and invoking g2o
    Parameters:
    starttf::Pose containing the start pose of the trajectory
    goaltf::Pose containing the goal pose of the trajectory
    start_velCurrent start velocity (e.g. the velocity of the robot, only linear.x, linear.y (holonomic) and angular.z are used)
    free_goal_velif true, a nonzero final velocity at the goal pose is allowed, otherwise the final velocity will be zero (default: false)
    Returns:
    true if planning was successful, false otherwise

Implements teb_local_planner::PlannerInterface.

Definition at line 255 of file optimal_planner.cpp.

bool teb_local_planner::TebOptimalPlanner::plan ( const PoseSE2 &  start,
const PoseSE2 &  goal,
const geometry_msgs::Twist *  start_vel = NULL,
bool  free_goal_vel = false 
) [virtual]

Plan a trajectory between a given start and goal pose.

Call this method to create and optimize a trajectory that is initialized between a given start and goal pose.
The method supports hot-starting from previous solutions, if avaiable:

  • If no trajectory exist yet, a new trajectory is initialized between start and goal poses
    See also:
    TimedElasticBand::initTEBtoGoal
  • If a previous solution is avaiable, update the trajectory
    See also:
    bool TimedElasticBand::updateAndPruneTEB
  • Afterwards optimize the recently initialized or updated trajectory by calling optimizeTEB() and invoking g2o
    Parameters:
    startPoseSE2 containing the start pose of the trajectory
    goalPoseSE2 containing the goal pose of the trajectory
    start_velInitial velocity at the start pose (twist message containing the translational and angular velocity).
    free_goal_velif true, a nonzero final velocity at the goal pose is allowed, otherwise the final velocity will be zero (default: false)
    Returns:
    true if planning was successful, false otherwise

Implements teb_local_planner::PlannerInterface.

Definition at line 262 of file optimal_planner.cpp.

void teb_local_planner::TebOptimalPlanner::registerG2OTypes ( ) [static]

Register the vertices and edges defined for the TEB to the g2o::Factory.

This allows the user to export the internal graph to a text file for instance. Access the optimizer() for more details.

Definition at line 117 of file optimal_planner.cpp.

void teb_local_planner::TebOptimalPlanner::setObstVector ( ObstContainer *  obst_vector) [inline]

Assign a new set of obstacles.

Parameters:
obst_vectorpointer to an obstacle container (can also be a nullptr)
Remarks:
This method overrids the obstacle container optinally assigned in the constructor.

Definition at line 282 of file optimal_planner.h.

void teb_local_planner::TebOptimalPlanner::setVelocityGoal ( const geometry_msgs::Twist &  vel_goal)

Set the desired final velocity at the trajectory's goal pose.

Remarks:
Call this function only if a non-zero velocity is desired and if free_goal_vel is set to false in plan()
Parameters:
vel_goaltwist message containing the translational and angular final velocity

Definition at line 216 of file optimal_planner.cpp.

void teb_local_planner::TebOptimalPlanner::setVelocityGoalFree ( ) [inline]

Set the desired final velocity at the trajectory's goal pose to be the maximum velocity limit.

Remarks:
Calling this function is not neccessary if free_goal_vel is set to false in plan()

Definition at line 268 of file optimal_planner.h.

void teb_local_planner::TebOptimalPlanner::setVelocityStart ( const geometry_msgs::Twist &  vel_start)

Set the initial velocity at the trajectory's start pose (e.g. the robot's velocity) [twist overload].

Remarks:
Calling this function is not neccessary if the initial velocity is passed via the plan() method
Parameters:
vel_startCurrent start velocity (e.g. the velocity of the robot, only linear.x and angular.z are used, for holonomic robots also linear.y)

Definition at line 208 of file optimal_planner.cpp.

void teb_local_planner::TebOptimalPlanner::setViaPoints ( const ViaPointContainer *  via_points) [inline]

Assign a new set of via-points.

Parameters:
via_pointspointer to a via_point container (can also be a nullptr)

Any previously set container will be overwritten.

Definition at line 301 of file optimal_planner.h.

void teb_local_planner::TebOptimalPlanner::setVisualization ( TebVisualizationPtr  visualization)

Register a TebVisualization class to enable visiualization routines (e.g. publish the local plan and pose sequence)

Parameters:
visualizationshared pointer to a TebVisualization instance
See also:
visualize

Definition at line 93 of file optimal_planner.cpp.

TimedElasticBand& teb_local_planner::TebOptimalPlanner::teb ( ) [inline]

Access the internal TimedElasticBand trajectory.

Warning:
In general, the underlying teb must not be modified directly. Use with care...
Returns:
reference to the teb

Definition at line 358 of file optimal_planner.h.

const TimedElasticBand& teb_local_planner::TebOptimalPlanner::teb ( ) const [inline]

Access the internal TimedElasticBand trajectory (read-only).

Returns:
const reference to the teb

Definition at line 364 of file optimal_planner.h.

void teb_local_planner::TebOptimalPlanner::visualize ( ) [virtual]

Publish the local plan and pose sequence via ros topics (e.g. subscribe with rviz).

Make sure to register a TebVisualization instance before using setVisualization() or an overlaoded constructor.

See also:
setVisualization

Reimplemented from teb_local_planner::PlannerInterface.

Definition at line 98 of file optimal_planner.cpp.

Member Data Documentation

const TebConfig* teb_local_planner::TebOptimalPlanner::cfg_ [protected]

Config class that stores and manages all related parameters.

Definition at line 662 of file optimal_planner.h.

double teb_local_planner::TebOptimalPlanner::cost_ [protected]

Store cost value of the current hyper-graph.

Definition at line 666 of file optimal_planner.h.

bool teb_local_planner::TebOptimalPlanner::initialized_ [protected]

Keeps track about the correct initialization of this class.

Definition at line 676 of file optimal_planner.h.

ObstContainer* teb_local_planner::TebOptimalPlanner::obstacles_ [protected]

Store obstacles that are relevant for planning.

Definition at line 663 of file optimal_planner.h.

bool teb_local_planner::TebOptimalPlanner::optimized_ [protected]

This variable is true as long as the last optimization has been completed successful.

Definition at line 677 of file optimal_planner.h.

boost::shared_ptr<g2o::SparseOptimizer> teb_local_planner::TebOptimalPlanner::optimizer_ [protected]

g2o optimizer for trajectory optimization

Definition at line 672 of file optimal_planner.h.

RobotFootprintModelPtr teb_local_planner::TebOptimalPlanner::robot_model_ [protected]

Robot model.

Definition at line 671 of file optimal_planner.h.

TimedElasticBand teb_local_planner::TebOptimalPlanner::teb_ [protected]

Actual trajectory object.

Definition at line 670 of file optimal_planner.h.

std::pair<bool, geometry_msgs::Twist> teb_local_planner::TebOptimalPlanner::vel_goal_ [protected]

Store the final velocity at the goal pose.

Definition at line 674 of file optimal_planner.h.

std::pair<bool, geometry_msgs::Twist> teb_local_planner::TebOptimalPlanner::vel_start_ [protected]

Store the initial velocity at the start pose.

Definition at line 673 of file optimal_planner.h.

const ViaPointContainer* teb_local_planner::TebOptimalPlanner::via_points_ [protected]

Store via points for planning.

Definition at line 664 of file optimal_planner.h.

TebVisualizationPtr teb_local_planner::TebOptimalPlanner::visualization_ [protected]

Instance of the visualization class.

Definition at line 669 of file optimal_planner.h.

The documentation for this class was generated from the following files:

teb_local_planner
Author(s): Christoph Rösmann
autogenerated on Sat Jun 8 2019 20:21:34