This class optimizes an internal Timed Elastic Band trajectory using the g2oframework. More...
#include <optimal_planner.h>
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 viapoints into account  
void  setViaPoints (const ViaPointContainer *via_points) 
Assign a new set of viapoints. More...  
const ViaPointContainer &  getViaPoints () const 
Access the internal viapoint 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...  
HyperGraph creation and optimization  
bool  buildGraph (double weight_multiplier=1.0) 
Build the hypergraph representing the TEB optimization problem. More...  
bool  optimizeGraph (int no_iterations, bool clear_after=true) 
Optimize the previously constructed hypergraph to deform / optimize the TEB. More...  
void  clearGraph () 
Clear an existing internal hypergraph. More...  
void  AddTEBVertices () 
Add all relevant vertices to the hypergraph 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 viapoints. 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 hypergraph. 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 (readonly). 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 (readonly). 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 (hypergraph 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...  
This class optimizes an internal Timed Elastic Band trajectory using the g2oframework.
For an introduction and further details about the TEB optimization problem refer to:
: Call buildGraph() only if the teb structure has been modified to speed up hotstarting from previous solutions.
: We introduced the nonfast mode with the support of dynamic obstacles (which leads to better results in terms of xyt 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.
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.
cfg  Const reference to the TebConfig class for internal parameters 
obstacles  Container storing all relevant obstacles (see Obstacle) 
robot_model  Shared pointer to the robot shape model used for optimization (optional) 
visual  Shared pointer to the TebVisualization class (optional) 
via_points  Container storing viapoints (optional) 
Definition at line 54 of file optimal_planner.cpp.

virtual 
Destruct the optimal planner.
Definition at line 59 of file optimal_planner.cpp.

protected 
Add all edges (local cost functions) for limiting the translational and angular acceleration.
Definition at line 770 of file optimal_planner.cpp.

protected 
Add all edges (local cost functions) related to keeping a distance from dynamic (moving) obstacles.
weight_multiplier  Specify an additional weight multipler (in addition to the the config weight) 
Definition at line 645 of file optimal_planner.cpp.

protected 
Add all edges (local cost functions) for satisfying kinematic constraints of a carlike robot.
Definition at line 937 of file optimal_planner.cpp.

protected 
Add all edges (local cost functions) for satisfying kinematic constraints of a differential drive robot.
Definition at line 915 of file optimal_planner.cpp.

protected 
Add all edges (local cost functions) related to keeping a distance from static obstacles.
weight_multiplier  Specify an additional weight multipler (in addition to the the config weight) 
Definition at line 418 of file optimal_planner.cpp.

protected 
Add all edges (local cost functions) related to keeping a distance from static obstacles (legacy association strategy)
weight_multiplier  Specify an additional weight multipler (in addition to the the config weight) 
Definition at line 550 of file optimal_planner.cpp.

protected 
Add all edges (local cost functions) for prefering a specifiy turning direction (by penalizing the other one)
Definition at line 960 of file optimal_planner.cpp.

protected 
Add all edges (local cost functions) for minimizing the path length.
Definition at line 894 of file optimal_planner.cpp.

protected 
Add all edges (local cost functions) for minimizing the transition time (resp. minimize time differences)
Definition at line 876 of file optimal_planner.cpp.

protected 
Add all edges (local cost functions) for limiting the translational and angular velocity.
Definition at line 719 of file optimal_planner.cpp.

protected 
Add all edges (local cost functions) related to minimizing the distance to viapoints.
Definition at line 674 of file optimal_planner.cpp.

protected 
Add all relevant vertices to the hypergraph as optimizable variables.
Vertices (if unfixed) represent the variables that will be optimized.
In case of the TimedElasticBand poses and time differences form the vertices of the hypergraph.
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.
Definition at line 400 of file optimal_planner.cpp.

protected 
Build the hypergraph representing the TEB optimization problem.
This method creates the optimization problem according to the hypergraph formulation.
For more details refer to the literature cited in the TebOptimalPlanner class description.
weight_multiplier  Specify a weight multipler for selected weights in optimizeGraph This might be used for weight adapation strategies. Currently, only obstacle collision weights are considered. 
true
, if the graph was created successfully, false
otherwise. Definition at line 310 of file optimal_planner.cpp.

protected 
Clear an existing internal hypergraph.
Definition at line 387 of file optimal_planner.cpp.

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 (hypergraph 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: . Sometimes, the user may want to get a value that is proportional or identical to the actual trajectory transition time .
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.
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?
obst_cost_scale  Specify extra scaling for obstacle costs. 
viapoint_cost_scale  Specify extra scaling for via points. 
alternative_time_cost  Replace the cost for the time optimal objective by the actual (weighted) transition time. 
Definition at line 998 of file optimal_planner.cpp.

inlinevirtual 
Compute and return the cost of the current optimization graph (supports multiple trajectories)
[out]  cost  current cost value for each trajectory [for a planner with just a single trajectory: size=1, vector will not be cleared] 
obst_cost_scale  Specify extra scaling for obstacle costs  
viapoint_cost_scale  Specify extra scaling for via points.  
alternative_time_cost  Replace the cost for the time optimal objective by the actual (weighted) transition time 
Definition at line 434 of file optimal_planner.h.

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.
pose1  pose at time k  
pose2  consecutive pose at time k+1  
dt  actual time difference between k and k+1 (must be >0 !!!)  
[out]  vx  translational velocity 
[out]  vy  strafing velocity which can be nonzero for holonomic robots 
[out]  omega  rotational velocity 
Definition at line 1054 of file optimal_planner.cpp.

inline 
Access the cost vector.
The accumulated cost value previously calculated using computeCurrentCost or by calling optimizeTEB with enabled cost flag.
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 openloop 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.
[out]  trajectory  the resulting trajectory 
Definition at line 1154 of file optimal_planner.cpp.

inline 
Access the internal obstacle container.
Definition at line 295 of file optimal_planner.h.

virtual 
Get the velocity command from a previously optimized plan to control the robot at the current sampling interval.
[out]  vx  translational velocity [m/s] 
[out]  vy  strafing velocity which can be nonzero for holonomic robots[m/s] 
[out]  omega  rotational velocity [rad/s] 
[in]  look_ahead_poses  index of the final pose used to compute the velocity command. 
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...end1 are related to the transition from pose_{k1} 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,...end1] = +(pose_{k+1}pose{k})/dt, v(end) = v_goal It can be used for evaluation and debugging purposes or for openloop control. For computing the velocity required for controlling the robot to the next step refer to getVelocityCommand().
[out]  velocity_profile  velocity 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.

inline 
Access the internal viapoint 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.
cfg  Const reference to the TebConfig class for internal parameters 
obstacles  Container storing all relevant obstacles (see Obstacle) 
robot_model  Shared pointer to the robot shape model used for optimization (optional) 
visual  Shared pointer to the TebVisualization class (optional) 
via_points  Container storing viapoints (optional) 
Definition at line 69 of file optimal_planner.cpp.

protected 
Initialize and configure the g2o sparse optimizer.
Definition at line 149 of file optimal_planner.cpp.

inline 
Check if last optimization was successful.
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.

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.
costmap_model  Pointer to the costmap model 
footprint_spec  The specification of the footprint of the robot in world coordinates 
inscribed_radius  The radius of the inscribed circle of the robot 
circumscribed_radius  The radius of the circumscribed circle of the robot 
look_ahead_idx  Number of poses along the trajectory that should be verified, if 1, the complete trajectory will be checked. 
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.

protected 
Optimize the previously constructed hypergraph 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 nonconstrained sparse LevenbergMarquardt algorithm. Constraints are considered by utilizing penalty approximations. Refer to the literature cited in the TebOptimalPlanner class description.
no_iterations  Number of solver iterations 
clear_after  Clear the graph after optimization. 
true
, if optimization terminates successfully, false
otherwise. Definition at line 351 of file optimal_planner.cpp.

inline 
Access the internal g2o optimizer.
Definition at line 388 of file optimal_planner.h.

inline 
Access the internal g2o optimizer (readonly).
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:
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, 26 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().
iterations_innerloop  Number of iterations for the actual solver loop 
iterations_outerloop  Specifies how often the trajectory should be resized followed by the inner solver loop. 
compute_cost_afterwards  if true Calculate the cost vector according to computeCurrentCost(), the vector can be accessed afterwards using getCurrentCost(). 
obst_cost_scale  Specify extra scaling for obstacle costs (only used if compute_cost_afterwards is true) 
viapoint_cost_scale  Specify extra scaling for viapoint costs (only used if compute_cost_afterwards is true) 
alternative_time_cost  Replace the cost for the time optimal objective by the actual (weighted) transition time (only used if compute_cost_afterwards is true). 
true
if the optimization terminates successfully, false
otherwise Definition at line 170 of file optimal_planner.cpp.

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 hotstarting from previous solutions, if avaiable:
initial_plan  vector of geometry_msgs::PoseStamped 
start_vel  Current start velocity (e.g. the velocity of the robot, only linear.x, linear.y (holonomic) and angular.z are used) 
free_goal_vel  if true , a nonzero final velocity at the goal pose is allowed, otherwise the final velocity will be zero (default: false) 
true
if planning was successful, false
otherwise Implements teb_local_planner::PlannerInterface.
Definition at line 235 of file optimal_planner.cpp.

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 hotstarting from previous solutions, if avaiable:
start  tf::Pose containing the start pose of the trajectory 
goal  tf::Pose containing the goal pose of the trajectory 
start_vel  Current start velocity (e.g. the velocity of the robot, only linear.x, linear.y (holonomic) and angular.z are used) 
free_goal_vel  if true , a nonzero final velocity at the goal pose is allowed, otherwise the final velocity will be zero (default: false) 
true
if planning was successful, false
otherwise Implements teb_local_planner::PlannerInterface.
Definition at line 270 of file optimal_planner.cpp.

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 hotstarting from previous solutions, if avaiable:
start  PoseSE2 containing the start pose of the trajectory 
goal  PoseSE2 containing the goal pose of the trajectory 
start_vel  Initial velocity at the start pose (twist message containing the translational and angular velocity). 
free_goal_vel  if true , a nonzero final velocity at the goal pose is allowed, otherwise the final velocity will be zero (default: false) 
true
if planning was successful, false
otherwise Implements teb_local_planner::PlannerInterface.
Definition at line 277 of file optimal_planner.cpp.

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.

inline 
Assign a new set of obstacles.
obst_vector  pointer to an obstacle container (can also be a nullptr) 
Definition at line 289 of file optimal_planner.h.

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.
dir  This 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.
free_goal_vel
is set to false
in plan() vel_goal  twist message containing the translational and angular final velocity 
Definition at line 229 of file optimal_planner.cpp.

inline 
Set the desired final velocity at the trajectory's goal pose to be the maximum velocity limit.
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].
vel_start  Current 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.

inline 
Assign a new set of viapoints.
via_points  pointer 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)
visualization  shared pointer to a TebVisualization instance 
Definition at line 95 of file optimal_planner.cpp.

inline 
Access the internal TimedElasticBand trajectory.
Definition at line 375 of file optimal_planner.h.

inline 
Access the internal TimedElasticBand trajectory (readonly).
Definition at line 381 of file optimal_planner.h.

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.
Reimplemented from teb_local_planner::PlannerInterface.
Definition at line 100 of file optimal_planner.cpp.

protected 
Config class that stores and manages all related parameters.
Definition at line 679 of file optimal_planner.h.

protected 
Store cost value of the current hypergraph.
Definition at line 683 of file optimal_planner.h.

protected 
Keeps track about the correct initialization of this class.
Definition at line 694 of file optimal_planner.h.

protected 
Store obstacles that are relevant for planning.
Definition at line 680 of file optimal_planner.h.

protected 
This variable is true
as long as the last optimization has been completed successful.
Definition at line 695 of file optimal_planner.h.

protected 
g2o optimizer for trajectory optimization
Definition at line 690 of file optimal_planner.h.

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.

protected 
Robot model.
Definition at line 689 of file optimal_planner.h.

protected 
Actual trajectory object.
Definition at line 688 of file optimal_planner.h.

protected 
Store the final velocity at the goal pose.
Definition at line 692 of file optimal_planner.h.

protected 
Store the initial velocity at the start pose.
Definition at line 691 of file optimal_planner.h.

protected 
Store via points for planning.
Definition at line 681 of file optimal_planner.h.

protected 
Instance of the visualization class.
Definition at line 687 of file optimal_planner.h.