ADORe
ADORe is a modular open source software library and toolkit for decision making, planning, control and simulation of automated vehicles
tcdset.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  * Stephan Lapoehn - initial implementation and API
13  ********************************************************************************/
14 
15 #pragma once
16 
19 #include <boost/geometry.hpp>
20 #include <boost/geometry/geometries/point.hpp>
21 #include <boost/geometry/geometries/box.hpp>
22 #include <boost/geometry/index/rtree.hpp>
23 #include <unordered_map>
24 #include <list>
25 
26 namespace adore
27 {
28  namespace env
29  {
33  class TCDSet
34  {
35  private:
36  /* R-TREE STUFF */
37  /* implement on higher level! rTree-implementation probably belongs as template into adore_MAD */
38  typedef boost::geometry::model::box<env::BorderBased::Coordinate::boost_point> boost_box;
39 
40  template<typename T1, typename T2>
41  struct itpair
42  {
43  T1 first;
44  T2 second;
46  T1& current(){return first;}
47  T2& end(){return second;}
48  };
49 
50  template<typename value_type,typename Tfirst>
51  struct my_equal
52  {
53  typedef bool result_type;
54  result_type operator() (value_type const& v1, value_type const& v2) const
55  {
56  return boost::geometry::equals<Tfirst,Tfirst>(v1.first, v2.first) && v1.second == v2.second;
57  }
58  };
59 
60  /* boost_point, tcdID */
61  typedef std::pair<env::BorderBased::Coordinate::boost_point,int> idxCoordinate2tcdID;
62 
63  typedef boost::geometry::index::rtree< idxCoordinate2tcdID,
64  boost::geometry::index::quadratic<16>,
65  boost::geometry::index::indexable<idxCoordinate2tcdID>,
68 
69  typedef std::pair<std::unordered_map<int, env::TrafficControlDevice*>::iterator,std::unordered_map<int, env::TrafficControlDevice*>::iterator> TCDIteratorPair;
71 
72  protected:
73  /* get TCD_ID from coordinate */
75 
76  /* get TCD from ID */
77  std::unordered_map<int, env::TrafficControlDevice*> m_id2TCD;
79 
80  /* constants */
81  double m_guard; //min/max value
82 
83  bool m_isOwner;
84 
85  public:
90  {
91  m_guard = 1e99;
92  m_isOwner = true;
93  }
94 
99  virtual ~TCDSet()
100  {
101  clear();
102  }
103 
108  void setIsOwner(bool b)
109  {
110  m_isOwner = b;
111  }
112 
113 
118  bool getIsOwner()
119  {
120  return m_isOwner;
121  }
122 
129  {
130  bool returnValue = true;
131 
132  /* tcd already exists */
133  if(hasTCD(tcd->getID()))
134  {
135  /* @TODO What to do when tcd already exists? As for now: replace the old one */
136  eraseTCD(tcd->getID());
137  returnValue = false;
138  }
139  m_id2TCD.insert(std::make_pair(tcd->getID(),tcd));
141  {
142  m_tcdTlSet.addTcd(tcd);
143  }
144  m_coordinate2tcdIDs.insert(std::make_pair(tcd->getCoordinate().getBoostPoint(),tcd->getID()));
145  return returnValue;
146 
147  }
148 
149  void setMovementId(int tcdId, int movementId)
150  {
151  m_tcdTlSet.setMovementId(tcdId, movementId);
152  }
153 
154  void setJunctionId(int movementId, int junctionId)
155  {
156  m_tcdTlSet.setJunctionId(movementId, junctionId);
157  }
158 
165  bool eraseTCD(int tcdID)
166  {
167  /* TCD does not exist */
168  if(!hasTCD(tcdID))
169  {
170  return false;
171  }
172 
173  env::TrafficControlDevice* tcdToRemove = m_id2TCD[tcdID];
174  m_coordinate2tcdIDs.remove(std::make_pair(tcdToRemove->getCoordinate().getBoostPoint(), tcdToRemove->getID()));
175  m_id2TCD.erase(tcdID);
176  m_tcdTlSet.erase(tcdID);
177  if(getIsOwner()) delete tcdToRemove;
178  return true;
179  }
180 
185  void clear()
186  {
187  if(getIsOwner())
188  {
189  auto iterators = getAllTCDs();
190  for(auto it = iterators.first; it != iterators.second; it++)
191  {
192  delete it->second;
193  }
194  }
195  m_id2TCD.clear();
196  m_coordinate2tcdIDs.clear();
197  m_tcdTlSet.clear();
198  // m_region2TCDs.clear();
199  }
200 
206  {
207  return TCDIteratorPair(m_id2TCD.begin(), m_id2TCD.end());
208  }
209 
214  ItCoordinate2tcdID getTCDsInRegion(double x0, double x1, double y0, double y1)
215  {
216  auto it = m_coordinate2tcdIDs.qbegin(boost::geometry::index::intersects(
217  boost_box(
220  )));
221  return ItCoordinate2tcdID(it,m_coordinate2tcdIDs.qend());
222  }
223 
228  ItCoordinate2tcdID getTCDsOutsideRegion(double x0,double x1, double y0, double y1)
229  {
230  auto it = m_coordinate2tcdIDs.qbegin(boost::geometry::index::disjoint(
231  boost_box(
234  )));
235  return ItCoordinate2tcdID(it,m_coordinate2tcdIDs.qend());
236  }
240  void removeTCDsOutsideRegion(double x0,double x1, double y0, double y1)
241  {
242  std::vector<int> tcdsToRemove;
244  {
245  /* pushing ID of TCD */
246  tcdsToRemove.push_back(itpair.first->second);
247  }
248  for(auto it = tcdsToRemove.begin(); it!=tcdsToRemove.end();it++)
249  {
250  eraseTCD(*it);
251  }
252  }
253 
254 
259  bool hasTCD(int tcdID)
260  {
261  return m_id2TCD.find(tcdID)!=m_id2TCD.end();
262  }
263 
265  {
266  if(hasTCD(tcdID))
267  {
268  return m_id2TCD[tcdID];
269  }
270  std::cout << "TCDSet::getTCD(int tcdID): TrafficControlDevice with ID " << tcdID << " does not exist." << std::endl;
271  return nullptr;
272  }
274  {
275  if(hasTCD(tcdID))
276  {
277  return m_tcdTlSet.at(tcdID);
278  }
279  std::cout << "TCDSet::getTCD(int tcdID): TrafficControlDevice with ID " << tcdID << " does not exist." << std::endl;
280  return TTCDTrafficLightTuple();
281  }
282  };
283  }
284 }
Definition: tcdset.h:34
double m_guard
Definition: tcdset.h:81
bool m_isOwner
Definition: tcdset.h:83
bool insertTCD(env::TrafficControlDevice *tcd)
Definition: tcdset.h:128
void setMovementId(int tcdId, int movementId)
Definition: tcdset.h:149
virtual ~TCDSet()
Definition: tcdset.h:99
std::pair< std::unordered_map< int, env::TrafficControlDevice * >::iterator, std::unordered_map< int, env::TrafficControlDevice * >::iterator > TCDIteratorPair
Definition: tcdset.h:69
Coordinate2tcdIDs_RT m_coordinate2tcdIDs
Definition: tcdset.h:74
bool getIsOwner()
Definition: tcdset.h:118
void clear()
Definition: tcdset.h:185
bool eraseTCD(int tcdID)
Definition: tcdset.h:165
bool hasTCD(int tcdID)
Definition: tcdset.h:259
void setJunctionId(int movementId, int junctionId)
Definition: tcdset.h:154
std::pair< env::BorderBased::Coordinate::boost_point, int > idxCoordinate2tcdID
Definition: tcdset.h:61
void removeTCDsOutsideRegion(double x0, double x1, double y0, double y1)
Definition: tcdset.h:240
boost::geometry::index::rtree< idxCoordinate2tcdID, boost::geometry::index::quadratic< 16 >, boost::geometry::index::indexable< idxCoordinate2tcdID >, my_equal< idxCoordinate2tcdID, env::BorderBased::Coordinate::boost_point > > Coordinate2tcdIDs_RT
Definition: tcdset.h:67
adore::env::TCDTrafficLightSet m_tcdTlSet
Definition: tcdset.h:78
ItCoordinate2tcdID getTCDsInRegion(double x0, double x1, double y0, double y1)
Definition: tcdset.h:214
TCDSet()
Definition: tcdset.h:89
itpair< Coordinate2tcdIDs_RT::const_query_iterator, Coordinate2tcdIDs_RT::const_query_iterator > ItCoordinate2tcdID
Definition: tcdset.h:70
env::TrafficControlDevice * getTCD(int tcdID)
Definition: tcdset.h:264
TCDIteratorPair getAllTCDs()
Definition: tcdset.h:205
std::unordered_map< int, env::TrafficControlDevice * > m_id2TCD
Definition: tcdset.h:77
boost::geometry::model::box< env::BorderBased::Coordinate::boost_point > boost_box
Definition: tcdset.h:38
void setIsOwner(bool b)
Definition: tcdset.h:108
env::TTCDTrafficLightTuple getTCDTrafficLight(int tcdID)
Definition: tcdset.h:273
ItCoordinate2tcdID getTCDsOutsideRegion(double x0, double x1, double y0, double y1)
Definition: tcdset.h:228
Definition: tcdtrafficlightset.h:31
void setJunctionId(int movementId, int junctionId)
Definition: tcdtrafficlightset.h:60
void setMovementId(int tcdId, int movementId)
Definition: tcdtrafficlightset.h:52
void addTcd(TrafficControlDevice *tcd)
Definition: tcdtrafficlightset.h:47
Definition: trafficcontroldevice.h:24
BorderBased::Coordinate getCoordinate() const
Definition: trafficcontroldevice.h:175
int getID() const
Definition: trafficcontroldevice.h:146
TCDType getType() const
Definition: trafficcontroldevice.h:155
@ TRAFFIC_LIGHT
Definition: trafficcontroldevice.h:29
std::tuple< TrafficControlDevice *, int, int > TTCDTrafficLightTuple
Definition: tcdtrafficlightset.h:25
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
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
Definition: tcdset.h:42
T1 first
Definition: tcdset.h:43
T2 second
Definition: tcdset.h:44
T1 & current()
Definition: tcdset.h:46
itpair(T1 first, T2 second)
Definition: tcdset.h:45
T2 & end()
Definition: tcdset.h:47
Definition: tcdset.h:52
bool result_type
Definition: tcdset.h:53
result_type operator()(value_type const &v1, value_type const &v2) const
Definition: tcdset.h:54