39 #ifndef TIMED_ELASTIC_BAND_H_ 40 #define TIMED_ELASTIC_BAND_H_ 45 #include <geometry_msgs/PoseStamped.h> 46 #include <geometry_msgs/PoseArray.h> 235 void addPose(
const Eigen::Ref<const Eigen::Vector2d>& position,
double theta,
bool fixed=
false);
244 void addPose(
double x,
double y,
double theta,
bool fixed=
false);
270 void addPoseAndTimeDiff(
const Eigen::Ref<const Eigen::Vector2d>& position,
double theta,
double dt);
301 void insertPose(
int index,
const Eigen::Ref<const Eigen::Vector2d>& position,
double theta);
310 void insertPose(
int index,
double x,
double y,
double theta);
369 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);
406 template<
typename B
idirIter,
typename Fun>
407 bool initTrajectoryToGoal(BidirIter path_start, BidirIter path_end, Fun fun_position,
double max_vel_x,
double max_vel_theta,
408 boost::optional<double> max_acc_x, boost::optional<double> max_acc_theta,
409 boost::optional<double> start_orientation, boost::optional<double> goal_orientation,
int min_samples = 3,
bool guess_backwards_motion =
false);
426 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);
431 ROS_WARN_ONCE(
"initTEBtoGoal is deprecated and has been replaced by initTrajectoryToGoal. The signature has changed: timestep has been replaced by max_vel_x. \ 432 this deprecated method sets max_vel_x = 1. Please update your code.");
433 return initTrajectoryToGoal(start, goal, diststep, timestep, min_samples, guess_backwards_motion);
436 template<
typename B
idirIter,
typename Fun>
438 boost::optional<double> max_acc_x, boost::optional<double> max_acc_theta,
439 boost::optional<double> start_orientation, boost::optional<double> goal_orientation,
int min_samples = 3,
bool guess_backwards_motion =
false)
441 return initTrajectoryToGoal<BidirIter, Fun>(path_start, path_end, fun_position, max_vel_x, max_vel_theta,
442 max_acc_x, max_acc_theta, start_orientation, goal_orientation, min_samples, guess_backwards_motion);
445 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)
447 ROS_WARN_ONCE(
"initTEBtoGoal is deprecated and has been replaced by initTrajectoryToGoal. The signature has changed: dt has been replaced by max_vel_x. \ 448 this deprecated method sets max_vel = 1. Please update your code.");
474 void updateAndPruneTEB(boost::optional<const PoseSE2&> new_start, boost::optional<const PoseSE2&> new_goal,
int min_samples = 3);
506 void autoResize(
double dt_ref,
double dt_hysteresis,
int min_samples = 3,
int max_samples=1000,
bool fast_mode=
false);
564 int findClosestTrajectoryPose(
const Eigen::Ref<const Eigen::Vector2d>& ref_line_start,
const Eigen::Ref<const Eigen::Vector2d>& ref_line_end,
double*
distance = NULL)
const;
648 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
const PoseSE2 & Pose(int index) const
Access the pose at pos index of the pose sequence (read-only)
void setTimeDiffVertexFixed(int index, bool status)
Set a timediff vertex at pos index of the timediff sequence to be fixed or unfixed during optimizatio...
void deleteTimeDiffs(int index, int number)
Delete multiple (number) time differences starting at pos. index in the timediff sequence.
double getAccumulatedDistance() const
Calculate the length (accumulated euclidean distance) of the trajectory.
void insertPose(int index, const PoseSE2 &pose)
Insert a new pose vertex at pos. index to the pose sequence.
PoseSequence pose_vec_
Internal container storing the sequence of optimzable pose vertices.
std::vector< VertexTimeDiff * > TimeDiffSequence
Container of time differences that define the temporal of the trajectory.
double getSumOfAllTimeDiffs() const
Calculate the total transition time (sum over all time intervals of the timediff sequence) ...
Class that defines a trajectory modeled as an elastic band with augmented tempoarl information...
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.
void addPoseAndTimeDiff(const PoseSE2 &pose, double dt)
Append a (pose, timediff) vertex pair to the end of the current trajectory (pose and timediff sequenc...
std::vector< Eigen::Vector2d, Eigen::aligned_allocator< Eigen::Vector2d > > Point2dContainer
Abbrev. for a container storing 2d points.
TimeDiffSequence & timediffs()
Access the complete timediff sequence.
PoseSE2 & BackPose()
Access the last PoseSE2 in the pose sequence.
TFSIMD_FORCE_INLINE const tfScalar & y() const
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 addPose(const PoseSE2 &pose, bool fixed=false)
Append a new pose vertex to the back of the pose sequence.
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.
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.
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)
double distance(double x0, double y0, double x1, double y1)
const double & TimeDiff(int index) const
Access the time difference at pos index of the time sequence (read-only)
int sizeTimeDiffs() const
Get the length of the internal timediff sequence.
#define ROS_WARN_ONCE(...)
const PoseSequence & poses() const
Access the complete pose sequence (read-only)
double & TimeDiff(int index)
Access the time difference at pos index of the time sequence.
double & BackTimeDiff()
Access the last TimeDiff in the time diff sequence.
TFSIMD_FORCE_INLINE const tfScalar & x() const
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.
TimeDiffSequence timediff_vec_
Internal container storing the sequence of optimzable timediff vertices.
void clearTimedElasticBand()
clear all poses and timediffs from the trajectory. The pose and timediff sequences will be empty and ...
PoseSE2 & Pose(int index)
Access the pose at pos index of the pose sequence.
VertexTimeDiff * TimeDiffVertex(int index)
Access the vertex of a time difference at pos index for optimization purposes.
This class implements a pose in the domain SE2: The pose consist of the position x and y and an orie...
std::vector< VertexPose * > PoseSequence
Container of poses that represent the spatial part of the trajectory.
VertexPose * PoseVertex(int index)
Access the vertex of a pose at pos index for optimization purposes.
virtual ~TimedElasticBand()
Destruct the class.
void addTimeDiff(double dt, bool fixed=false)
Append a new time difference vertex to the back of the time diff sequence.
This class stores and wraps a SE2 pose (position and orientation) into a vertex that can be optimized...
void deleteTimeDiff(int index)
Delete pose at pos. index in the timediff sequence.
PoseSequence & poses()
Access the complete pose sequence.
double getSumOfTimeDiffsUpToIdx(int index) const
Calculate the estimated transition time up to the pose denoted by index.
const double & BackTimeDiff() const
Access the last TimeDiff in the time diff sequence (read-only)
void setPoseVertexFixed(int index, bool status)
Set a pose vertex at pos index of the pose sequence to be fixed or unfixed during optimization...
TimedElasticBand()
Construct the class.
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)
const PoseSE2 & BackPose() const
Access the last PoseSE2 in the pose sequence (read-only)
void insertTimeDiff(int index, double dt)
Insert a new timediff vertex at pos. index to the timediff sequence.
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 res...
int sizePoses() const
Get the length of the internal pose sequence.
const TimeDiffSequence & timediffs() const
Access the complete timediff sequence.
bool isInit() const
Check whether the trajectory is initialized (nonzero pose and timediff sequences) ...
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)
Abstract class that defines the interface for modelling obstacles.
This class stores and wraps a time difference into a vertex that can be optimized via g2o...