graph_search.h
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  * Authors: Christoph Rösmann, Franz Albers
37  *********************************************************************/
38 
39 #ifndef GRAPH_SEARCH_INTERFACE_H
40 #define GRAPH_SEARCH_INTERFACE_H
41 
42 #ifdef BOOST_NO_CXX11_DEFAULTED_FUNCTIONS
43  #include <boost/graph/adjacency_list.hpp>
44 #else
45  // Workaround for a bug in boost graph library (concerning directed graphs), boost version 1.48:
46  // boost::add_vertex requires a move constructor/assignment operator in one of the underlying boost objects if C++11 is activated,
47  // but they are missing. The compiler fails due to an implicit deletion. We just deactivate C++11 default functions for now.
48  #define BOOST_NO_CXX11_DEFAULTED_FUNCTIONS
49  #include <boost/graph/adjacency_list.hpp>
50  #undef BOOST_NO_CXX11_DEFAULTED_FUNCTIONS
51 #endif
52 
53 #include <boost/graph/graph_traits.hpp>
54 #include <boost/graph/depth_first_search.hpp>
55 #include <boost/utility.hpp>
56 #include <boost/random.hpp>
57 
58 #include <Eigen/Core>
59 
60 #include <geometry_msgs/Twist.h>
61 
65 
66 namespace teb_local_planner
67 {
68 
69 class HomotopyClassPlanner; // Forward declaration
70 
73 {
74 public:
75  Eigen::Vector2d pos; // position of vertices in the map
76  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
77 };
78 
80 typedef boost::adjacency_list < boost::listS, boost::vecS, boost::directedS, HcGraphVertex, boost::no_property > HcGraph;
82 typedef boost::graph_traits<HcGraph>::vertex_descriptor HcGraphVertexType;
84 typedef boost::graph_traits<HcGraph>::edge_descriptor HcGraphEdgeType;
86 typedef boost::graph_traits<HcGraph>::vertex_iterator HcGraphVertexIterator;
88 typedef boost::graph_traits<HcGraph>::edge_iterator HcGraphEdgeIterator;
90 typedef boost::graph_traits<HcGraph>::adjacency_iterator HcGraphAdjecencyIterator;
91 
93 inline std::complex<long double> getCplxFromHcGraph(HcGraphVertexType vert_descriptor, const HcGraph& graph)
94 {
95  return std::complex<long double>(graph[vert_descriptor].pos.x(), graph[vert_descriptor].pos.y());
96 }
97 
99 inline const Eigen::Vector2d& getVector2dFromHcGraph(HcGraphVertexType vert_descriptor, const HcGraph& graph)
100 {
101  return graph[vert_descriptor].pos;
102 }
103 
108 {
109 public:
110 
111  virtual void createGraph(const PoseSE2& start, const PoseSE2& goal, double dist_to_obst, double obstacle_heading_threshold, const geometry_msgs::Twist* start_velocity) = 0;
112 
116  void clearGraph() {graph_.clear();}
117 
118  // HcGraph graph() const {return graph_;}
119  // Workaround. graph_ is public for now, beacuse otherwise the compilation fails with the same boost bug mentioned above.
120  HcGraph graph_;
121 
122 protected:
126  GraphSearchInterface(const TebConfig& cfg, HomotopyClassPlanner* hcp) : cfg_(&cfg), hcp_(hcp){}
127 
140  void DepthFirst(HcGraph& g, std::vector<HcGraphVertexType>& visited, const HcGraphVertexType& goal, double start_orientation, double goal_orientation, const geometry_msgs::Twist* start_velocity);
141 
142 
143 protected:
144  const TebConfig* cfg_;
146 
147 public:
148  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
149 };
150 
151 
152 
154 {
155 public:
157 
158  virtual ~lrKeyPointGraph(){}
159 
175  virtual void createGraph(const PoseSE2& start, const PoseSE2& goal, double dist_to_obst, double obstacle_heading_threshold, const geometry_msgs::Twist* start_velocity);
176 };
177 
178 
179 
180 
182 {
183 public:
185 
186  virtual ~ProbRoadmapGraph(){}
187 
188 
205  virtual void createGraph(const PoseSE2& start, const PoseSE2& goal, double dist_to_obst, double obstacle_heading_threshold, const geometry_msgs::Twist* start_velocity);
206 
207 private:
208  boost::random::mt19937 rnd_generator_;
209 };
210 } // end namespace
211 
212 #endif // GRAPH_SEARCH_INTERFACE_H
boost::graph_traits< HcGraph >::vertex_descriptor HcGraphVertexType
Abbrev. for vertex type descriptors in the homotopy class search-graph.
Definition: graph_search.h:82
const TebConfig * cfg_
Config class that stores and manages all related parameters.
Definition: graph_search.h:144
Config class for the teb_local_planner and its components.
Definition: teb_config.h:61
HcGraph graph_
Store the graph that is utilized to find alternative homotopy classes.
Definition: graph_search.h:120
std::complex< long double > getCplxFromHcGraph(HcGraphVertexType vert_descriptor, const HcGraph &graph)
Inline function used for initializing the TEB in combination with HCP graph vertex descriptors...
Definition: graph_search.h:93
Local planner that explores alternative homotopy classes, create a plan for each alternative and fina...
boost::graph_traits< HcGraph >::vertex_iterator HcGraphVertexIterator
Abbrev. for the vertices iterator of the homotopy class search-graph.
Definition: graph_search.h:86
void clearGraph()
Clear any existing graph of the homotopy class search.
Definition: graph_search.h:116
Base class for graph based path planning / homotopy class sampling.
Definition: graph_search.h:107
HomotopyClassPlanner *const hcp_
Raw pointer to the HomotopyClassPlanner. The HomotopyClassPlanner itself is guaranteed to outlive the...
Definition: graph_search.h:145
boost::graph_traits< HcGraph >::edge_iterator HcGraphEdgeIterator
Abbrev. for the edges iterator of the homotopy class search-graph.
Definition: graph_search.h:88
GraphSearchInterface(const TebConfig &cfg, HomotopyClassPlanner *hcp)
Protected constructor that should be called by subclasses.
Definition: graph_search.h:126
boost::random::mt19937 rnd_generator_
Random number generator used by createProbRoadmapGraph to sample graph keypoints. ...
Definition: graph_search.h:208
Vertex in the graph that is used to find homotopy classes (only stores 2D positions) ...
Definition: graph_search.h:72
boost::graph_traits< HcGraph >::adjacency_iterator HcGraphAdjecencyIterator
Abbrev. for the adjacency iterator that iterates vertices that are adjecent to the specified one...
Definition: graph_search.h:90
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
boost::graph_traits< HcGraph >::edge_descriptor HcGraphEdgeType
Abbrev. for edge type descriptors in the homotopy class search-graph.
Definition: graph_search.h:84
ProbRoadmapGraph(const TebConfig &cfg, HomotopyClassPlanner *hcp)
Definition: graph_search.h:184
lrKeyPointGraph(const TebConfig &cfg, HomotopyClassPlanner *hcp)
Definition: graph_search.h:156
const Eigen::Vector2d & getVector2dFromHcGraph(HcGraphVertexType vert_descriptor, const HcGraph &graph)
Definition: graph_search.h:99
boost::adjacency_list< boost::listS, boost::vecS, boost::directedS, HcGraphVertex, boost::no_property > HcGraph
Abbrev. for the homotopy class search-graph type.
Definition: graph_search.h:80


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