ADORe
ADORe is a modular open source software library and toolkit for decision making, planning, control and simulation of automated vehicles
parkingspotset.h
Go to the documentation of this file.
1 /********************************************************************************
2  * Copyright (C) 2017-2020 German Aerospace Center (DLR).
3  * Eclipse ADORe, Automated Driving Open Research https://eclipse.org/adore
4  *
5  * This program and the accompanying materials are made available under the
6  * terms of the Eclipse Public License 2.0 which is available at
7  * http://www.eclipse.org/legal/epl-2.0.
8  *
9  * SPDX-License-Identifier: EPL-2.0
10  *
11  * Contributors:
12  * Thomas Lobig - initial API and implementation
13  ********************************************************************************/
14 
15 
16 #pragma once
17 
20 #include <boost/geometry.hpp>
21 #include <boost/geometry/geometries/point.hpp>
22 #include <boost/geometry/geometries/box.hpp>
23 #include <boost/geometry/index/rtree.hpp>
24 #include <unordered_map>
25 #include <list>
26 #include <vector>
27 
28 namespace adore
29 {
30  namespace env
31  {
32  namespace BorderBased
33  {
34 
39  {
40  private:
41  /* R-TREE STUFF */
42  /* box to search in for Parking Spots */
43 
44  template<typename T1, typename T2>
45  struct itpair
46  {
47  T1 first;
48  T2 second;
50  T1& current(){return first;}
51  T2& end(){return second;}
52  };
53 
54  /* equal for parking spots -> boost points are equal, heading doesn't matter */
55  template<typename value_type,typename Tfirst>
56  struct my_equal
57  {
58  typedef bool result_type;
59  result_type operator() (value_type const& v1, value_type const& v2) const
60  {
61  return boost::geometry::equals<Tfirst,Tfirst>(v1.first, v2.first);
62  }
63  };
64 
65  /* boost_point + heading = parking spot */
66  typedef std::pair<adore::env::BorderBased::Coordinate::boost_point,double> idxCoordinate2heading;
67 
68  typedef boost::geometry::index::rtree< idxCoordinate2heading,
69  boost::geometry::index::quadratic<16>,
70  boost::geometry::index::indexable<idxCoordinate2heading>,
73 
75 
76  protected:
77  /* get heading from coordinate, heading + coordinate = parking spot definition */
79 
80  /* constants */
81  double m_guard; //min/max value
82 
83  /* protected functions */
85  {
86  auto it = m_coordinate2heading.qbegin(boost::geometry::index::nearest(bp,1));
87  if(it!=m_coordinate2heading.qend() && Coordinate(bp)==Coordinate(it->first)) // boost::geometry::equals(bp,it->first)) -> this doesn't seem to be reliable enough, a lot of false negatives
88  {
89  return true;
90  }
91  return false;
92  }
99  {
100  if(hasParkingSpot(bp))
101  {
102  auto it = m_coordinate2heading.qbegin(boost::geometry::index::nearest(bp,1));
103  if(it!= m_coordinate2heading.qend() && boost::geometry::equals(bp,it->first))
104  {
105  m_coordinate2heading.remove(*it);
106  return true;
107  }
108  }
109  return false;
110  }
111 
112  public:
117  {
118  m_guard = 1e99;
119  }
120 
125  virtual ~ParkingSpotSet()
126  {
127  clear();
128  }
129 
130  /*
131  * insert Parking Spot
132  * as for now, if Parking Spot already exists, replace it
133  * returns false if Parking Spot was replaced
134  */
135  bool insertParkingSpot(ParkingSpot parkingSpot)
136  {
137  bool returnValue = true;
138 
139  /* Parking Spot already exists */
140  if(hasParkingSpot(parkingSpot))
141  {
142  /* @TODO What to do when Parking Spot already exists? As for now: replace the old one */
143  eraseParkingSpot(parkingSpot);
144  returnValue = false;
145  }
146  m_coordinate2heading.insert(std::make_pair(parkingSpot.getCoordinate().getBoostPoint(),parkingSpot.getHeading()));
147  return returnValue;
148 
149  }
150 
157  bool eraseParkingSpot(ParkingSpot parkingSpot)
158  {
159  return eraseParkingSpot(parkingSpot.getCoordinate().getBoostPoint());
160  }
161 
163  {
164  return eraseParkingSpot(coordinate.getBoostPoint());
165  }
166 
167 
172  void clear()
173  {
174  m_coordinate2heading.clear();
175  }
176 
182  {
183  auto first = m_coordinate2heading.qbegin(boost::geometry::index::intersects(m_coordinate2heading.bounds()));
184  auto last = m_coordinate2heading.qend();
185  return ItPairCoordinate2heading(first, last);
186  }
187 
192  ItPairCoordinate2heading getParkingSpotsInRegion(double x0, double x1, double y0, double y1)
193  {
194  auto it = m_coordinate2heading.qbegin(boost::geometry::index::intersects(
195  boost::geometry::model::box<env::BorderBased::Coordinate::boost_point>(
198  )));
200  }
201 
207  {
208  auto it = m_coordinate2heading.qbegin(boost::geometry::index::disjoint(
209  boost::geometry::model::box<env::BorderBased::Coordinate::boost_point>(
212  )));
214  }
218  void removeParkingSpotsOutsideRegion(double x0,double x1, double y0, double y1)
219  {
220  std::vector<adore::env::BorderBased::Coordinate::boost_point> parkingSpotsToRemove;
222  {
223  parkingSpotsToRemove.push_back(itpair.first->first);
224  }
225  for(auto it = parkingSpotsToRemove.begin(); it!=parkingSpotsToRemove.end();it++)
226  {
227  eraseParkingSpot(*it);
228  }
229  }
230 
231 
236  bool hasParkingSpot(ParkingSpot parkingSpot)
237  {
238  return hasParkingSpot(parkingSpot.getCoordinate().getBoostPoint());
239  }
240 
245  std::vector<ParkingSpot> itPair2ParkingSpotVector(ItPairCoordinate2heading itPair)
246  {
247  std::vector<ParkingSpot> returnValue;
248  for(auto it = itPair.current(); it!=itPair.end(); it++)
249  {
250  returnValue.push_back(ParkingSpot(BorderBased::Coordinate(it->first.get<0>(),it->first.get<1>(),it->first.get<2>()),it->second));
251  }
252  return returnValue;
253  }
254  };
255  }
256  }
257 }
Definition: parkingspotset.h:39
void clear()
Definition: parkingspotset.h:172
double m_guard
Definition: parkingspotset.h:81
void removeParkingSpotsOutsideRegion(double x0, double x1, double y0, double y1)
Definition: parkingspotset.h:218
boost::geometry::index::rtree< idxCoordinate2heading, boost::geometry::index::quadratic< 16 >, boost::geometry::index::indexable< idxCoordinate2heading >, my_equal< idxCoordinate2heading, env::BorderBased::Coordinate::boost_point > > Coordinate2heading_RT
Definition: parkingspotset.h:72
itpair< Coordinate2heading_RT::const_query_iterator, Coordinate2heading_RT::const_query_iterator > ItPairCoordinate2heading
Definition: parkingspotset.h:74
bool insertParkingSpot(ParkingSpot parkingSpot)
Definition: parkingspotset.h:135
ItPairCoordinate2heading getParkingSpotsOutsideRegion(double x0, double x1, double y0, double y1)
Definition: parkingspotset.h:206
bool eraseParkingSpot(ParkingSpot parkingSpot)
Definition: parkingspotset.h:157
Coordinate2heading_RT m_coordinate2heading
Definition: parkingspotset.h:78
ParkingSpotSet()
Definition: parkingspotset.h:116
bool eraseParkingSpot(adore::env::BorderBased::Coordinate::boost_point bp)
Definition: parkingspotset.h:98
std::vector< ParkingSpot > itPair2ParkingSpotVector(ItPairCoordinate2heading itPair)
Definition: parkingspotset.h:245
ItPairCoordinate2heading getParkingSpotsInRegion(double x0, double x1, double y0, double y1)
Definition: parkingspotset.h:192
bool eraseParkingSpot(adore::env::BorderBased::Coordinate coordinate)
Definition: parkingspotset.h:162
virtual ~ParkingSpotSet()
Definition: parkingspotset.h:125
bool hasParkingSpot(adore::env::BorderBased::Coordinate::boost_point bp)
Definition: parkingspotset.h:84
std::pair< adore::env::BorderBased::Coordinate::boost_point, double > idxCoordinate2heading
Definition: parkingspotset.h:66
ItPairCoordinate2heading getAllParkingSpots()
Definition: parkingspotset.h:181
bool hasParkingSpot(ParkingSpot parkingSpot)
Definition: parkingspotset.h:236
This class definition is to represent parking spots.
Definition: parkingspot.h:31
double getHeading() const
Get the heading of the parking spot.
Definition: parkingspot.h:61
BorderBased::Coordinate getCoordinate() const
Get the Coordinate object.
Definition: parkingspot.h:70
x0
Definition: adore_set_goal.py:25
y0
Definition: adore_set_goal.py:26
y1
Definition: adore_set_pose.py:29
x1
Definition: adore_set_pose.py:28
Definition: areaofeffectconverter.h:20
This struct represents 3-dimensional coordines.
Definition: coordinate.h:34
boost::geometry::model::point< double, 3, boost::geometry::cs::cartesian > boost_point
Definition: coordinate.h:36
boost_point getBoostPoint()
Get a boost_point that has the same coordinates as the Coordinate object.
Definition: coordinate.h:202
T1 first
Definition: parkingspotset.h:47
T2 & end()
Definition: parkingspotset.h:51
T2 second
Definition: parkingspotset.h:48
T1 & current()
Definition: parkingspotset.h:50
itpair(T1 first, T2 second)
Definition: parkingspotset.h:49
result_type operator()(value_type const &v1, value_type const &v2) const
Definition: parkingspotset.h:59
bool result_type
Definition: parkingspotset.h:58