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:

Public Member Functions
void	initialize (const TebConfig &cfg, ObstContainer obstacles=NULL, TebVisualizationPtr visual=TebVisualizationPtr(), const ViaPointContainer via_points=NULL)
	Initializes the optimal planner. More...

	TebOptimalPlanner ()
	Default constructor. More...

	TebOptimalPlanner (const TebConfig &cfg, ObstContainer obstacles=NULL, 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
	PlannerInterface ()
	Default constructor. More...

virtual	~PlannerInterface ()
	Virtual destructor. More...

virtual void	updateRobotModel (RobotFootprintModelPtr robot_model)

virtual void	computeCurrentCost (std::vector< double > &cost, double obst_cost_scale=1.0, bool alternative_time_cost=false)

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

bool	hasDiverged () const override
	Returns true if the planner has diverged. 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, double feasibility_check_lookahead_distance=-1.0)
	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...

Hyper-Graph creation and optimization
const TebConfig *	cfg_
	Config class that stores and manages all related parameters. More...

ObstContainer *	obstacles_
	Store obstacles that are relevant for planning. More...

const ViaPointContainer *	via_points_
	Store via points for planning. More...

std::vector< ObstContainer >	obstacles_per_vertex_
	Store the obstacles associated with the n-1 initial vertices. More...

double	cost_
	Store cost value of the current hyper-graph. More...

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

TebVisualizationPtr	visualization_
	Instance of the visualization class. More...

TimedElasticBand	teb_
	Actual trajectory object. More...

boost::shared_ptr< g2o::SparseOptimizer >	optimizer_
	g2o optimizer for trajectory optimization More...

std::pair< bool, geometry_msgs::Twist >	vel_start_
	Store the initial velocity at the start pose. More...

std::pair< bool, geometry_msgs::Twist >	vel_goal_
	Store the final velocity at the goal pose. More...

bool	initialized_
	Keeps track about the correct initialization of this class. More...

bool	optimized_
	This variable is `true` as long as the last optimization has been completed successful. More...

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

void	AddEdgesVelocityObstacleRatio ()
	Add all edges (local cost function) for reducing the velocity of a vertex due to its associated obstacles. More...

boost::shared_ptr< g2o::SparseOptimizer >	initOptimizer ()
	Initialize and configure the g2o sparse optimizer. 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:

C. Rösmann et al.: Trajectory modification considering dynamic constraints of autonomous robots, ROBOTIK, 2012.
C. Rösmann et al.: Efficient trajectory optimization using a sparse model, ECMR, 2013.
R. Kümmerle et al.: G2o: A general framework for graph optimization, ICRA, 2011.

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 136 of file optimal_planner.h.

Constructor & Destructor Documentation

◆ TebOptimalPlanner() [1/2]

teb_local_planner::TebOptimalPlanner::TebOptimalPlanner ( )

Default constructor.

Definition at line 98 of file optimal_planner.cpp.

◆ TebOptimalPlanner() [2/2]

teb_local_planner::TebOptimalPlanner::TebOptimalPlanner	(	const TebConfig &	cfg,
		ObstContainer *	obstacles = `NULL`,
		TebVisualizationPtr	visual = `TebVisualizationPtr()`,
		const ViaPointContainer *	via_points = `NULL`
	)

Construct and initialize the TEB optimal planner.

Parameters

cfg	Const reference to the TebConfig class for internal parameters
obstacles	Container storing all relevant obstacles (see Obstacle)
visual	Shared pointer to the TebVisualization class (optional)
via_points	Container storing via-points (optional)

Definition at line 103 of file optimal_planner.cpp.

◆ ~TebOptimalPlanner()

teb_local_planner::TebOptimalPlanner::~TebOptimalPlanner ( )

virtual

Destruct the optimal planner.

Definition at line 108 of file optimal_planner.cpp.

Member Function Documentation

◆ AddEdgesAcceleration()

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 807 of file optimal_planner.cpp.

◆ AddEdgesDynamicObstacles()

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_multiplier Specify an additional weight multipler (in addition to the the config weight)

Definition at line 682 of file optimal_planner.cpp.

◆ AddEdgesKinematicsCarlike()

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 974 of file optimal_planner.cpp.

◆ AddEdgesKinematicsDiffDrive()

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 952 of file optimal_planner.cpp.

◆ AddEdgesObstacles()

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_multiplier Specify an additional weight multipler (in addition to the the config weight)

Definition at line 480 of file optimal_planner.cpp.

◆ AddEdgesObstaclesLegacy()

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)

See also: EdgeObstacle; buildGraph; optimizeGraph

Parameters

weight_multiplier Specify an additional weight multipler (in addition to the the config weight)

Definition at line 587 of file optimal_planner.cpp.

◆ AddEdgesPreferRotDir()

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 997 of file optimal_planner.cpp.

◆ AddEdgesShortestPath()

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 931 of file optimal_planner.cpp.

◆ AddEdgesTimeOptimal()

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 913 of file optimal_planner.cpp.

◆ AddEdgesVelocity()

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 756 of file optimal_planner.cpp.

◆ AddEdgesVelocityObstacleRatio()

void teb_local_planner::TebOptimalPlanner::AddEdgesVelocityObstacleRatio ( )

protected

Add all edges (local cost function) for reducing the velocity of a vertex due to its associated obstacles.

See also: buildGraph; optimizeGraph

Definition at line 1035 of file optimal_planner.cpp.

◆ AddEdgesViaPoints()

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 711 of file optimal_planner.cpp.

◆ AddTEBVertices()

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 458 of file optimal_planner.cpp.

◆ buildGraph()

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

Returns: true, if the graph was created successfully, false otherwise.

Definition at line 359 of file optimal_planner.cpp.

◆ clearGraph()

void teb_local_planner::TebOptimalPlanner::clearGraph ( )

protected

Clear an existing internal hyper-graph.

See also: buildGraph; optimizeGraph

Definition at line 440 of file optimal_planner.cpp.

◆ clearPlanner()

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 367 of file optimal_planner.h.

◆ computeCurrentCost() [1/2]

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

Returns: TebCostVec containing the cost values

Definition at line 1077 of file optimal_planner.cpp.

◆ computeCurrentCost() [2/2]

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]	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 460 of file optimal_planner.h.

◆ extractVelocity()

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

	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 1133 of file optimal_planner.cpp.

◆ getCurrentCost()

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 473 of file optimal_planner.h.

◆ getFullTrajectory()

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] trajectory the resulting trajectory

Definition at line 1233 of file optimal_planner.cpp.

◆ getObstVector()

const ObstContainer& teb_local_planner::TebOptimalPlanner::getObstVector ( ) const

inline

Access the internal obstacle container.

Returns: Const reference to the obstacle container

Definition at line 316 of file optimal_planner.h.

◆ getVelocityCommand()

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

Returns: true if command is valid, false otherwise

Implements teb_local_planner::PlannerInterface.

Definition at line 1171 of file optimal_planner.cpp.

◆ getVelocityProfile()

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_profile velocity profile will be written to this vector (after clearing any existing content) with the size=no_poses+1

Definition at line 1206 of file optimal_planner.cpp.

◆ getViaPoints()

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 335 of file optimal_planner.h.

◆ hasDiverged()

bool teb_local_planner::TebOptimalPlanner::hasDiverged ( ) const

overridevirtual

Returns true if the planner has diverged.

Implements teb_local_planner::PlannerInterface.

Definition at line 1059 of file optimal_planner.cpp.

◆ initialize()

void teb_local_planner::TebOptimalPlanner::initialize	(	const TebConfig &	cfg,
		ObstContainer *	obstacles = `NULL`,
		TebVisualizationPtr	visual = `TebVisualizationPtr()`,
		const ViaPointContainer *	via_points = `NULL`
	)

Initializes the optimal planner.

Parameters

cfg	Const reference to the TebConfig class for internal parameters
obstacles	Container storing all relevant obstacles (see Obstacle)
visual	Shared pointer to the TebVisualization class (optional)
via_points	Container storing via-points (optional)

Definition at line 118 of file optimal_planner.cpp.

◆ initOptimizer()

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 197 of file optimal_planner.cpp.

◆ isOptimized()

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 422 of file optimal_planner.h.

◆ isTrajectoryFeasible()

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`,
		double	feasibility_check_lookahead_distance = `-1.0`
	)

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

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 1286 of file optimal_planner.cpp.

◆ optimizeGraph()

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_iterations	Number of solver iterations
clear_after	Clear the graph after optimization.

Returns: true, if optimization terminates successfully, false otherwise.

Definition at line 404 of file optimal_planner.cpp.

◆ optimizer() [1/2]

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 409 of file optimal_planner.h.

◆ optimizer() [2/2]

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 415 of file optimal_planner.h.

◆ optimizeTEB()

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_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 via-point 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).

Returns: true if the optimization terminates successfully, false otherwise

Definition at line 218 of file optimal_planner.cpp.

◆ plan() [1/3]

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

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)

Returns: true if planning was successful, false otherwise

Implements teb_local_planner::PlannerInterface.

Definition at line 326 of file optimal_planner.cpp.

◆ plan() [2/3]

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_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)

Returns: true if planning was successful, false otherwise

Implements teb_local_planner::PlannerInterface.

Definition at line 283 of file optimal_planner.cpp.

◆ plan() [3/3]

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

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)

Returns: true if planning was successful, false otherwise

Implements teb_local_planner::PlannerInterface.

Definition at line 319 of file optimal_planner.cpp.

◆ registerG2OTypes()

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 167 of file optimal_planner.cpp.

◆ setObstVector()

void teb_local_planner::TebOptimalPlanner::setObstVector ( ObstContainer * obst_vector )

inline

Assign a new set of obstacles.

Parameters

obst_vector pointer to an obstacle container (can also be a nullptr)

Remarks: This method overrids the obstacle container optinally assigned in the constructor.

Definition at line 310 of file optimal_planner.h.

◆ setPreferredTurningDir()

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

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 381 of file optimal_planner.h.

◆ setVelocityGoal()

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_goal twist message containing the translational and angular final velocity

Definition at line 277 of file optimal_planner.cpp.

◆ setVelocityGoalFree()

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 296 of file optimal_planner.h.

◆ setVelocityStart()

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_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 269 of file optimal_planner.cpp.

◆ setViaPoints()

void teb_local_planner::TebOptimalPlanner::setViaPoints ( const ViaPointContainer * via_points )

inline

Assign a new set of via-points.

Parameters

via_points pointer to a via_point container (can also be a nullptr)

Any previously set container will be overwritten.

Definition at line 329 of file optimal_planner.h.

◆ setVisualization()

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

visualization shared pointer to a TebVisualization instance

See also: visualize

Definition at line 143 of file optimal_planner.cpp.

◆ teb() [1/2]

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 396 of file optimal_planner.h.

◆ teb() [2/2]

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 402 of file optimal_planner.h.

◆ visualize()

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 148 of file optimal_planner.cpp.

Member Data Documentation

◆ cfg_

const TebConfig* teb_local_planner::TebOptimalPlanner::cfg_

protected

Config class that stores and manages all related parameters.

Definition at line 711 of file optimal_planner.h.

◆ cost_

double teb_local_planner::TebOptimalPlanner::cost_

protected

Store cost value of the current hyper-graph.

Definition at line 716 of file optimal_planner.h.

◆ initialized_

bool teb_local_planner::TebOptimalPlanner::initialized_

protected

Keeps track about the correct initialization of this class.

Definition at line 726 of file optimal_planner.h.

◆ obstacles_

ObstContainer* teb_local_planner::TebOptimalPlanner::obstacles_

protected

Store obstacles that are relevant for planning.

Definition at line 712 of file optimal_planner.h.

◆ obstacles_per_vertex_

std::vector<ObstContainer> teb_local_planner::TebOptimalPlanner::obstacles_per_vertex_

protected

Store the obstacles associated with the n-1 initial vertices.

Definition at line 714 of file optimal_planner.h.

◆ optimized_

bool teb_local_planner::TebOptimalPlanner::optimized_

protected

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

Definition at line 727 of file optimal_planner.h.

◆ optimizer_

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

protected

g2o optimizer for trajectory optimization

Definition at line 722 of file optimal_planner.h.

◆ prefer_rotdir_

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 717 of file optimal_planner.h.

◆ teb_

TimedElasticBand teb_local_planner::TebOptimalPlanner::teb_

protected

Actual trajectory object.

Definition at line 721 of file optimal_planner.h.

◆ vel_goal_

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

protected

Store the final velocity at the goal pose.

Definition at line 724 of file optimal_planner.h.

◆ vel_start_

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

protected

Store the initial velocity at the start pose.

Definition at line 723 of file optimal_planner.h.

◆ via_points_

const ViaPointContainer* teb_local_planner::TebOptimalPlanner::via_points_

protected

Store via points for planning.

Definition at line 713 of file optimal_planner.h.

◆ visualization_

TebVisualizationPtr teb_local_planner::TebOptimalPlanner::visualization_

protected

Instance of the visualization class.

Definition at line 720 of file optimal_planner.h.

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

Public Member Functions

Utility methods and more

Hyper-Graph creation and optimization

Detailed Description

Constructor & Destructor Documentation

◆ TebOptimalPlanner() [1/2]

◆ TebOptimalPlanner() [2/2]

◆ ~TebOptimalPlanner()

Member Function Documentation

◆ AddEdgesAcceleration()

◆ AddEdgesDynamicObstacles()

◆ AddEdgesKinematicsCarlike()

◆ AddEdgesKinematicsDiffDrive()

◆ AddEdgesObstacles()

◆ AddEdgesObstaclesLegacy()

◆ AddEdgesPreferRotDir()

◆ AddEdgesShortestPath()

◆ AddEdgesTimeOptimal()

◆ AddEdgesVelocity()

◆ AddEdgesVelocityObstacleRatio()

◆ AddEdgesViaPoints()

◆ AddTEBVertices()

◆ buildGraph()

◆ clearGraph()

◆ clearPlanner()

◆ computeCurrentCost() [1/2]

◆ computeCurrentCost() [2/2]

◆ extractVelocity()

◆ getCurrentCost()

◆ getFullTrajectory()

◆ getObstVector()

◆ getVelocityCommand()

◆ getVelocityProfile()

◆ getViaPoints()

◆ hasDiverged()

◆ initialize()

◆ initOptimizer()

◆ isOptimized()

◆ isTrajectoryFeasible()

◆ optimizeGraph()

◆ optimizer() [1/2]

◆ optimizer() [2/2]

◆ optimizeTEB()

◆ plan() [1/3]

◆ plan() [2/3]

◆ plan() [3/3]

◆ registerG2OTypes()

◆ setObstVector()

◆ setPreferredTurningDir()

◆ setVelocityGoal()

◆ setVelocityGoalFree()

◆ setVelocityStart()

◆ setViaPoints()

◆ setVisualization()

◆ teb() [1/2]

◆ teb() [2/2]

◆ visualize()

Member Data Documentation

◆ cfg_

◆ cost_

◆ initialized_

◆ obstacles_

◆ obstacles_per_vertex_

◆ optimized_

◆ optimizer_

◆ prefer_rotdir_

◆ teb_

◆ vel_goal_

◆ vel_start_

◆ via_points_

◆ visualization_