segment_expander.h
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2017, <copyright holder> <email>
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  * * Redistributions of source code must retain the above copyright
8  * notice, this list of conditions and the following disclaimer.
9  * * Redistributions in binary form must reproduce the above copyright
10  * notice, this list of conditions and the following disclaimer in the
11  * documentation and/or other materials provided with the distribution.
12  * * Neither the name of the <organization> nor the
13  * names of its contributors may be used to endorse or promote products
14  * derived from this software without specific prior written permission.
15  *
16  * THIS SOFTWARE IS PROVIDED BY <copyright holder> <email> ''AS IS'' AND ANY
17  * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18  * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19  * DISCLAIMED. IN NO EVENT SHALL <copyright holder> <email> BE LIABLE FOR ANY
20  * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21  * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23  * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26  *
27  */
28 
29 #ifndef SEGMENT_EXPANDER_H
30 #define SEGMENT_EXPANDER_H
31 
32 #define POT_HIGH 1.0e10
39 #include <algorithm>
40 #include <memory>
41 #include <queue>
42 
43 namespace multi_robot_router
44 {
46 {
47  public:
49  {
50  none,
52  avoidance
53  };
68  bool calculatePotentials(const RouteCoordinatorWrapper *_p, Vertex &_start, Vertex &_end, const uint32_t _maxIterations, const uint32_t _radius);
72  const std::vector<uint32_t> &getRobotCollisions() const;
76  void reset();
80  void setSpeed(const float &_speed);
89 
90  private:
91  template <class T, class S, class C>
92  void clearpq(std::priority_queue<T, S, C> &q)
93  {
94  q = std::priority_queue<T, S, C>();
95  }
96 
98  {
99  bool operator()(const Vertex *_a, const Vertex *_b) const
100  {
101  return _a->weight > _b->weight;
102  }
103  };
104 
105  //assigns the potential to all necessary vertices
106  Vertex *expandVoronoi(Vertex &_start, Vertex &_end, const uint32_t _cycles);
107  //assigns the next Vertex with the given potential
108  void addExpansoionCandidate(Vertex &_current, Vertex &_next, Vertex &_end);
109  //adds teh first expenstion candidate
110  void addStartExpansionCandidate(Vertex &_start, Vertex &_current, Vertex &_next, Vertex &_end);
111  //Resolves start issues when start and goal point is on the same segment
112  void resolveStartCollision(Vertex &_start, Vertex &_end);
113  //checks if a list contains a specific vertex (seg id)
114  bool containsVertex(const Vertex &_v, const std::vector<std::reference_wrapper<Vertex>> &_list) const;
115 
116  std::priority_queue<Vertex *, std::vector<Vertex *>, greaterSegmentWrapper> seg_queue_;
117  uint32_t neutral_cost_ = 1;
118  uint32_t diameter_;
119 
126 
128  std::vector<uint32_t> collisions_robots_;
129  std::vector<std::unique_ptr<Vertex>> startSegments_;
131 };
132 } // namespace multi_robot_router
133 #endif
void addExpansoionCandidate(Vertex &_current, Vertex &_next, Vertex &_end)
std::vector< std::unique_ptr< Vertex > > startSegments_
CollisionResolution * collision_resolution_
Vertex * expandVoronoi(Vertex &_start, Vertex &_end, const uint32_t _cycles)
const std::vector< uint32_t > & getRobotCollisions() const
returns the found robotCollisions while planning
std::vector< uint32_t > collisions_robots_
std::priority_queue< Vertex *, std::vector< Vertex * >, greaterSegmentWrapper > seg_queue_
void setCollisionResolver(const CollisionResolverType cRes)
sets the desired collision resolver
bool containsVertex(const Vertex &_v, const std::vector< std::reference_wrapper< Vertex >> &_list) const
CollisionResolverType getCollisionResolver() const
gets the currently used collision resolver
void resolveStartCollision(Vertex &_start, Vertex &_end)
void setSpeed(const float &_speed)
Sets the multiplier to reduce a robots speed (pCalc...)
void clearpq(std::priority_queue< T, S, C > &q)
void addStartExpansionCandidate(Vertex &_start, Vertex &_current, Vertex &_next, Vertex &_end)
SegmentExpander(const CollisionResolverType _cRes)
constructor
bool operator()(const Vertex *_a, const Vertex *_b) const
bool calculatePotentials(const RouteCoordinatorWrapper *_p, Vertex &_start, Vertex &_end, const uint32_t _maxIterations, const uint32_t _radius)
assigns all Vertices in the Search graph with a potential according to the distance to the start ...
const RouteCoordinatorWrapper * route_querry_


tuw_multi_robot_router
Author(s): Benjamin Binder
autogenerated on Mon Jun 10 2019 15:42:49