homotopy_class_planner.hpp
Go to the documentation of this file.
1 /*********************************************************************
2  *
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2017,
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 
41 
42 namespace teb_local_planner
43 {
44 
45 
46 template<typename BidirIter, typename Fun>
47 EquivalenceClassPtr HomotopyClassPlanner::calculateEquivalenceClass(BidirIter path_start, BidirIter path_end, Fun fun_cplx_point, const ObstContainer* obstacles,
48  boost::optional<TimeDiffSequence::iterator> timediff_start, boost::optional<TimeDiffSequence::iterator> timediff_end)
49 {
51  {
52  HSignature3d* H = new HSignature3d(*cfg_);
53  H->calculateHSignature(path_start, path_end, fun_cplx_point, obstacles, timediff_start, timediff_end);
54  return EquivalenceClassPtr(H);
55  }
56  else
57  {
58  HSignature* H = new HSignature(*cfg_);
59  H->calculateHSignature(path_start, path_end, fun_cplx_point, obstacles);
60  return EquivalenceClassPtr(H);
61  }
62 }
63 
64 
65 template<typename BidirIter, typename Fun>
66 TebOptimalPlannerPtr HomotopyClassPlanner::addAndInitNewTeb(BidirIter path_start, BidirIter path_end, Fun fun_position, double start_orientation, double goal_orientation, const geometry_msgs::Twist* start_velocity)
67 {
69 
70  candidate->teb().initTrajectoryToGoal(path_start, path_end, fun_position, cfg_->robot.max_vel_x, cfg_->robot.max_vel_theta,
71  cfg_->robot.acc_lim_x, cfg_->robot.acc_lim_theta, start_orientation, goal_orientation, cfg_->trajectory.min_samples,
73 
74  if (start_velocity)
75  candidate->setVelocityStart(*start_velocity);
76 
77  EquivalenceClassPtr H = calculateEquivalenceClass(candidate->teb().poses().begin(), candidate->teb().poses().end(), getCplxFromVertexPosePtr, obstacles_,
78  candidate->teb().timediffs().begin(), candidate->teb().timediffs().end());
79 
81  {
82  tebs_.push_back(candidate);
83  return tebs_.back();
84  }
85 
86  // If the candidate constitutes no new equivalence class, return a null pointer
87  return TebOptimalPlannerPtr();
88 }
89 
90 } // namespace teb_local_planner
91 
bool allow_init_with_backwards_motion
If true, the underlying trajectories might be initialized with backwards motions in case the goal is ...
Definition: teb_config.h:77
double acc_lim_x
Maximum translational acceleration of the robot.
Definition: teb_config.h:98
The H-signature in three dimensions (here: x-y-t) defines an equivalence relation based on homology u...
Definition: h_signature.h:254
struct teb_local_planner::TebConfig::Robot robot
Robot related parameters.
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...
struct teb_local_planner::TebConfig::Trajectory trajectory
Trajectory related parameters.
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.
The H-signature defines an equivalence relation based on homology in terms of complex calculus...
Definition: h_signature.h:67
boost::shared_ptr< TebOptimalPlanner > TebOptimalPlannerPtr
Abbrev. for shared instances of the TebOptimalPlanner.
void calculateHSignature(BidirIter path_start, BidirIter path_end, Fun fun_cplx_point, const ObstContainer *obstacles)
Calculate the H-Signature of a path.
Definition: h_signature.h:97
RobotFootprintModelPtr robot_model_
Robot model shared instance.
ObstContainer * obstacles_
Store obstacles that are relevant for planning.
double acc_lim_theta
Maximum angular acceleration of the robot.
Definition: teb_config.h:100
const TebConfig * cfg_
Config class that stores and manages all related parameters.
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 ...
double max_vel_theta
Maximum angular velocity of the robot.
Definition: teb_config.h:97
boost::shared_ptr< EquivalenceClass > EquivalenceClassPtr
void calculateHSignature(BidirIter path_start, BidirIter path_end, Fun fun_cplx_point, const ObstContainer *obstacles, boost::optional< TimeDiffSequence::iterator > timediff_start, boost::optional< TimeDiffSequence::iterator > timediff_end)
Calculate the H-Signature of a path.
Definition: h_signature.h:282
TebOptPlannerContainer tebs_
Container that stores multiple local teb planners (for alternative equivalence classes) and their cor...
This class optimizes an internal Timed Elastic Band trajectory using the g2o-framework.
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...
bool include_dynamic_obstacles
Specify whether the movement of dynamic obstacles should be predicted by a constant velocity model (t...
Definition: teb_config.h:122
struct teb_local_planner::TebConfig::Obstacles obstacles
Obstacle related parameters.
int min_samples
Minimum number of samples (should be always greater than 2)
Definition: teb_config.h:74
double max_vel_x
Maximum translational velocity of the robot.
Definition: teb_config.h:94


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