timed_elastic_band.h
Go to the documentation of this file.
1 /*********************************************************************
2  *
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2016,
6  * TU Dortmund - Institute of Control Theory and Systems Engineering.
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of the institute nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  * Author: Christoph Rösmann
37  *********************************************************************/
38 
39 #ifndef TIMED_ELASTIC_BAND_H_
40 #define TIMED_ELASTIC_BAND_H_
41 
42 #include <ros/ros.h>
43 #include <ros/assert.h>
44 
45 #include <geometry_msgs/PoseStamped.h>
46 #include <geometry_msgs/PoseArray.h>
47 #include <tf/tf.h>
48 
49 #include <complex>
50 #include <iterator>
51 
53 
54 // G2O Types
57 
58 
59 namespace teb_local_planner
60 {
61 
63 typedef std::vector<VertexPose*> PoseSequence;
65 typedef std::vector<VertexTimeDiff*> TimeDiffSequence;
66 
67 
87 {
88 public:
89 
94 
98  virtual ~TimedElasticBand();
99 
100 
101 
104 
109  PoseSequence& poses() {return pose_vec_;};
110 
115  const PoseSequence& poses() const {return pose_vec_;};
116 
121  TimeDiffSequence& timediffs() {return timediff_vec_;};
122 
127  const TimeDiffSequence& timediffs() const {return timediff_vec_;};
128 
134  double& TimeDiff(int index)
135  {
136  ROS_ASSERT(index<sizeTimeDiffs());
137  return timediff_vec_.at(index)->dt();
138  }
139 
145  const double& TimeDiff(int index) const
146  {
147  ROS_ASSERT(index<sizeTimeDiffs());
148  return timediff_vec_.at(index)->dt();
149  }
150 
156  PoseSE2& Pose(int index)
157  {
158  ROS_ASSERT(index<sizePoses());
159  return pose_vec_.at(index)->pose();
160  }
161 
167  const PoseSE2& Pose(int index) const
168  {
169  ROS_ASSERT(index<sizePoses());
170  return pose_vec_.at(index)->pose();
171  }
172 
176  PoseSE2& BackPose() {return pose_vec_.back()->pose(); }
177 
181  const PoseSE2& BackPose() const {return pose_vec_.back()->pose();}
182 
186  double& BackTimeDiff() {return timediff_vec_.back()->dt(); }
187 
191  const double& BackTimeDiff() const {return timediff_vec_.back()->dt(); }
192 
198  VertexPose* PoseVertex(int index)
199  {
200  ROS_ASSERT(index<sizePoses());
201  return pose_vec_.at(index);
202  }
203 
210  {
211  ROS_ASSERT(index<sizeTimeDiffs());
212  return timediff_vec_.at(index);
213  }
214 
216 
217 
218 
221 
227  void addPose(const PoseSE2& pose, bool fixed=false);
228 
235  void addPose(const Eigen::Ref<const Eigen::Vector2d>& position, double theta, bool fixed=false);
236 
244  void addPose(double x, double y, double theta, bool fixed=false);
245 
251  void addTimeDiff(double dt, bool fixed=false);
252 
261  void addPoseAndTimeDiff(const PoseSE2& pose, double dt);
262 
270  void addPoseAndTimeDiff(const Eigen::Ref<const Eigen::Vector2d>& position, double theta, double dt);
271 
280  void addPoseAndTimeDiff(double x, double y, double theta, double dt);
281 
283 
284 
287 
293  void insertPose(int index, const PoseSE2& pose);
294 
301  void insertPose(int index, const Eigen::Ref<const Eigen::Vector2d>& position, double theta);
302 
310  void insertPose(int index, double x, double y, double theta);
311 
317  void insertTimeDiff(int index, double dt);
318 
323  void deletePose(int index);
324 
330  void deletePoses(int index, int number);
331 
336  void deleteTimeDiff(int index);
337 
343  void deleteTimeDiffs(int index, int number);
344 
346 
347 
350 
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);
370 
371 
406  template<typename BidirIter, 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);
410 
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);
427 
428 
429  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)
430  {
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);
434  }
435 
436  template<typename BidirIter, typename Fun>
437  ROS_DEPRECATED bool initTEBtoGoal(BidirIter path_start, BidirIter path_end, Fun fun_position, double max_vel_x, double max_vel_theta,
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)
440  {
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);
443  }
444 
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)
446  {
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.");
449  return initTrajectoryToGoal(plan, dt, estimate_orient, min_samples, guess_backwards_motion);
450  }
451 
452 
454 
457 
458 
474  void updateAndPruneTEB(boost::optional<const PoseSE2&> new_start, boost::optional<const PoseSE2&> new_goal, int min_samples = 3);
475 
476 
506  void autoResize(double dt_ref, double dt_hysteresis, int min_samples = 3, int max_samples=1000, bool fast_mode=false);
507 
513  void setPoseVertexFixed(int index, bool status);
514 
520  void setTimeDiffVertexFixed(int index, bool status);
521 
526  void clearTimedElasticBand();
527 
529 
530 
533 
548  int findClosestTrajectoryPose(const Eigen::Ref<const Eigen::Vector2d>& ref_point, double* distance = NULL, int begin_idx=0) const;
549 
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;
565 
579  int findClosestTrajectoryPose(const Point2dContainer& vertices, double* distance = NULL) const;
580 
592  int findClosestTrajectoryPose(const Obstacle& obstacle, double* distance = NULL) const;
593 
594 
598  int sizePoses() const {return (int)pose_vec_.size();};
599 
603  int sizeTimeDiffs() const {return (int)timediff_vec_.size();};
604 
608  bool isInit() const {return !timediff_vec_.empty() && !pose_vec_.empty();}
609 
613  double getSumOfAllTimeDiffs() const;
614 
620  double getSumOfTimeDiffsUpToIdx(int index) const;
621 
625  double getAccumulatedDistance() const;
626 
637  bool isTrajectoryInsideRegion(double radius, double max_dist_behind_robot=-1, int skip_poses=0);
638 
639 
640 
642 
643 protected:
644  PoseSequence pose_vec_;
645  TimeDiffSequence timediff_vec_;
646 
647 public:
648  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
649 };
650 
651 } // namespace teb_local_planner
652 
653 
654 // include template implementations / definitions
656 
657 
658 #endif /* TIMED_ELASTIC_BAND_H_ */
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.
#define ROS_DEPRECATED
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...
Definition: pose_se2.h:57
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...
Definition: vertex_pose.h:63
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...
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)
#define ROS_ASSERT(cond)
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.
Definition: obstacles.h:67
This class stores and wraps a time difference into a vertex that can be optimized via g2o...


teb_local_planner
Author(s): Christoph Rösmann
autogenerated on Wed Jun 3 2020 04:03:08