Public Member Functions | Protected Member Functions | Protected Attributes | List of all members
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]

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. More...
 
 TebOptimalPlanner ()
 Default constructor. More...
 
 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. More...
 
virtual ~TebOptimalPlanner ()
 Destruct the optimal planner. More...
 
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. More...
 
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) More...
 
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. More...
 
virtual bool getVelocityCommand (double &vx, double &vy, double &omega, int look_ahead_poses) const
 Get the velocity command from a previously optimized plan to control the robot at the current sampling interval. More...
 
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). More...
 
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]. More...
 
void setVelocityGoal (const geometry_msgs::Twist &vel_goal)
 Set the desired final velocity at the trajectory's goal pose. More...
 
void setVelocityGoalFree ()
 Set the desired final velocity at the trajectory's goal pose to be the maximum velocity limit. More...
 
Take obstacles into account
void setObstVector (ObstContainer *obst_vector)
 Assign a new set of obstacles. More...
 
const ObstContainer & getObstVector () const
 Access the internal obstacle container. More...
 
Take via-points into account
void setViaPoints (const ViaPointContainer *via_points)
 Assign a new set of via-points. More...
 
const ViaPointContainer & getViaPoints () const
 Access the internal via-point container. More...
 
Visualization
void setVisualization (TebVisualizationPtr visualization)
 Register a TebVisualization class to enable visiualization routines (e.g. publish the local plan and pose sequence) More...
 
virtual void visualize ()
 Publish the local plan and pose sequence via ros topics (e.g. subscribe with rviz). More...
 
- Public Member Functions inherited from teb_local_planner::PlannerInterface
virtual void computeCurrentCost (std::vector< double > &cost, double obst_cost_scale=1.0, bool alternative_time_cost=false)
 
 PlannerInterface ()
 Default constructor. More...
 
virtual ~PlannerInterface ()
 Virtual destructor. More...
 

Protected Member Functions

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

Protected Attributes

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

Utility methods and more

virtual void clearPlanner ()
 Reset the planner by clearing the internal graph and trajectory. More...
 
virtual void setPreferredTurningDir (RotType dir)
 Prefer a desired initial turning direction (by penalizing the opposing one) More...
 
TimedElasticBand & teb ()
 Access the internal TimedElasticBand trajectory. More...
 
const TimedElasticBand & teb () const
 Access the internal TimedElasticBand trajectory (read-only). More...
 
boost::shared_ptr< g2o::SparseOptimizer > optimizer ()
 Access the internal g2o optimizer. More...
 
boost::shared_ptr< const g2o::SparseOptimizer > optimizer () const
 Access the internal g2o optimizer (read-only). More...
 
bool isOptimized () const
 Check if last optimization was successful. More...
 
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). More...
 
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. More...
 
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) More...
 
void getVelocityProfile (std::vector< geometry_msgs::Twist > &velocity_profile) const
 Compute the velocity profile of the trajectory. More...
 
void getFullTrajectory (std::vector< TrajectoryPointMsg > &trajectory) const
 Return the complete trajectory including poses, velocity profiles and temporal information. More...
 
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. More...
 
static void registerG2OTypes ()
 Register the vertices and edges defined for the TEB to the g2o::Factory. More...
 

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.

: We introduced the non-fast mode with the support of dynamic obstacles (which leads to better results in terms of x-y-t homotopy planning). However, we have not tested this mode intensively yet, so we keep the legacy fast mode as default until we finish our tests.

Definition at line 111 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 54 of file optimal_planner.cpp.

teb_local_planner::TebOptimalPlanner::~TebOptimalPlanner ( )
virtual

Destruct the optimal planner.

Definition at line 59 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 770 of file optimal_planner.cpp.

void teb_local_planner::TebOptimalPlanner::AddEdgesDynamicObstacles ( double  weight_multiplier = 1.0)
protected

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

Warning
experimental
Todo:
Should we also add neighbors to decrease jiggling/oscillations
See also
EdgeDynamicObstacle
buildGraph
optimizeGraph
Parameters
weight_multiplierSpecify an additional weight multipler (in addition to the the config weight)

Definition at line 645 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 937 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 915 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
See also
EdgeObstacle
buildGraph
optimizeGraph
Parameters
weight_multiplierSpecify an additional weight multipler (in addition to the the config weight)

Definition at line 418 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 550 of file optimal_planner.cpp.

void teb_local_planner::TebOptimalPlanner::AddEdgesPreferRotDir ( )
protected

Add all edges (local cost functions) for prefering a specifiy turning direction (by penalizing the other one)

See also
buildGraph
optimizeGraph

Definition at line 960 of file optimal_planner.cpp.

void teb_local_planner::TebOptimalPlanner::AddEdgesShortestPath ( )
protected

Add all edges (local cost functions) for minimizing the path length.

See also
EdgeShortestPath
buildGraph
optimizeGraph

Definition at line 894 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 876 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 719 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 674 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 400 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 310 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 387 of file optimal_planner.cpp.

virtual void teb_local_planner::TebOptimalPlanner::clearPlanner ( )
inlinevirtual

Reset the planner by clearing the internal graph and trajectory.

Implements teb_local_planner::PlannerInterface.

Definition at line 346 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 998 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 
)
inlinevirtual

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 434 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 1054 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 447 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 1154 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 295 of file optimal_planner.h.

bool teb_local_planner::TebOptimalPlanner::getVelocityCommand ( double &  vx,
double &  vy,
double &  omega,
int  look_ahead_poses 
) 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]
[in]look_ahead_posesindex of the final pose used to compute the velocity command.
Returns
true if command is valid, false otherwise

Implements teb_local_planner::PlannerInterface.

Definition at line 1092 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 1127 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 314 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 69 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 149 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 401 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 1207 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 351 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 388 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 394 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 170 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 235 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 270 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 277 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 119 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 289 of file optimal_planner.h.

virtual void teb_local_planner::TebOptimalPlanner::setPreferredTurningDir ( RotType  dir)
inlinevirtual

Prefer a desired initial turning direction (by penalizing the opposing one)

A desired (initial) turning direction might be specified in case the planned trajectory oscillates between two solutions (in the same equivalence class!) with similar cost. Check the parameters in order to adjust the weight of the penalty. Initial means that the penalty is applied only to the first few poses of the trajectory.

Parameters
dirThis parameter might be RotType::left (prefer left), RotType::right (prefer right) or RotType::none (prefer none)

Reimplemented from teb_local_planner::PlannerInterface.

Definition at line 360 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 229 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 275 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 221 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 308 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 95 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 375 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 381 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 100 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 679 of file optimal_planner.h.

double teb_local_planner::TebOptimalPlanner::cost_
protected

Store cost value of the current hyper-graph.

Definition at line 683 of file optimal_planner.h.

bool teb_local_planner::TebOptimalPlanner::initialized_
protected

Keeps track about the correct initialization of this class.

Definition at line 694 of file optimal_planner.h.

ObstContainer* teb_local_planner::TebOptimalPlanner::obstacles_
protected

Store obstacles that are relevant for planning.

Definition at line 680 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 695 of file optimal_planner.h.

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

g2o optimizer for trajectory optimization

Definition at line 690 of file optimal_planner.h.

RotType teb_local_planner::TebOptimalPlanner::prefer_rotdir_
protected

Store whether to prefer a specific initial rotation in optimization (might be activated in case the robot oscillates)

Definition at line 684 of file optimal_planner.h.

RobotFootprintModelPtr teb_local_planner::TebOptimalPlanner::robot_model_
protected

Robot model.

Definition at line 689 of file optimal_planner.h.

TimedElasticBand teb_local_planner::TebOptimalPlanner::teb_
protected

Actual trajectory object.

Definition at line 688 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 692 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 691 of file optimal_planner.h.

const ViaPointContainer* teb_local_planner::TebOptimalPlanner::via_points_
protected

Store via points for planning.

Definition at line 681 of file optimal_planner.h.

TebVisualizationPtr teb_local_planner::TebOptimalPlanner::visualization_
protected

Instance of the visualization class.

Definition at line 687 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 Wed Jun 3 2020 04:03:08