Class that defines a trajectory modeled as an elastic band with augmented tempoarl information. More...

#include <timed_elastic_band.h>

Public Member Functions
	TimedElasticBand ()
	Construct the class. More...

virtual	~TimedElasticBand ()
	Destruct the class. More...

Access pose and timediff sequences
PoseSequence &	poses ()
	Access the complete pose sequence. More...

const PoseSequence &	poses () const
	Access the complete pose sequence (read-only) More...

TimeDiffSequence &	timediffs ()
	Access the complete timediff sequence. More...

const TimeDiffSequence &	timediffs () const
	Access the complete timediff sequence. More...

double &	TimeDiff (int index)
	Access the time difference at pos `index` of the time sequence. More...

const double &	TimeDiff (int index) const
	Access the time difference at pos `index` of the time sequence (read-only) More...

PoseSE2 &	Pose (int index)
	Access the pose at pos `index` of the pose sequence. More...

const PoseSE2 &	Pose (int index) const
	Access the pose at pos `index` of the pose sequence (read-only) More...

PoseSE2 &	BackPose ()
	Access the last PoseSE2 in the pose sequence. More...

const PoseSE2 &	BackPose () const
	Access the last PoseSE2 in the pose sequence (read-only) More...

double &	BackTimeDiff ()
	Access the last TimeDiff in the time diff sequence. More...

const double &	BackTimeDiff () const
	Access the last TimeDiff in the time diff sequence (read-only) More...

VertexPose *	PoseVertex (int index)
	Access the vertex of a pose at pos `index` for optimization purposes. More...

VertexTimeDiff *	TimeDiffVertex (int index)
	Access the vertex of a time difference at pos `index` for optimization purposes. More...

Append new elements to the pose and timediff sequences
void	addPose (const PoseSE2 &pose, bool fixed=false)
	Append a new pose vertex to the back of the pose sequence. More...

void	addPose (const Eigen::Ref< const Eigen::Vector2d > &position, double theta, bool fixed=false)
	Append a new pose vertex to the back of the pose sequence. More...

void	addPose (double x, double y, double theta, bool fixed=false)
	Append a new pose vertex to the back of the pose sequence. More...

void	addTimeDiff (double dt, bool fixed=false)
	Append a new time difference vertex to the back of the time diff sequence. More...

void	addPoseAndTimeDiff (const PoseSE2 &pose, double dt)
	Append a (pose, timediff) vertex pair to the end of the current trajectory (pose and timediff sequences) More...

void	addPoseAndTimeDiff (const Eigen::Ref< const Eigen::Vector2d > &position, double theta, double dt)
	Append a (pose, timediff) vertex pair to the end of the current trajectory (pose and timediff sequences) More...

void	addPoseAndTimeDiff (double x, double y, double theta, double dt)
	Append a (pose, timediff) vertex pair to the end of the current trajectory (pose and timediff sequences) More...

Insert new elements and remove elements of the pose and timediff sequences
void	insertPose (int index, const PoseSE2 &pose)
	Insert a new pose vertex at pos. `index` to the pose sequence. More...

void	insertPose (int index, const Eigen::Ref< const Eigen::Vector2d > &position, double theta)
	Insert a new pose vertex at pos. `index` to the pose sequence. More...

void	insertPose (int index, double x, double y, double theta)
	Insert a new pose vertex at pos. `index` to the pose sequence. More...

void	insertTimeDiff (int index, double dt)
	Insert a new timediff vertex at pos. `index` to the timediff sequence. More...

void	deletePose (int index)
	Delete pose at pos. `index` in the pose sequence. More...

void	deletePoses (int index, int number)
	Delete multiple (`number`) poses starting at pos. `index` in the pose sequence. More...

void	deleteTimeDiff (int index)
	Delete pose at pos. `index` in the timediff sequence. More...

void	deleteTimeDiffs (int index, int number)
	Delete multiple (`number`) time differences starting at pos. `index` in the timediff sequence. More...

Init the trajectory
bool	initTrajectoryToGoal (const PoseSE2 &start, const PoseSE2 &goal, double diststep=0, double max_vel_x=0.5, int min_samples=3, bool guess_backwards_motion=false)
	Initialize a trajectory between a given start and goal pose. More...

template<typename BidirIter , typename Fun >
bool	initTrajectoryToGoal (BidirIter path_start, BidirIter path_end, Fun fun_position, double max_vel_x, double max_vel_theta, boost::optional< double > max_acc_x, boost::optional< double > max_acc_theta, boost::optional< double > start_orientation, boost::optional< double > goal_orientation, int min_samples=3, bool guess_backwards_motion=false)
	Initialize a trajectory from a generic 2D reference path. More...

bool	initTrajectoryToGoal (const std::vector< geometry_msgs::PoseStamped > &plan, double max_vel_x, bool estimate_orient=false, int min_samples=3, bool guess_backwards_motion=false)
	Initialize a trajectory from a reference pose sequence (positions and orientations). More...

ROS_DEPRECATED bool	initTEBtoGoal (const PoseSE2 &start, const PoseSE2 &goal, double diststep=0, double timestep=1, int min_samples=3, bool guess_backwards_motion=false)

template<typename BidirIter , typename Fun >
ROS_DEPRECATED bool	initTEBtoGoal (BidirIter path_start, BidirIter path_end, Fun fun_position, double max_vel_x, double max_vel_theta, boost::optional< double > max_acc_x, boost::optional< double > max_acc_theta, boost::optional< double > start_orientation, boost::optional< double > goal_orientation, int min_samples=3, bool guess_backwards_motion=false)

ROS_DEPRECATED bool	initTEBtoGoal (const std::vector< geometry_msgs::PoseStamped > &plan, double dt, bool estimate_orient=false, int min_samples=3, bool guess_backwards_motion=false)

Update and modify the trajectory
void	updateAndPruneTEB (boost::optional< const PoseSE2 & > new_start, boost::optional< const PoseSE2 & > new_goal, int min_samples=3)
	Hot-Start from an existing trajectory with updated start and goal poses. More...

void	autoResize (double dt_ref, double dt_hysteresis, int min_samples=3, int max_samples=1000, bool fast_mode=false)
	Resize the trajectory by removing or inserting a (pose,dt) pair depending on a reference temporal resolution. More...

void	setPoseVertexFixed (int index, bool status)
	Set a pose vertex at pos `index` of the pose sequence to be fixed or unfixed during optimization. More...

void	setTimeDiffVertexFixed (int index, bool status)
	Set a timediff vertex at pos `index` of the timediff sequence to be fixed or unfixed during optimization. More...

void	clearTimedElasticBand ()
	clear all poses and timediffs from the trajectory. The pose and timediff sequences will be empty and isInit() will return `false` More...

Utility and status methods
int	findClosestTrajectoryPose (const Eigen::Ref< const Eigen::Vector2d > &ref_point, double *distance=NULL, int begin_idx=0) const
	Find the closest point on the trajectory w.r.t. to a provided reference point. More...

int	findClosestTrajectoryPose (const Eigen::Ref< const Eigen::Vector2d > &ref_line_start, const Eigen::Ref< const Eigen::Vector2d > &ref_line_end, double *distance=NULL) const
	Find the closest point on the trajectory w.r.t. to a provided reference line. More...

int	findClosestTrajectoryPose (const Point2dContainer &vertices, double *distance=NULL) const
	Find the closest point on the trajectory w.r.t. to a provided reference polygon. More...

int	findClosestTrajectoryPose (const Obstacle &obstacle, double *distance=NULL) const
	Find the closest point on the trajectory w.r.t to a provided obstacle type. More...

int	sizePoses () const
	Get the length of the internal pose sequence. More...

int	sizeTimeDiffs () const
	Get the length of the internal timediff sequence. More...

bool	isInit () const
	Check whether the trajectory is initialized (nonzero pose and timediff sequences) More...

double	getSumOfAllTimeDiffs () const
	Calculate the total transition time (sum over all time intervals of the timediff sequence) More...

double	getSumOfTimeDiffsUpToIdx (int index) const
	Calculate the estimated transition time up to the pose denoted by index. More...

double	getAccumulatedDistance () const
	Calculate the length (accumulated euclidean distance) of the trajectory. More...

bool	detectDetoursBackwards (double threshold=0) const
	Detect whether the trajectory contains detours. More...

bool	isTrajectoryInsideRegion (double radius, double max_dist_behind_robot=-1, int skip_poses=0)
	Check if all trajectory points are contained in a specific region. More...

Protected Attributes
PoseSequence	pose_vec_
	Internal container storing the sequence of optimzable pose vertices. More...

TimeDiffSequence	timediff_vec_
	Internal container storing the sequence of optimzable timediff vertices. More...

Detailed Description

Class that defines a trajectory modeled as an elastic band with augmented tempoarl information.

All trajectory related methods (initialization, modifying, ...) are implemented inside this class.
Let $Q = \lbrace \mathbf{s}_i \rbrace_{i=0...n},\ n \in \mathbb{N}$ be a sequence of poses,
in which $\mathbf{s}_i = [x_i, y_i, \beta_i]^T \in \mathbb{R}^2 \times S^1$ denotes a single pose of the robot.
The Timed Elastic Band (TEB) augments this sequence of poses by incorporating time intervals between two consecutive poses, resuting in a sequence of n-1 time intervals $\Delta T_i$ :
$\tau = \lbrace \Delta T_i \rbrace_{i=0...n-1}$ .
Each time interval (time diff) denotes the time that the robot requires to transit from the current configuration to the next configuration. The tuple of both sequences defines the underlying trajectory.

Poses and time differences are wrapped into a g2o::Vertex class in order to enable the efficient optimization in TebOptimalPlanner.
TebOptimalPlanner utilizes this Timed_Elastic_band class for representing an optimizable trajectory.

Todo:: Move decision if the start or goal state should be marked as fixed or unfixed for the optimization to the TebOptimalPlanner class.

Definition at line 86 of file timed_elastic_band.h.

Constructor & Destructor Documentation

teb_local_planner::TimedElasticBand::TimedElasticBand ( )

Construct the class.

Definition at line 46 of file timed_elastic_band.cpp.

teb_local_planner::TimedElasticBand::~TimedElasticBand ( )

virtual

Destruct the class.

Definition at line 50 of file timed_elastic_band.cpp.

Member Function Documentation

void teb_local_planner::TimedElasticBand::addPose	(	const PoseSE2 &	pose,
		bool	fixed = `false`
	)

Append a new pose vertex to the back of the pose sequence.

Parameters

pose	PoseSE2 to push back on the internal PoseSequence
fixed	Mark the pose to be fixed or unfixed during trajectory optimization (important for the TebOptimalPlanner)

Definition at line 57 of file timed_elastic_band.cpp.

void teb_local_planner::TimedElasticBand::addPose	(	const Eigen::Ref< const Eigen::Vector2d > &	position,
		double	theta,
		bool	fixed = `false`
	)

Append a new pose vertex to the back of the pose sequence.

Parameters

position	2D vector representing the position part
theta	yaw angle representing the orientation part
fixed	Mark the pose to be fixed or unfixed during trajectory optimization (important for the TebOptimalPlanner)

Definition at line 64 of file timed_elastic_band.cpp.

void teb_local_planner::TimedElasticBand::addPose	(	double	x,
		double	y,
		double	theta,
		bool	fixed = `false`
	)

Append a new pose vertex to the back of the pose sequence.

Parameters

x	x-coordinate of the position part
y	y-coordinate of the position part
theta	yaw angle representing the orientation part
fixed	Mark the pose to be fixed or unfixed during trajectory optimization (important for the TebOptimalPlanner)

Definition at line 71 of file timed_elastic_band.cpp.

void teb_local_planner::TimedElasticBand::addPoseAndTimeDiff	(	const PoseSE2 &	pose,
		double	dt
	)

Append a (pose, timediff) vertex pair to the end of the current trajectory (pose and timediff sequences)

Parameters

pose	PoseSE2 to push back on the internal PoseSequence
dt	time difference value to push back on the internal TimeDiffSequence

Warning: Since the timediff is defined to connect two consecutive poses, this call is only allowed if there exist already n poses and n-1 timediffs in the sequences (n=1,2,...): therefore add a single pose using addPose() first!

Definition at line 100 of file timed_elastic_band.cpp.

void teb_local_planner::TimedElasticBand::addPoseAndTimeDiff	(	const Eigen::Ref< const Eigen::Vector2d > &	position,
		double	theta,
		double	dt
	)

Append a (pose, timediff) vertex pair to the end of the current trajectory (pose and timediff sequences)

Parameters

position	2D vector representing the position part
theta	yaw angle representing the orientation part
dt	time difference value to push back on the internal TimeDiffSequence

Warning: see addPoseAndTimeDiff(const PoseSE2& pose, double dt)

Definition at line 111 of file timed_elastic_band.cpp.

void teb_local_planner::TimedElasticBand::addPoseAndTimeDiff	(	double	x,
		double	y,
		double	theta,
		double	dt
	)

Append a (pose, timediff) vertex pair to the end of the current trajectory (pose and timediff sequences)

Parameters

x	x-coordinate of the position part
y	y-coordinate of the position part
theta	yaw angle representing the orientation part
dt	time difference value to push back on the internal TimeDiffSequence

Warning: see addPoseAndTimeDiff(const PoseSE2& pose, double dt)

Definition at line 86 of file timed_elastic_band.cpp.

void teb_local_planner::TimedElasticBand::addTimeDiff	(	double	dt,
		bool	fixed = `false`
	)

Append a new time difference vertex to the back of the time diff sequence.

Parameters

dt	time difference value to push back on the internal TimeDiffSequence
fixed	Mark the pose to be fixed or unfixed during trajectory optimization (important for the TebOptimalPlanner)

Definition at line 78 of file timed_elastic_band.cpp.

void teb_local_planner::TimedElasticBand::autoResize	(	double	dt_ref,
		double	dt_hysteresis,
		int	min_samples = `3`,
		int	max_samples = `1000`,
		bool	fast_mode = `false`
	)

Resize the trajectory by removing or inserting a (pose,dt) pair depending on a reference temporal resolution.

Resizing the trajectory is helpful e.g. for the following scenarios:

    - Obstacles requires the teb to be extended in order to 
      satisfy the given discritization width (plan resolution)
 and to avoid undesirable behavior due to a large/small discretization step widths \form#16
      After clearance of obstacles, the teb should (re-) contract to its (time-)optimal version.

If the distance to the goal state is getting smaller, dt is decreasing as well. This leads to a heavily fine-grained discretization in combination with many discrete poses. Thus, the computation time will be/remain high and in addition numerical instabilities can appear (e.g. due to the division by a small $\Delta T_i$ ).

The implemented strategy checks all timediffs $\Delta T_i$ and

    - inserts a new sample if \form#37

removes a sample if $\Delta T_i < \Delta T_{ref} - \Delta T_{hyst}$

Each call only one new sample (pose-dt-pair) is inserted or removed.

Parameters

dt_ref	reference temporal resolution
dt_hysteresis	hysteresis to avoid oscillations
min_samples	minimum number of samples that should be remain in the trajectory after resizing
max_samples	maximum number of samples that should not be exceeded during resizing
fast_mode	if true, the trajectory is iterated once to insert or erase points; if false the trajectory is repeatedly iterated until no poses are added or removed anymore

iterate through all TEB states and add/remove states!

Definition at line 203 of file timed_elastic_band.cpp.

PoseSE2& teb_local_planner::TimedElasticBand::BackPose ( )

inline

Access the last PoseSE2 in the pose sequence.

Definition at line 176 of file timed_elastic_band.h.

const PoseSE2& teb_local_planner::TimedElasticBand::BackPose ( ) const

inline

Access the last PoseSE2 in the pose sequence (read-only)

Definition at line 181 of file timed_elastic_band.h.

double& teb_local_planner::TimedElasticBand::BackTimeDiff ( )

inline

Access the last TimeDiff in the time diff sequence.

Definition at line 186 of file timed_elastic_band.h.

const double& teb_local_planner::TimedElasticBand::BackTimeDiff ( ) const

inline

Access the last TimeDiff in the time diff sequence (read-only)

Definition at line 191 of file timed_elastic_band.h.

void teb_local_planner::TimedElasticBand::clearTimedElasticBand ( )

clear all poses and timediffs from the trajectory. The pose and timediff sequences will be empty and isInit() will return false

Definition at line 178 of file timed_elastic_band.cpp.

void teb_local_planner::TimedElasticBand::deletePose ( int index )

Delete pose at pos. index in the pose sequence.

Parameters

index element position inside the internal PoseSequence

Definition at line 123 of file timed_elastic_band.cpp.

void teb_local_planner::TimedElasticBand::deletePoses	(	int	index,
		int	number
	)

Delete multiple (number) poses starting at pos. index in the pose sequence.

Parameters

index	first element position inside the internal PoseSequence
number	number of elements that should be deleted

Definition at line 130 of file timed_elastic_band.cpp.

void teb_local_planner::TimedElasticBand::deleteTimeDiff ( int index )

Delete pose at pos. index in the timediff sequence.

Parameters

index element position inside the internal TimeDiffSequence

Definition at line 138 of file timed_elastic_band.cpp.

void teb_local_planner::TimedElasticBand::deleteTimeDiffs	(	int	index,
		int	number
	)

Delete multiple (number) time differences starting at pos. index in the timediff sequence.

Parameters

index	first element position inside the internal TimeDiffSequence
number	number of elements that should be deleted

Definition at line 145 of file timed_elastic_band.cpp.

bool teb_local_planner::TimedElasticBand::detectDetoursBackwards ( double threshold = 0 ) const

Detect whether the trajectory contains detours.

Two different cases forces the method to return true :

The trajectory contain parts that increase the distance to the goal.
This is checked by comparing the orientations theta_i with the goal heading.
If the scalar product is below the threshold param, the method returns true.
The trajectory consist of backwards motions at the beginning of the trajectory, e.g. the second pose is behind the start pose w.r.t. to the same goal heading.

Detours are not critical, but can be takein into account if multiple trajectory candidates are avaiable.

Parameters

threshold Threshold paramter for allowed orientation changes (below 0 -> greater than 90 deg)

Returns: true if one of both cases mentioned above are satisfied, false otherwise

detect based on orientation

check if upcoming configuration (next index) ist pushed behind the start (e.g. due to obstacles)

Definition at line 552 of file timed_elastic_band.cpp.

int teb_local_planner::TimedElasticBand::findClosestTrajectoryPose	(	const Eigen::Ref< const Eigen::Vector2d > &	ref_point,
		double *	distance = `NULL`,
		int	begin_idx = `0`
	)		const

Find the closest point on the trajectory w.r.t. to a provided reference point.

This function can be useful to find the part of a trajectory that is close to an obstacle.

Todo:

implement a more efficient version that first performs a coarse search.

implement a fast approximation that assumes only one local minima w.r.t to the distance: Allows simple comparisons starting from the middle of the trajectory.

Parameters

	ref_point	reference point (2D position vector)
[out]	distance	[optional] the resulting minimum distance
	begin_idx	start search at this pose index

Returns: Index to the closest pose in the pose sequence

Definition at line 414 of file timed_elastic_band.cpp.

int teb_local_planner::TimedElasticBand::findClosestTrajectoryPose	(	const Eigen::Ref< const Eigen::Vector2d > &	ref_line_start,
		const Eigen::Ref< const Eigen::Vector2d > &	ref_line_end,
		double *	distance = `NULL`
	)		const

Find the closest point on the trajectory w.r.t. to a provided reference line.

This function can be useful to find the part of a trajectory that is close to an (line) obstacle.

Todo:

implement a more efficient version that first performs a coarse search.

implement a fast approximation that assumes only one local minima w.r.t to the distance: Allows simple comparisons starting from the middle of the trajectory.

Parameters

	ref_line_start	start of the reference line (2D position vector)
	ref_line_end	end of the reference line (2D position vector)
[out]	distance	[optional] the resulting minimum distance

Returns: Index to the closest pose in the pose sequence

Definition at line 449 of file timed_elastic_band.cpp.

int teb_local_planner::TimedElasticBand::findClosestTrajectoryPose	(	const Point2dContainer &	vertices,
		double *	distance = `NULL`
	)		const

Find the closest point on the trajectory w.r.t. to a provided reference polygon.

This function can be useful to find the part of a trajectory that is close to an (polygon) obstacle.

Todo:

implement a more efficient version that first performs a coarse search.

implement a fast approximation that assumes only one local minima w.r.t to the distance: Allows simple comparisons starting from the middle of the trajectory.

Parameters

	vertices	vertex container containing Eigen::Vector2d points (the last and first point are connected)
[out]	distance	[optional] the resulting minimum distance

Returns: Index to the closest pose in the pose sequence

Definition at line 484 of file timed_elastic_band.cpp.

int teb_local_planner::TimedElasticBand::findClosestTrajectoryPose	(	const Obstacle &	obstacle,
		double *	distance = `NULL`
	)		const

Find the closest point on the trajectory w.r.t to a provided obstacle type.

This function can be useful to find the part of a trajectory that is close to an obstacle. The method is calculates appropriate distance metrics for point, line and polygon obstacles. For all unknown obstacles the centroid is used.

Parameters

	obstacle	Subclass of the Obstacle base class
[out]	distance	[optional] the resulting minimum distance

Returns: Index to the closest pose in the pose sequence

Definition at line 532 of file timed_elastic_band.cpp.

double teb_local_planner::TimedElasticBand::getAccumulatedDistance ( ) const

Calculate the length (accumulated euclidean distance) of the trajectory.

Definition at line 271 of file timed_elastic_band.cpp.

double teb_local_planner::TimedElasticBand::getSumOfAllTimeDiffs ( ) const

Calculate the total transition time (sum over all time intervals of the timediff sequence)

Definition at line 246 of file timed_elastic_band.cpp.

double teb_local_planner::TimedElasticBand::getSumOfTimeDiffsUpToIdx ( int index ) const

Calculate the estimated transition time up to the pose denoted by index.

Parameters

index Index of the pose up to which the transition times are summed up

Returns: Estimated transition time up to pose index

Definition at line 257 of file timed_elastic_band.cpp.

ROS_DEPRECATED bool teb_local_planner::TimedElasticBand::initTEBtoGoal	(	const PoseSE2 &	start,
		const PoseSE2 &	goal,
		double	diststep = `0`,
		double	timestep = `1`,
		int	min_samples = `3`,
		bool	guess_backwards_motion = `false`
	)

inline

Definition at line 429 of file timed_elastic_band.h.

template<typename BidirIter , typename Fun >

ROS_DEPRECATED bool teb_local_planner::TimedElasticBand::initTEBtoGoal	(	BidirIter	path_start,
		BidirIter	path_end,
		Fun	fun_position,
		double	max_vel_x,
		double	max_vel_theta,
		boost::optional< double >	max_acc_x,
		boost::optional< double >	max_acc_theta,
		boost::optional< double >	start_orientation,
		boost::optional< double >	goal_orientation,
		int	min_samples = `3`,
		bool	guess_backwards_motion = `false`
	)

inline

Definition at line 437 of file timed_elastic_band.h.

ROS_DEPRECATED bool teb_local_planner::TimedElasticBand::initTEBtoGoal	(	const std::vector< geometry_msgs::PoseStamped > &	plan,
		double	dt,
		bool	estimate_orient = `false`,
		int	min_samples = `3`,
		bool	guess_backwards_motion = `false`
	)

inline

Definition at line 445 of file timed_elastic_band.h.

bool teb_local_planner::TimedElasticBand::initTrajectoryToGoal	(	const PoseSE2 &	start,
		const PoseSE2 &	goal,
		double	diststep = `0`,
		double	max_vel_x = `0.5`,
		int	min_samples = `3`,
		bool	guess_backwards_motion = `false`
	)

Initialize a trajectory between a given start and goal pose.

The implemented algorithm subsamples the straight line between start and goal using a given discretiziation width.
The discretization width can be defined in the euclidean space using the diststep parameter. Each time difference between two consecutive poses is initialized to timestep.
If the diststep is chosen to be zero, the resulting trajectory contains the start and goal pose only.

Parameters

start	PoseSE2 defining the start of the trajectory
goal	PoseSE2 defining the goal of the trajectory (final pose)
diststep	euclidean distance between two consecutive poses (if 0, no intermediate samples are inserted despite min_samples)
max_vel_x	maximum translational velocity used for determining time differences
min_samples	Minimum number of samples that should be initialized at least
guess_backwards_motion	Allow the initialization of backwards oriented trajectories if the goal heading is pointing behind the robot

Returns: true if everything was fine, false otherwise

Definition at line 282 of file timed_elastic_band.cpp.

template<typename BidirIter , typename Fun >

bool teb_local_planner::TimedElasticBand::initTrajectoryToGoal	(	BidirIter	path_start,
		BidirIter	path_end,
		Fun	fun_position,
		double	max_vel_x,
		double	max_vel_theta,
		boost::optional< double >	max_acc_x,
		boost::optional< double >	max_acc_theta,
		boost::optional< double >	start_orientation,
		boost::optional< double >	goal_orientation,
		int	min_samples = `3`,
		bool	guess_backwards_motion = `false`
	)

Initialize a trajectory from a generic 2D reference path.

The temporal information is determined using a given maximum velocity (2D vector containing the translational and angular velocity).
A constant velocity profile is implemented.
A possible maximum acceleration is considered if max_acceleration param is provided.

Since the orientation is not included in the reference path, it can be provided seperately (e.g. from the robot pose and robot goal).
Otherwise the goal heading will be used as start and goal orientation.
The orientation along the trajectory will be determined using the connection vector between two consecutive positions of the reference path.

The reference path is provided using a start and end iterator to a container class. You must provide a unary function that accepts the dereferenced iterator and returns a copy or (const) reference to an Eigen::Vector2d type containing the 2d position.

Parameters

path_start	start iterator of a generic 2d path
path_end	end iterator of a generic 2d path
fun_position	unary function that returns the Eigen::Vector2d object
max_vel_x	maximum translational velocity used for determining time differences
max_vel_theta	maximum angular velocity used for determining time differences
max_acc_x	specify to satisfy a maxmimum transl. acceleration and decceleration (optional)
max_acc_theta	specify to satisfy a maxmimum angular acceleration and decceleration (optional)
start_orientation	Orientation of the first pose of the trajectory (optional, otherwise use goal heading)
goal_orientation	Orientation of the last pose of the trajectory (optional, otherwise use goal heading)
min_samples	Minimum number of samples that should be initialized at least
guess_backwards_motion	Allow the initialization of backwards oriented trajectories if the goal heading is pointing behind the robot

Template Parameters

BidirIter	Bidirectional iterator type
Fun	unyary function that transforms the dereferenced iterator into an Eigen::Vector2d

Returns: true if everything was fine, false otherwise

Remarks: Use boost::none to skip optional arguments.

Definition at line 47 of file timed_elastic_band.hpp.

bool teb_local_planner::TimedElasticBand::initTrajectoryToGoal	(	const std::vector< geometry_msgs::PoseStamped > &	plan,
		double	max_vel_x,
		bool	estimate_orient = `false`,
		int	min_samples = `3`,
		bool	guess_backwards_motion = `false`
	)

Initialize a trajectory from a reference pose sequence (positions and orientations).

This method initializes the timed elastic band using a pose container (e.g. as local plan from the ros navigation stack).
The initial time difference between two consecutive poses can be uniformly set via the argument dt.

Parameters

plan	vector of geometry_msgs::PoseStamped
max_vel_x	maximum translational velocity used for determining time differences
estimate_orient	if `true`, calculate orientation using the straight line distance vector between consecutive poses (only copy start and goal orientation; recommended if no orientation data is available).
min_samples	Minimum number of samples that should be initialized at least
guess_backwards_motion	Allow the initialization of backwards oriented trajectories if the goal heading is pointing behind the robot (this parameter is used only if `estimate_orient` is enabled.

Returns: true if everything was fine, false otherwise

Definition at line 346 of file timed_elastic_band.cpp.

void teb_local_planner::TimedElasticBand::insertPose	(	int	index,
		const PoseSE2 &	pose
	)

Insert a new pose vertex at pos. index to the pose sequence.

Parameters

index	element position inside the internal PoseSequence
pose	PoseSE2 element to insert into the internal PoseSequence

Definition at line 153 of file timed_elastic_band.cpp.

void teb_local_planner::TimedElasticBand::insertPose	(	int	index,
		const Eigen::Ref< const Eigen::Vector2d > &	position,
		double	theta
	)

Insert a new pose vertex at pos. index to the pose sequence.

Parameters

index	element position inside the internal PoseSequence
position	2D vector representing the position part
theta	yaw-angle representing the orientation part

Definition at line 159 of file timed_elastic_band.cpp.

void teb_local_planner::TimedElasticBand::insertPose	(	int	index,
		double	x,
		double	y,
		double	theta
	)

Insert a new pose vertex at pos. index to the pose sequence.

Parameters

index	element position inside the internal PoseSequence
x	x-coordinate of the position part
y	y-coordinate of the position part
theta	yaw-angle representing the orientation part

Definition at line 165 of file timed_elastic_band.cpp.

void teb_local_planner::TimedElasticBand::insertTimeDiff	(	int	index,
		double	dt
	)

Insert a new timediff vertex at pos. index to the timediff sequence.

Parameters

index	element position inside the internal TimeDiffSequence
dt	timediff value

Definition at line 171 of file timed_elastic_band.cpp.

bool teb_local_planner::TimedElasticBand::isInit ( ) const

inline

Check whether the trajectory is initialized (nonzero pose and timediff sequences)

Definition at line 608 of file timed_elastic_band.h.

bool teb_local_planner::TimedElasticBand::isTrajectoryInsideRegion	(	double	radius,
		double	max_dist_behind_robot = `-1`,
		int	skip_poses = `0`
	)

Check if all trajectory points are contained in a specific region.

The specific region is a circle around the current robot position (Pose(0)) with given radius radius. This method investigates a different radius for points behind the robot if max_dist_behind_robot >= 0.

Parameters

radius	radius of the region with the robot position (Pose(0)) as center
max_dist_behind_robot	A separate radius for trajectory points behind the robot, activated if 0 or positive
skip_poses	If >0: the specified number of poses are skipped for the test, e.g. Pose(0), Pose(0+skip_poses+1), Pose(2*skip_poses+2), ... are tested.

Returns: true, if all tested trajectory points are inside the specified region, false otherwise.

Definition at line 635 of file timed_elastic_band.cpp.

PoseSE2& teb_local_planner::TimedElasticBand::Pose ( int index )

inline

Access the pose at pos index of the pose sequence.

Parameters

index element position inside the internal PoseSequence

Returns: reference to the pose at pos index

Definition at line 156 of file timed_elastic_band.h.

const PoseSE2& teb_local_planner::TimedElasticBand::Pose ( int index ) const

inline

Access the pose at pos index of the pose sequence (read-only)

Parameters

index element position inside the internal PoseSequence

Returns: const reference to the pose at pos index

Definition at line 167 of file timed_elastic_band.h.

PoseSequence& teb_local_planner::TimedElasticBand::poses ( )

inline

Access the complete pose sequence.

Returns: reference to the pose sequence

Definition at line 109 of file timed_elastic_band.h.

const PoseSequence& teb_local_planner::TimedElasticBand::poses ( ) const

inline

Access the complete pose sequence (read-only)

Returns: const reference to the pose sequence

Definition at line 115 of file timed_elastic_band.h.

VertexPose* teb_local_planner::TimedElasticBand::PoseVertex ( int index )

inline

Access the vertex of a pose at pos index for optimization purposes.

Parameters

index element position inside the internal PoseSequence

Returns: Weak raw pointer to the pose vertex at pos index

Definition at line 198 of file timed_elastic_band.h.

void teb_local_planner::TimedElasticBand::setPoseVertexFixed	(	int	index,
		bool	status
	)

Set a pose vertex at pos index of the pose sequence to be fixed or unfixed during optimization.

Parameters

index	index to the pose vertex
status	if `true`, the vertex will be fixed, otherwise unfixed

Definition at line 190 of file timed_elastic_band.cpp.

void teb_local_planner::TimedElasticBand::setTimeDiffVertexFixed	(	int	index,
		bool	status
	)

Set a timediff vertex at pos index of the timediff sequence to be fixed or unfixed during optimization.

Parameters

index	index to the timediff vertex
status	if `true`, the vertex will be fixed, otherwise unfixed

Definition at line 196 of file timed_elastic_band.cpp.

int teb_local_planner::TimedElasticBand::sizePoses ( ) const

inline

Get the length of the internal pose sequence.

Definition at line 598 of file timed_elastic_band.h.

int teb_local_planner::TimedElasticBand::sizeTimeDiffs ( ) const

inline

Get the length of the internal timediff sequence.

Definition at line 603 of file timed_elastic_band.h.

double& teb_local_planner::TimedElasticBand::TimeDiff ( int index )

inline

Access the time difference at pos index of the time sequence.

Parameters

index element position inside the internal TimeDiffSequence

Returns: reference to the time difference at pos index

Definition at line 134 of file timed_elastic_band.h.

const double& teb_local_planner::TimedElasticBand::TimeDiff ( int index ) const

inline

Access the time difference at pos index of the time sequence (read-only)

Parameters

index element position inside the internal TimeDiffSequence

Returns: const reference to the time difference at pos index

Definition at line 145 of file timed_elastic_band.h.

TimeDiffSequence& teb_local_planner::TimedElasticBand::timediffs ( )

inline

Access the complete timediff sequence.

Returns: reference to the dimediff sequence

Definition at line 121 of file timed_elastic_band.h.

const TimeDiffSequence& teb_local_planner::TimedElasticBand::timediffs ( ) const

inline

Access the complete timediff sequence.

Returns: reference to the dimediff sequence

Definition at line 127 of file timed_elastic_band.h.

VertexTimeDiff* teb_local_planner::TimedElasticBand::TimeDiffVertex ( int index )

inline

Access the vertex of a time difference at pos index for optimization purposes.

Parameters

index element position inside the internal TimeDiffSequence

Returns: Weak raw pointer to the timediff vertex at pos index

Definition at line 209 of file timed_elastic_band.h.

void teb_local_planner::TimedElasticBand::updateAndPruneTEB	(	boost::optional< const PoseSE2 & >	new_start,
		boost::optional< const PoseSE2 & >	new_goal,
		int	min_samples = `3`
	)

Hot-Start from an existing trajectory with updated start and goal poses.

This method updates a previously optimized trajectory with a new start and/or a new goal pose.
The current simple implementation cuts of pieces of the trajectory that are already passed due to the new start.
Afterwards the start and goal pose are replaced by the new ones. The resulting discontinuity will not be smoothed. The optimizer has to smooth the trajectory in TebOptimalPlanner.

Todo:

Smooth the trajectory here and test the performance improvement of the optimization.

Implement a updateAndPruneTEB based on a new reference path / pose sequence.

Parameters

new_start	New start pose (optional)
new_goal	New goal pose (optional)
min_samples	Specify the minimum number of samples that should at least remain in the trajectory

Definition at line 590 of file timed_elastic_band.cpp.

Member Data Documentation

PoseSequence teb_local_planner::TimedElasticBand::pose_vec_

protected

Internal container storing the sequence of optimzable pose vertices.

Definition at line 660 of file timed_elastic_band.h.

TimeDiffSequence teb_local_planner::TimedElasticBand::timediff_vec_

protected

Internal container storing the sequence of optimzable timediff vertices.

Definition at line 661 of file timed_elastic_band.h.

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

Public Member Functions

Protected Attributes

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation