ADORe
ADORe is a modular open source software library and toolkit for decision making, planning, control and simulation of automated vehicles
lanepositionedobjectset.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  * Daniel Heß - initial API and implementation
13  ********************************************************************************/
14 
15 
16 #pragma once
20 #include <vector>
21 #include <unordered_map>
22 
23 namespace adore
24 {
25  namespace env
26  {
27  namespace BorderBased
28  {
34  {
35 
36 
37  public:
39  typedef std::unordered_multimap<BorderID,object*, BorderIDHasher> BorderID2Object;
40  typedef BorderID2Object::iterator ObjectIterator;
41  typedef std::pair<ObjectIterator,ObjectIterator> ObjectIteratorPair;
42  private:
44  bool m_isOwner;
46  public:
52  {
53  m_isOwner = true;
54  }
60  {
61  clear();
62  }
68  void setIsOwner(bool isOwner)
69  {
70  m_isOwner = isOwner;
71  }
76  void clear()
77  {
78  if(m_isOwner)
79  {
80  for(auto it = borderId2Object.begin();it!=borderId2Object.end();it++)
81  {
82  delete it->second;
83  }
84  }
85  borderId2Object.clear();
86  }
94  void discard_distant_objects(double x, double y, double distance = 100.0)
95  {
96  for (auto it = getAllObjects().first; it != getAllObjects().second; )
97  {
98  auto checkCoord = Coordinate(x,y,0.0);
99 
100  if(it->first.m_first.distance(checkCoord)>=distance
101  && it->first.m_last.distance(checkCoord) >= distance)
102  //if (!it->first.m_first.isInArea(x-distance,x+distance,y-distance, y+distance))
103  {
104  delete it->second;
105  it=borderId2Object.erase(it);
106  continue;
107  }
108  it++;
109  }
110  }
120  template <typename T>
121  void discard_distant_objects(double x, double y, std::vector<T> &removedObjects, double distance = 100.0)
122  {
123  for (auto it = getAllObjects().first; it != getAllObjects().second; )
124  {
125  auto checkCoord = Coordinate(x,y,0.0);
126 
127  if(it->first.m_first.distance(checkCoord)>=distance
128  && it->first.m_last.distance(checkCoord) >= distance)
129  //if (!it->first.m_first.isInArea(x-distance,x+distance,y-distance, y+distance))
130  {
131  removedObjects.push_back(*(dynamic_cast<T*>((*it).second)));
132 
133  delete it->second;
134  it=borderId2Object.erase(it);
135  continue;
136  }
137  it++;
138  }
139  }
147  void insert_object(object* obj,bool remove_duplicates=false,double precision=0.5)
148  {
149  if(remove_duplicates)
150  erase_object(obj->getLanePosition(),precision);
151 
152  borderId2Object.insert(std::make_pair(obj->getLanePosition().m_rightID,obj));
153  }
160  void erase_object(const LanePosition& position,double precision=0.5)
161  {
162  for(auto it = borderId2Object.equal_range(position.m_rightID);it.first!=it.second;)
163  {
164  if((std::abs)(it.first->second->getLanePosition().m_progress-position.m_progress)<precision)
165  {
166  if(m_isOwner)delete it.first->second;
167  it.first = borderId2Object.erase(it.first);
168  continue;
169  }
170  it.first++;
171  }
172  }
178  void eraseObjectsBorderBased(const std::vector<Border*> borderSet)
179  {
180  for(auto it = borderSet.begin();it!=borderSet.end();it++)//borderSet
181  {
182  for(auto it2 = borderId2Object.equal_range((*it)->m_id);it2.first!=it2.second;)//borderid2object
183  {
184  if(m_isOwner)delete it2.first->second;
185  it2.first = borderId2Object.erase(it2.first);
186  }
187  }
188  }
194  void eraseObjectsBorderBased(const std::vector<BorderID> borderIDSet)
195  {
196  for(auto it = borderIDSet.begin();it!=borderIDSet.end();it++)//borderIDVector
197  {
198  for(auto it2 = borderId2Object.equal_range(*it);it2.first!=it2.second;)//borderid2object
199  {
200  if(m_isOwner)delete it2.first->second;
201  it2.first = borderId2Object.erase(it2.first);
202  }
203  }
204  }
212  bool hasObjects(const BorderID& borderID)
213  {
214  if(borderId2Object.size() == 0)
215  return false;
216 
217  return borderId2Object.count(borderID)>0;
218  }
225  {
226  return ObjectIteratorPair(borderId2Object.begin(),borderId2Object.end());
227  }
235  {
236  return borderId2Object.equal_range(borderID);
237  }
245  std::vector<object*> getObjects(const LanePosition& position,double precision=0.5)
246  {
247  std::vector<object*> result;
248  for( auto itpair=getObjects(position.m_rightID);itpair.first!=itpair.second;itpair.first++ )
249  {
250  if((std::abs)(itpair.first->second->getLanePosition().m_progress-position.m_progress)<precision)
251  {
252  result.push_back(itpair.first->second);
253  }
254  }
255  return result;
256  }
265  bool hasObjects(const LanePosition& position,double precision=0.5)
266  {
267  for( auto itpair=getObjects(position.m_rightID);itpair.first!=itpair.second;itpair.first++ )
268  {
269  if((std::abs)(itpair.first->second->getLanePosition().m_progress-position.m_progress)<precision)
270  {
271  return true;
272  }
273  }
274  return false;
275  }
284  {
285  for( auto it = borderId2Object.begin();it!=borderId2Object.end(); )
286  {
287  auto bid = it->first;
288  if(!borderSet->hasBorder(bid))
289  {
290  if(bid.m_first.distance(center)>radius
291  && bid.m_last.distance(center)>radius)
292  {
293  if(m_isOwner)delete it->second;
294  it = borderId2Object.erase(it);
295  continue;
296  }
297  }
298  it++;
299  }
300  }
301  };
302 
303  }
304  }
305 }
Abstract class for Objects that are positioned by a connection to a certain lane.
Definition: alanepositionedobject.h:28
virtual const LanePosition & getLanePosition()=0
Get the LanePosition of the Object.
efficiently store borders in boost R-tree
Definition: borderset.h:99
bool hasBorder(const BorderID &id) const
check whether a border exists in the set
Definition: borderset.h:966
This class represents a set of objects that are positioned by LanePosition.
Definition: lanepositionedobjectset.h:34
void discard_distant_objects(double x, double y, double distance=100.0)
Discard distant objects.
Definition: lanepositionedobjectset.h:94
BorderID2Object borderId2Object
Definition: lanepositionedobjectset.h:43
void discard_distant_objects(double x, double y, std::vector< T > &removedObjects, double distance=100.0)
Discard distant objects.
Definition: lanepositionedobjectset.h:121
ObjectIteratorPair getAllObjects()
Get the begin()- and end()-iterator for the whole set.
Definition: lanepositionedobjectset.h:224
void eraseObjectsBorderBased(const std::vector< Border * > borderSet)
Erase Objects that are positioned on certain borders.
Definition: lanepositionedobjectset.h:178
void insert_object(object *obj, bool remove_duplicates=false, double precision=0.5)
Insert a new object.
Definition: lanepositionedobjectset.h:147
bool m_isOwner
Definition: lanepositionedobjectset.h:44
virtual ~LanePositionedObjectSet()
Destroy the LanePositionedObjectSet object.
Definition: lanepositionedobjectset.h:59
void eraseObjectsBorderBased(const std::vector< BorderID > borderIDSet)
Erase Objects that are positioned on certain borders given by their BorderIDs.
Definition: lanepositionedobjectset.h:194
BorderID2Object::iterator ObjectIterator
Definition: lanepositionedobjectset.h:40
ObjectIteratorPair getObjects(const BorderID &borderID)
Get the begin()- and end()-iterator for objects that are positioned on a certain border.
Definition: lanepositionedobjectset.h:234
std::unordered_multimap< BorderID, object *, BorderIDHasher > BorderID2Object
Definition: lanepositionedobjectset.h:39
LanePositionedObjectSet()
Construct a new LanePositionedObjectSet object.
Definition: lanepositionedobjectset.h:51
void erase_objectsWithUnknownBordersOutsideRadius(BorderSet *borderSet, Coordinate center, double radius)
Erase objects that are positioned that are outside a specified circle.
Definition: lanepositionedobjectset.h:283
bool hasObjects(const BorderID &borderID)
Check whether at least one object is contained that is positioned on a certain Border.
Definition: lanepositionedobjectset.h:212
bool hasObjects(const LanePosition &position, double precision=0.5)
Check whether the set holds at least one object on a certain LanePosition.
Definition: lanepositionedobjectset.h:265
void setIsOwner(bool isOwner)
Set the owner flag.
Definition: lanepositionedobjectset.h:68
std::pair< ObjectIterator, ObjectIterator > ObjectIteratorPair
Definition: lanepositionedobjectset.h:41
std::vector< object * > getObjects(const LanePosition &position, double precision=0.5)
Get the objects that are positioned on a certain LanePosition.
Definition: lanepositionedobjectset.h:245
void clear()
Clear the LanePositionedObjectSet.
Definition: lanepositionedobjectset.h:76
void erase_object(const LanePosition &position, double precision=0.5)
Erase objects on a certain LanePosition.
Definition: lanepositionedobjectset.h:160
ALanePositionedObject object
Definition: lanepositionedobjectset.h:38
x
Definition: adore_set_goal.py:30
y
Definition: adore_set_goal.py:31
Definition: areaofeffectconverter.h:20
This struct identifies a Border by the coordinates of the starting and the end point.
Definition: borderid.h:31
This struct represents 3-dimensional coordines.
Definition: coordinate.h:34
This is a struct that contains a position defined by a BorderID and a progress on that border.
Definition: laneposition.h:30
double m_progress
Definition: laneposition.h:32
BorderID m_rightID
Definition: laneposition.h:31
pair of iterators to iterate from first iterator till second iterator is reached
Definition: borderset.h:46
T1 first
Definition: borderset.h:47
T2 second
Definition: borderset.h:48