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

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

#include <timed_elastic_band.h>

List of all members.

Public Member Functions

 TimedElasticBand ()
 Construct the class.
virtual ~TimedElasticBand ()
 Destruct the class.
Access pose and timediff sequences
PoseSequence & poses ()
 Access the complete pose sequence.
const PoseSequence & poses () const
 Access the complete pose sequence (read-only)
TimeDiffSequence & timediffs ()
 Access the complete timediff sequence.
const TimeDiffSequence & timediffs () const
 Access the complete timediff sequence.
double & TimeDiff (int index)
 Access the time difference at pos index of the time sequence.
const double & TimeDiff (int index) const
 Access the time difference at pos index of the time sequence (read-only)
PoseSE2 & Pose (int index)
 Access the pose at pos index of the pose sequence.
const PoseSE2 & Pose (int index) const
 Access the pose at pos index of the pose sequence (read-only)
PoseSE2 & BackPose ()
 Access the last PoseSE2 in the pose sequence.
const PoseSE2 & BackPose () const
 Access the last PoseSE2 in the pose sequence (read-only)
double & BackTimeDiff ()
 Access the last TimeDiff in the time diff sequence.
const double & BackTimeDiff () const
 Access the last TimeDiff in the time diff sequence (read-only)
VertexPose * PoseVertex (int index)
 Access the vertex of a pose at pos index for optimization purposes.
VertexTimeDiff * TimeDiffVertex (int index)
 Access the vertex of a time difference at pos index for optimization purposes.
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.
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.
void addPose (double x, double y, double theta, bool fixed=false)
 Append a new pose vertex to the back of the pose sequence.
void addTimeDiff (double dt, bool fixed=false)
 Append a new time difference vertex to the back of the time diff sequence.
void addPoseAndTimeDiff (const PoseSE2 &pose, double dt)
 Append a (pose, timediff) vertex pair to the end of the current trajectory (pose and timediff sequences)
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)
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)
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.
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.
void insertPose (int index, double x, double y, double theta)
 Insert a new pose vertex at pos. index to the pose sequence.
void insertTimeDiff (int index, double dt)
 Insert a new timediff vertex at pos. index to the timediff sequence.
void deletePose (int index)
 Delete pose at pos. index in the pose sequence.
void deletePoses (int index, int number)
 Delete multiple (number) poses starting at pos. index in the pose sequence.
void deleteTimeDiff (int index)
 Delete pose at pos. index in the timediff sequence.
void deleteTimeDiffs (int index, int number)
 Delete multiple (number) time differences starting at pos. index in the timediff sequence.
Init the trajectory
bool initTEBtoGoal (const PoseSE2 &start, const PoseSE2 &goal, double diststep=0, double timestep=1, int min_samples=3, bool guess_backwards_motion=false)
 Initialize a trajectory between a given start and goal pose.
template<typename BidirIter , typename Fun >
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)
 Initialize a trajectory from a generic 2D reference path.
bool initTEBtoGoal (const std::vector< geometry_msgs::PoseStamped > &plan, double dt, bool estimate_orient=false, int min_samples=3, bool guess_backwards_motion=false)
 Initialize a trajectory from a reference pose sequence (positions and orientations).
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.
void autoResize (double dt_ref, double dt_hysteresis, int min_samples=3, int max_samples=1000)
 Resize the trajectory by removing or inserting a (pose,dt) pair depending on a reference temporal resolution.
void setPoseVertexFixed (int index, bool status)
 Set a pose vertex at pos index of the pose sequence to be fixed or unfixed during optimization.
void setTimeDiffVertexFixed (int index, bool status)
 Set a timediff vertex at pos index of the timediff sequence to be fixed or unfixed during optimization.
void clearTimedElasticBand ()
 clear all poses and timediffs from the trajectory. The pose and timediff sequences will be empty and isInit() will return false
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.
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.
int findClosestTrajectoryPose (const Point2dContainer &vertices, double *distance=NULL) const
 Find the closest point on the trajectory w.r.t. to a provided reference polygon.
int findClosestTrajectoryPose (const Obstacle &obstacle, double *distance=NULL) const
 Find the closest point on the trajectory w.r.t to a provided obstacle type.
int sizePoses () const
 Get the length of the internal pose sequence.
int sizeTimeDiffs () const
 Get the length of the internal timediff sequence.
bool isInit () const
 Check whether the trajectory is initialized (nonzero pose and timediff sequences)
double getSumOfAllTimeDiffs () const
 Calculate the total transition time (sum over all time intervals of the timediff sequence)
double getAccumulatedDistance () const
 Calculate the length (accumulated euclidean distance) of the trajectory.
bool detectDetoursBackwards (double threshold=0) const
 Detect whether the trajectory contains detours.
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.

Protected Attributes

PoseSequence pose_vec_
 Internal container storing the sequence of optimzable pose vertices.
TimeDiffSequence timediff_vec_
 Internal container storing the sequence of optimzable timediff vertices.

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:
posePoseSE2 to push back on the internal PoseSequence
fixedMark 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:
position2D vector representing the position part
thetayaw angle representing the orientation part
fixedMark 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:
xx-coordinate of the position part
yy-coordinate of the position part
thetayaw angle representing the orientation part
fixedMark 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:
posePoseSE2 to push back on the internal PoseSequence
dttime 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:
position2D vector representing the position part
thetayaw angle representing the orientation part
dttime 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:
xx-coordinate of the position part
yy-coordinate of the position part
thetayaw angle representing the orientation part
dttime 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:
dttime difference value to push back on the internal TimeDiffSequence
fixedMark 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 
)

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 $ \Delta T_i $ 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 $ \Delta T_i > \Delta T_{ref} + \Delta T_{hyst} $

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_refreference temporal resolution
dt_hysteresishysteresis to avoid oscillations
min_samplesminimum number of samples that should be remain in the trajectory after resizing
max_samplesmaximum number of samples that should not be exceeded during resizing

iterate through all TEB states only once 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:
indexelement 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:
indexfirst element position inside the internal PoseSequence
numbernumber 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:
indexelement 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:
indexfirst element position inside the internal TimeDiffSequence
numbernumber 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 :
1. 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. 2. 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:
thresholdThreshold 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 512 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_pointreference point (2D position vector)
[out]distance[optional] the resulting minimum distance
begin_idxstart search at this pose index
Returns:
Index to the closest pose in the pose sequence

Definition at line 374 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_startstart of the reference line (2D position vector)
ref_line_endend 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 409 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:
verticesvertex 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 444 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:
obstacleSubclass 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 492 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 246 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 235 of file timed_elastic_band.cpp.

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 
)

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:
startPoseSE2 defining the start of the trajectory
goalPoseSE2 defining the goal of the trajectory (final pose)
diststepeuclidean distance between two consecutive poses (if 0, no intermediate samples are inserted despite min_samples)
timestepintialization for the timediff between two consecutive poses
min_samplesMinimum number of samples that should be initialized at least
guess_backwards_motionAllow 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 257 of file timed_elastic_band.cpp.

template<typename BidirIter , typename Fun >
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 
)

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_startstart iterator of a generic 2d path
path_endend iterator of a generic 2d path
fun_positionunary function that returns the Eigen::Vector2d object
max_vel_xmaximum translational velocity used for determining time differences
max_vel_thetamaximum angular velocity used for determining time differences
max_acc_xspecify to satisfy a maxmimum transl. acceleration and decceleration (optional)
max_acc_thetaspecify to satisfy a maxmimum angular acceleration and decceleration (optional)
start_orientationOrientation of the first pose of the trajectory (optional, otherwise use goal heading)
goal_orientationOrientation of the last pose of the trajectory (optional, otherwise use goal heading)
min_samplesMinimum number of samples that should be initialized at least
guess_backwards_motionAllow the initialization of backwards oriented trajectories if the goal heading is pointing behind the robot
Template Parameters:
BidirIterBidirectional iterator type
Fununyary 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::initTEBtoGoal ( const std::vector< geometry_msgs::PoseStamped > &  plan,
double  dt,
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:
planvector of geometry_msgs::PoseStamped
dtspecify a uniform time difference between two consecutive poses
estimate_orientif 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_samplesMinimum number of samples that should be initialized at least
guess_backwards_motionAllow 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 313 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:
indexelement position inside the internal PoseSequence
posePoseSE2 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:
indexelement position inside the internal PoseSequence
position2D vector representing the position part
thetayaw-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:
indexelement position inside the internal PoseSequence
xx-coordinate of the position part
yy-coordinate of the position part
thetayaw-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:
indexelement position inside the internal TimeDiffSequence
dttimediff 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 582 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:
radiusradius of the region with the robot position (Pose(0)) as center
max_dist_behind_robotA separate radius for trajectory points behind the robot, activated if 0 or positive
skip_posesIf >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 595 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:
indexelement 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:
indexelement 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:
indexelement 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:
indexindex to the pose vertex
statusif 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:
indexindex to the timediff vertex
statusif 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 572 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 577 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:
indexelement 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:
indexelement 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:
indexelement 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_startNew start pose (optional)
new_goalNew goal pose (optional)
min_samplesSpecify the minimum number of samples that should at least remain in the trajectory

Definition at line 550 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 627 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 628 of file timed_elastic_band.h.

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

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