homotopy_class_planner.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 HOMOTOPY_CLASS_PLANNER_H_
40 #define HOMOTOPY_CLASS_PLANNER_H_
41 
42 #include <math.h>
43 #include <algorithm>
44 #include <functional>
45 #include <vector>
46 #include <iterator>
47 
48 #include <boost/shared_ptr.hpp>
49 
50 #include <visualization_msgs/Marker.h>
51 #include <geometry_msgs/Point.h>
52 #include <std_msgs/ColorRGBA.h>
53 
54 #include <ros/console.h>
55 #include <ros/ros.h>
56 
65 
66 
67 namespace teb_local_planner
68 {
69 
71 inline std::complex<long double> getCplxFromVertexPosePtr(const VertexPose* pose)
72 {
73  return std::complex<long double>(pose->x(), pose->y());
74 };
75 
76 
78 inline std::complex<long double> getCplxFromMsgPoseStamped(const geometry_msgs::PoseStamped& pose)
79 {
80  return std::complex<long double>(pose.pose.position.x, pose.pose.position.y);
81 };
82 
108 {
109 public:
110 
111  using EquivalenceClassContainer = std::vector< std::pair<EquivalenceClassPtr, bool> >;
112 
117 
126  HomotopyClassPlanner(const TebConfig& cfg, ObstContainer* obstacles = NULL, RobotFootprintModelPtr robot_model = boost::make_shared<PointRobotFootprint>(),
127  TebVisualizationPtr visualization = TebVisualizationPtr(), const ViaPointContainer* via_points = NULL);
128 
132  virtual ~HomotopyClassPlanner();
133 
142  void initialize(const TebConfig& cfg, ObstContainer* obstacles = NULL, RobotFootprintModelPtr robot_model = boost::make_shared<PointRobotFootprint>(),
143  TebVisualizationPtr visualization = TebVisualizationPtr(), const ViaPointContainer* via_points = NULL);
144 
145 
146 
149 
162  virtual bool plan(const std::vector<geometry_msgs::PoseStamped>& initial_plan, const geometry_msgs::Twist* start_vel = NULL, bool free_goal_vel=false);
163 
175  virtual bool plan(const tf::Pose& start, const tf::Pose& goal, const geometry_msgs::Twist* start_vel = NULL, bool free_goal_vel=false);
176 
188  virtual bool plan(const PoseSE2& start, const PoseSE2& goal, const geometry_msgs::Twist* start_vel = NULL, bool free_goal_vel=false);
189 
199  virtual bool getVelocityCommand(double& vx, double& vy, double& omega, int look_ahead_poses) const;
200 
209  TebOptimalPlannerPtr bestTeb() const {return tebs_.empty() ? TebOptimalPlannerPtr() : tebs_.size()==1 ? tebs_.front() : best_teb_;}
210 
224  virtual bool isTrajectoryFeasible(base_local_planner::CostmapModel* costmap_model, const std::vector<geometry_msgs::Point>& footprint_spec,
225  double inscribed_radius = 0.0, double circumscribed_radius=0.0, int look_ahead_idx=-1);
226 
234 
240  TebOptPlannerContainer::iterator removeTeb(TebOptimalPlannerPtr& teb);
241 
243 
246 
252  void setVisualization(TebVisualizationPtr visualization);
253 
260  virtual void visualize();
261 
263 
266 
267 
283  void exploreEquivalenceClassesAndInitTebs(const PoseSE2& start, const PoseSE2& goal, double dist_to_obst, const geometry_msgs::Twist* start_vel);
284 
299  template<typename BidirIter, typename Fun>
300  TebOptimalPlannerPtr addAndInitNewTeb(BidirIter path_start, BidirIter path_end, Fun fun_position, double start_orientation, double goal_orientation, const geometry_msgs::Twist* start_velocity);
301 
309  TebOptimalPlannerPtr addAndInitNewTeb(const PoseSE2& start, const PoseSE2& goal, const geometry_msgs::Twist* start_velocity);
310 
317  TebOptimalPlannerPtr addAndInitNewTeb(const std::vector<geometry_msgs::PoseStamped>& initial_plan, const geometry_msgs::Twist* start_velocity);
318 
325  void updateAllTEBs(const PoseSE2* start, const PoseSE2* goal, const geometry_msgs::Twist* start_velocity);
326 
327 
335  void optimizeAllTEBs(int iter_innerloop, int iter_outerloop);
336 
342 
351 
353 
359  virtual void clearPlanner() {clearGraph(); equivalence_classes_.clear(); tebs_.clear(); initial_plan_ = NULL;}
360 
361 
370  virtual void setPreferredTurningDir(RotType dir);
371 
385  template<typename BidirIter, typename Fun>
386  EquivalenceClassPtr calculateEquivalenceClass(BidirIter path_start, BidirIter path_end, Fun fun_cplx_point, const ObstContainer* obstacles = NULL,
387  boost::optional<TimeDiffSequence::iterator> timediff_start = boost::none, boost::optional<TimeDiffSequence::iterator> timediff_end = boost::none);
388 
394 
403  virtual void computeCurrentCost(std::vector<double>& cost, double obst_cost_scale=1.0, double viapoint_cost_scale=1.0, bool alternative_time_cost=false);
404 
411  inline static bool isHSignatureSimilar(const std::complex<long double>& h1, const std::complex<long double>& h2, double threshold)
412  {
413  double diff_real = std::abs(h2.real() - h1.real());
414  double diff_imag = std::abs(h2.imag() - h1.imag());
415  if (diff_real<=threshold && diff_imag<=threshold)
416  return true; // Found! Homotopy class already exists, therefore nothing added
417  return false;
418  }
426  void deletePlansDetouringBackwards(const double orient_threshold, const double len_orientation_vector);
436  bool computeStartOrientation(const TebOptimalPlannerPtr plan, const double len_orientation_vector, double& orientation);
437 
438 
443  const TebConfig* config() const {return cfg_;}
444 
449  const ObstContainer* obstacles() const {return obstacles_;}
450 
454  bool isInitialized() const {return initialized_;}
455 
459  void clearGraph() {if(graph_search_) graph_search_->clearGraph();}
460 
466  int bestTebIdx() const;
467 
468 
475  bool addEquivalenceClassIfNew(const EquivalenceClassPtr& eq_class, bool lock=false);
476 
482 
483 
484 protected:
485 
488 
494  bool hasEquivalenceClass(const EquivalenceClassPtr& eq_class) const;
495 
496 
506  void renewAndAnalyzeOldTebs(bool delete_detours);
507 
516  void updateReferenceTrajectoryViaPoints(bool all_trajectories);
517 
519 
520 
521  // external objects (store weak pointers)
522  const TebConfig* cfg_;
525 
526  // internal objects (memory management owned)
530 
531  const std::vector<geometry_msgs::PoseStamped>* initial_plan_;
534 
536 
538  // The second parameter denotes whether to exclude the class from detour deletion or not (true: force keeping).
539 
541 
543 
545 
547 
548 
549 
550 public:
551  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
552 
553 
554 };
555 
558 
559 
560 } // namespace teb_local_planner
561 
562 // include template implementations / definitions
564 
565 #endif /* HOMOTOPY_CLASS_PLANNER_H_ */
boost::shared_ptr< GraphSearchInterface > graph_search_
virtual bool isTrajectoryFeasible(base_local_planner::CostmapModel *costmap_model, const std::vector< geometry_msgs::Point > &footprint_spec, double inscribed_radius=0.0, double circumscribed_radius=0.0, int look_ahead_idx=-1)
Check whether the planned trajectory is feasible or not.
TebVisualizationPtr visualization_
Instance of the visualization class (local/global plan, obstacles, ...)
boost::shared_ptr< HomotopyClassPlanner > HomotopyClassPlannerPtr
Abbrev. for a shared pointer of a HomotopyClassPlanner instance.
std::complex< long double > getCplxFromMsgPoseStamped(const geometry_msgs::PoseStamped &pose)
Inline function used for calculateHSignature() in combination with geometry_msgs::PoseStamped.
This abstract class defines an interface for local planners.
bool computeStartOrientation(const TebOptimalPlannerPtr plan, const double len_orientation_vector, double &orientation)
Given a plan, computes its start orientation using a vector of length >= len_orientation_vector start...
void exploreEquivalenceClassesAndInitTebs(const PoseSE2 &start, const PoseSE2 &goal, double dist_to_obst, const geometry_msgs::Twist *start_vel)
Explore paths in new equivalence classes (e.g. homotopy classes) and initialize TEBs from them...
double & y()
Access the y-coordinate the pose.
Definition: vertex_pose.h:166
bool isInitialized() const
Returns true if the planner is initialized.
const ObstContainer * obstacles() const
Access current obstacle container (read-only)
Config class for the teb_local_planner and its components.
Definition: teb_config.h:61
const std::vector< geometry_msgs::PoseStamped > * initial_plan_
Store the initial plan if available for a better trajectory initialization.
EquivalenceClassPtr initial_plan_eq_class_
Store the equivalence class of the initial plan.
TebOptimalPlannerPtr bestTeb() const
Access current best trajectory candidate (that relates to the "best" homotopy class).
bool initialized_
Keeps track about the correct initialization of this class.
const TebConfig * config() const
Access config (read-only)
virtual void computeCurrentCost(std::vector< double > &cost, double obst_cost_scale=1.0, double viapoint_cost_scale=1.0, bool alternative_time_cost=false)
Local planner that explores alternative homotopy classes, create a plan for each alternative and fina...
void deletePlansDetouringBackwards(const double orient_threshold, const double len_orientation_vector)
Checks if the orientation of the computed trajectories differs from that of the best plan of more tha...
TebOptimalPlannerPtr selectBestTeb()
In case of multiple, internally stored, alternative trajectories, select the best one according to th...
TebOptimalPlannerPtr initial_plan_teb_
Store pointer to the TEB related to the initial plan (use method getInitialPlanTEB() since it checks ...
TebOptimalPlannerPtr findBestTeb()
In case of empty best teb, scores again the available plans to find the best one. The best_teb_ varia...
TebOptimalPlannerPtr addAndInitNewTeb(BidirIter path_start, BidirIter path_end, Fun fun_position, double start_orientation, double goal_orientation, const geometry_msgs::Twist *start_velocity)
Add a new Teb to the internal trajectory container, if this teb constitutes a new equivalence class...
void updateReferenceTrajectoryViaPoints(bool all_trajectories)
Associate trajectories with via-points.
EquivalenceClassPtr calculateEquivalenceClass(BidirIter path_start, BidirIter path_end, Fun fun_cplx_point, const ObstContainer *obstacles=NULL, boost::optional< TimeDiffSequence::iterator > timediff_start=boost::none, boost::optional< TimeDiffSequence::iterator > timediff_end=boost::none)
Calculate the equivalence class of a path.
boost::shared_ptr< TebOptimalPlanner > TebOptimalPlannerPtr
Abbrev. for shared instances of the TebOptimalPlanner.
virtual bool plan(const std::vector< geometry_msgs::PoseStamped > &initial_plan, const geometry_msgs::Twist *start_vel=NULL, bool free_goal_vel=false)
Plan a trajectory based on an initial reference plan.
void clearGraph()
Clear any existing graph of the homotopy class search.
RobotFootprintModelPtr robot_model_
Robot model shared instance.
ObstContainer * obstacles_
Store obstacles that are relevant for planning.
RotType
Symbols for left/none/right rotations.
Definition: misc.h:53
const TebConfig * cfg_
Config class that stores and manages all related parameters.
const ViaPointContainer * via_points_
Store the current list of via-points.
virtual ~HomotopyClassPlanner()
Destruct the HomotopyClassPlanner.
virtual bool getVelocityCommand(double &vx, double &vy, double &omega, int look_ahead_poses) const
Get the velocity command from a previously optimized plan to control the robot at the current samplin...
void initialize(const TebConfig &cfg, ObstContainer *obstacles=NULL, RobotFootprintModelPtr robot_model=boost::make_shared< PointRobotFootprint >(), TebVisualizationPtr visualization=TebVisualizationPtr(), const ViaPointContainer *via_points=NULL)
Initialize the HomotopyClassPlanner.
boost::shared_ptr< TebVisualization > TebVisualizationPtr
Abbrev. for shared instances of the TebVisualization.
const TebOptPlannerContainer & getTrajectoryContainer() const
Read-only access to the internal trajectory container.
TebOptimalPlannerPtr getInitialPlanTEB()
Returns a shared pointer to the TEB related to the initial plan.
virtual void setPreferredTurningDir(RotType dir)
Prefer a desired initial turning direction (by penalizing the opposing one)
double & x()
Access the x-coordinate the pose.
Definition: vertex_pose.h:154
std::vector< ObstaclePtr > ObstContainer
Abbrev. for containers storing multiple obstacles.
Definition: obstacles.h:297
std::complex< long double > getCplxFromVertexPosePtr(const VertexPose *pose)
< Inline function used for calculateHSignature() in combination with VertexPose pointers ...
TebOptPlannerContainer::iterator removeTeb(TebOptimalPlannerPtr &teb)
Removes the specified teb and the corresponding homotopy class from the list of available ones...
bool hasEquivalenceClass(const EquivalenceClassPtr &eq_class) const
Check if a h-signature exists already.
const EquivalenceClassContainer & getEquivalenceClassRef() const
Return the current set of equivalence erelations (read-only)
virtual void clearPlanner()
Reset the planner.
TebOptPlannerContainer tebs_
Container that stores multiple local teb planners (for alternative equivalence classes) and their cor...
std::vector< TebOptimalPlannerPtr > TebOptPlannerContainer
Abbrev. for containers storing multiple teb optimal planners.
TebOptimalPlannerPtr last_best_teb_
Points to the plan used in the previous control cycle.
std::vector< std::pair< EquivalenceClassPtr, bool > > EquivalenceClassContainer
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
static bool isHSignatureSimilar(const std::complex< long double > &h1, const std::complex< long double > &h2, double threshold)
Check if two h-signatures are similar (w.r.t. a certain threshold)
void renewAndAnalyzeOldTebs(bool delete_detours)
Renew all found h-signatures for the new planning step based on existing TEBs. Optionally detours can...
virtual void visualize()
Publish the local plan, pose sequence and additional information via ros topics (e.g. subscribe with rviz).
EquivalenceClassContainer equivalence_classes_
Store all known quivalence classes (e.g. h-signatures) to allow checking for duplicates after finding...
This class stores and wraps a SE2 pose (position and orientation) into a vertex that can be optimized...
Definition: vertex_pose.h:63
bool addEquivalenceClassIfNew(const EquivalenceClassPtr &eq_class, bool lock=false)
Internal helper function that adds a new equivalence class to the list of known classes only if it is...
void setVisualization(TebVisualizationPtr visualization)
Register a TebVisualization class to enable visiualization routines (e.g. publish the local plan and ...
void optimizeAllTEBs(int iter_innerloop, int iter_outerloop)
Optimize all available trajectories by invoking the optimizer on each one.
ViaPointContainer via_points
TebOptimalPlannerPtr best_teb_
Store the current best teb.
void updateAllTEBs(const PoseSE2 *start, const PoseSE2 *goal, const geometry_msgs::Twist *start_velocity)
Update TEBs with new pose, goal and current velocity.
ros::Time last_eq_class_switching_time_
Store the time at which the equivalence class changed recently.
std::vector< Eigen::Vector2d, Eigen::aligned_allocator< Eigen::Vector2d > > ViaPointContainer
Typedef for a container storing via-points.
int bestTebIdx() const
find the index of the currently best TEB in the container


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