ADORe
ADORe is a modular open source software library and toolkit for decision making, planning, control and simulation of automated vehicles
borderaccumulator.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  * Robert Markowski - BASFollowNavigation
14  ********************************************************************************/
15 
16 #pragma once
17 
22 #include <adore/mad/csvlog.h>
23 #include <unordered_map>
24 #include <list>
25 #include <vector>
26 #include <set>
27 
28 namespace adore
29 {
30  namespace env
31  {
32  namespace BorderBased
33  {
34 
35  typedef std::vector<Border*> BAContainer;
36  //typedef std::unordered_map<adore::env::BorderBased::BorderID, double, adore::env::BorderBased::BorderIDHasher> TBorderID2Cost;
37  //typedef std::list<Border*> BAContainer;
38 
44  {
45  protected:
48  public:
50  {
51  }
58  virtual void getNextBorder(Border*& border,bool& inverted)=0;
67  {
68  return m_borderSet->borderTypeValid(b);
69  }
70  };
71 
72 
73 
78  {
79  private:
81  double m_distance;
83  public:
91  BASFollowStraight(Border* border,BorderSet* borderSet,double max_distance)
92  {
93  m_current = border;
94  m_borderSet = borderSet;
95  m_distance = max_distance;
96  }
97  virtual void getNextBorder(Border*& border,bool& inverted) override
98  {
99  if(m_current!=0 && m_distance>0)
100  {
101  double continuity = 1.0;
102  Border* successor = 0;
103  border = m_current;
104 
105  for(auto it = m_borderSet->getSuccessors(m_current);it.current()!=it.end();it.current()++)
106  {
107  Border* next = it.current()->second;
108  if(!borderValid(next)) {continue;}
109  Border* nextLeft = m_borderSet->getLeftNeighbor(next);
110  if(nextLeft!=0 && m_current->m_left!=0
111  && m_borderSet->isConnected(*(m_current->m_left),nextLeft->m_id)
112  && !(m_current->m_id==next->m_id))
113  {
114  double r = m_current->getContinuityRating(next);
115  if(successor==0 || r<continuity)
116  {
117  successor = next;
118  continuity = r;
119  }
120  }
121  }
123  m_current = successor;
124  inverted = false;
125  }
126  else
127  {
128  border = 0;
129  }
130  }
131  };
136  {
137  private:
139  double m_distance;
142  public:
151  BASFollowNavigation(Border* border, BorderSet* borderSet, BorderCostMap* borderID2Cost, double max_distance)
152  {
153  //:m_current(border),m_borderSet(borderSet),m_borderID2Cost(borderID2Cost),m_distance(max_distance){}
154  m_current = border;
155  m_borderSet = borderSet;
156  m_borderID2Cost = borderID2Cost;
157  m_distance = max_distance;
159  }
166  {
168  }
169  virtual void getNextBorder(Border*& border, bool& inverted) override
170  {
171  if(m_current!=0 && m_distance>0)
172  {
173  Border* successor = 0;
174  border = m_current;
175 
176  bool previousCostSet = false;
177  double previousCost = 0.0; // initialized to silence -Wmaybe-uninitialized warning
178  auto previousCostResult = m_borderID2Cost->find(m_current->m_id);
179  if( previousCostResult!=m_borderID2Cost->end() )
180  {
181  previousCost = previousCostResult->second.getCombinedCost();
182  previousCostSet = true;
183  }
184 
185  double minCost = 0.0; // initialized to silence -Wmaybe-uninitialized warning
186 
187  for(auto it = m_borderSet->getSuccessors(m_current);it.current()!=it.end();it.current()++)
188  {
189  // check if corresponding borders are properly connected
190  Border* next = it.current()->second;
191  if(!borderValid(next)) {continue;}
192  Border* nextLeft = m_borderSet->getLeftNeighbor(next);
193  if(nextLeft!=0 && m_current->m_left!=0
194  && m_borderSet->isConnected(*(m_current->m_left),nextLeft->m_id)
195  && !(m_current->m_id==next->m_id))
196  {
197  auto nextCostResult = m_borderID2Cost->find(next->m_id);
198  if( nextCostResult==m_borderID2Cost->end() )continue;
199  double nextCost = nextCostResult->second.getCombinedCost();
200 
201  if( (previousCostSet && nextCost>=previousCost) || (successor!=0 && nextCost>=minCost) )continue;
202 
203  minCost = nextCost;
204  successor = next;
205  }
206  }
207 
208  if( successor == 0 && m_continueOnIncreasingCost)
209  {
210  BASFollowStraight m_basFallback(m_current,m_borderSet,100000.0);
211  m_basFallback.getNextBorder(successor,inverted);
212  m_basFallback.getNextBorder(successor,inverted);
213  }
214 
216  m_current = successor;
217  inverted = false;
218  }
219  else
220  {
221  border = 0;
222  }
223  }
224  };
231  {
232  private:
233  BAContainer::iterator m_current;
235  public:
241  {
243  };
244  private:
246  public:
254  BASNeighbor(BAContainer* list,BorderSet* borderSet,Direction direction)
255  {
256  m_list = list;
257  m_current = list->begin();
258  m_borderSet =borderSet;
259  m_direction = direction;
260  }
261  virtual void getNextBorder(Border*& border,bool& inverted) override
262  {
263  if(m_current!=m_list->end())
264  {
265  if(m_direction==LEFT)
266  {
268  inverted = (*m_current)->getNeighborDirection()==Border::OPPOSITE_DIRECTION;
269  }
270  else
271  {
273  inverted = (*m_current)->getNeighborDirection()==Border::OPPOSITE_DIRECTION;
274  }
275  if(border==0)
276  {
277  border = *m_current;
278  inverted = false;
279  }
280  m_current ++;
281  }
282  else
283  {
284  border = 0;
285  }
286  }
287  };
288 
293  {
294  private:
296  std::vector<bool> m_list_inverted;
297  double m_distance;
299  public:
301 
306 
312  void clear()
313  {
314  m_distance = 0.0;
315  m_point_count = 0;
316  m_list.clear();
317  m_list_inverted.clear();
318  }
319 
326  {
327  append(bas);
328  }
329  // puts a sequence of borders into the internal list, according to BorderAccumulationStrategy, and returns length of the combined borders
331  // * append - accumulate the data from a border trace
332  // */
333  //void append(BorderTrace* borderTrace, BorderSet* borderSet)
334  //{
335  // for(auto it = borderTrace->rbegin();
336  // it!=borderTrace->rend();
337  // it++)
338  // {
339  // Border* current = borderSet->getBorder(it->first);
340  // m_distance += it->second;
341  // m_point_count += current->m_path->getData().nc();
342  // m_list.push_back(current);
343  // m_list_inverted.push_back(false);
344  // }
345  //}
346 
353  void append(Border* current,bool inverted)
354  {
355  m_distance += current->getLength();
356  m_point_count += current->m_path->getData().nc();
357  m_list.push_back(current);
358  m_list_inverted.push_back(inverted);
359  }
360 
367  {
368  bool inverted;//field is filled by getNextBorder
369  Border* current;
370  for( bas->getNextBorder(current,inverted);
371  current!=0;
372  bas->getNextBorder(current,inverted) )
373  {
374  m_distance += current->getLength();
375  m_point_count += current->m_path->getData().nc();
376  m_list.push_back(current);
377  m_list_inverted.push_back(inverted);
378  }
379  }
380 
387  {
388  return &m_list;
389  }
396  {
397  adoreMatrix<double,4,0> data;
398  data.set_size(4,m_point_count);
399  int count = 0;
400  auto it2 = m_list_inverted.begin();
401  for( auto it = m_list.begin(); it!= m_list.end(); it++,it2++ )
402  {
403  bool inverted = *it2;
404  int new_count = (*it)->m_path->getData().nc();
405  const adoreMatrix<double>& new_data = (*it)->m_path->getData();
406  for(int i=0;i<new_count;i++)
407  {
408  int idx_new = i;
409  int idx_combined = i + count;
410  if( inverted )
411  {
412  idx_new = new_count - i - 1;
413  }
414  for(int d=1;d<=3;d++)
415  {
416  data(d,idx_combined) = new_data(d,idx_new);
417  }
418  }
419  count += new_count;
420  }
421  data(0,0)=0;
422  for(int i=1;i<m_point_count;i++)
423  {
424  data(0,i) = data(0,i-1) + std::sqrt( (data(1,i)-data(1,i-1))*(data(1,i)-data(1,i-1))
425  +(data(2,i)-data(2,i-1))*(data(2,i)-data(2,i-1))
426  +(data(3,i)-data(3,i-1))*(data(3,i)-data(3,i-1)) );
427  }
428  f = function_type(data);
429  }
430  };
431  }
432  }
433 }
This class chooses the successor with the lowest cost until an upper limit on distance is reached.
Definition: borderaccumulator.h:136
BorderCostMap * m_borderID2Cost
Definition: borderaccumulator.h:140
Border * m_current
Definition: borderaccumulator.h:138
virtual void getNextBorder(Border *&border, bool &inverted) override
Get the next border.
Definition: borderaccumulator.h:169
void setContinueOnIncreasingCost(bool value)
set continuation on increasing cost
Definition: borderaccumulator.h:165
double m_distance
Definition: borderaccumulator.h:139
BASFollowNavigation(Border *border, BorderSet *borderSet, BorderCostMap *borderID2Cost, double max_distance)
Construct a new BASFollowNavigation object.
Definition: borderaccumulator.h:151
bool m_continueOnIncreasingCost
Definition: borderaccumulator.h:141
This class chooses the straightest successor of a border until an upper limit on distance is reached.
Definition: borderaccumulator.h:78
double m_distance
Definition: borderaccumulator.h:81
BASFollowStraight(Border *border, BorderSet *borderSet, double max_distance)
Construct a new BASFollowStraight object.
Definition: borderaccumulator.h:91
virtual void getNextBorder(Border *&border, bool &inverted) override
Get the next border.
Definition: borderaccumulator.h:97
Border * m_current
Definition: borderaccumulator.h:80
This class choses the left/right neighbors of a border sequence.
Definition: borderaccumulator.h:231
virtual void getNextBorder(Border *&border, bool &inverted) override
Get the next border.
Definition: borderaccumulator.h:261
BAContainer * m_list
Definition: borderaccumulator.h:234
Direction m_direction
Definition: borderaccumulator.h:245
BAContainer::iterator m_current
Definition: borderaccumulator.h:233
BASNeighbor(BAContainer *list, BorderSet *borderSet, Direction direction)
Construct a new BASNeighbor object.
Definition: borderaccumulator.h:254
Direction
Direction of BorderAccumulator.
Definition: borderaccumulator.h:241
@ LEFT
Definition: borderaccumulator.h:242
@ RIGHT
Definition: borderaccumulator.h:242
This class defines how successors of a border should be chosen.
Definition: borderaccumulator.h:44
BorderSet * m_borderSet
Definition: borderaccumulator.h:46
BorderAccumulationStrategy()
Definition: borderaccumulator.h:49
virtual void getNextBorder(Border *&border, bool &inverted)=0
Get the next border.
bool borderValid(Border *b)
Check whether a border is valid.
Definition: borderaccumulator.h:66
This class collects a sequence of borders, according to chosen BorderAccumulationStrategy.
Definition: borderaccumulator.h:293
BAContainer m_list
Definition: borderaccumulator.h:295
void append(Border *current, bool inverted)
‍**
Definition: borderaccumulator.h:353
double m_distance
Definition: borderaccumulator.h:297
int m_point_count
Definition: borderaccumulator.h:298
void clear()
Clear the BorderAccumulator.
Definition: borderaccumulator.h:312
adore::mad::LLinearPiecewiseFunctionM< double, 3 > function_type
Definition: borderaccumulator.h:300
BorderAccumulator(BorderAccumulationStrategy *bas)
Construct a new Border Accumulator object and initialize it with a BorderAccumulationStrategy.
Definition: borderaccumulator.h:325
BorderAccumulator()
Construct a new BorderAccumulator object and do an empty initialization.
Definition: borderaccumulator.h:305
void append(BorderAccumulationStrategy *bas)
Accumulate the data from the BorderAccumulationStrategy.
Definition: borderaccumulator.h:366
std::vector< bool > m_list_inverted
Definition: borderaccumulator.h:296
BAContainer * getBorders()
Get the Accumulated Borders.
Definition: borderaccumulator.h:386
void defineFunction(function_type &f)
Create (with new) a function, which contains all accumulated border paths.
Definition: borderaccumulator.h:395
Definition: bordercostmap.h:31
efficiently store borders in boost R-tree
Definition: borderset.h:99
Border * getRightNeighbor(Border *b)
get the right neighbor of a border
Definition: borderset.h:1279
itCoordinate2Border getSuccessors(Border *b)
get an interator pair for all borders which follow after b
Definition: borderset.h:996
Border * getLeftNeighbor(Border *b)
Get left neighbor of a border.
Definition: borderset.h:1252
bool isConnected(const BorderID &a, const BorderID &b)
check whether end of border a is beginning of border b
Definition: borderset.h:1381
bool borderTypeValid(Border *b)
check whether border type is in allowed types of set
Definition: borderset.h:204
adoreMatrix< T, n+1, 0 > & getData()
Definition: llinearpiecewisefunction.h:147
std::vector< Border * > BAContainer
Definition: borderaccumulator.h:35
r
Definition: adore_suppress_lanechanges.py:209
Definition: areaofeffectconverter.h:20
The border struct contains data of the smallest.
Definition: border.h:62
@ OPPOSITE_DIRECTION
Definition: border.h:507
Tborderpath * m_path
Definition: border.h:70
Direction getNeighborDirection()
Get the direction of the left neighbor.
Definition: border.h:517
double getLength()
Get the length of the border.
Definition: border.h:703
BorderID m_id
Definition: border.h:68
double getContinuityRating(Border *next)
Get the continuity rating.
Definition: border.h:803
BorderID * m_left
Definition: border.h:69
T1 & current()
Definition: borderset.h:50