ADORe
ADORe is a modular open source software library and toolkit for decision making, planning, control and simulation of automated vehicles
lanematchingstrategy.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 <adore/mad/adoremath.h>
22 #include <unordered_map>
23 
24 namespace adore
25 {
26  namespace env
27  {
28  namespace BorderBased
29  {
30 
32  {
33  public:
35  };
36 
38  {
39  private:
42  public:
43  LMSNearest(BorderSet* borderSet):m_borderSet(borderSet)
44  {
45  m_w_orientation=5.0;
46  }
48  {
49  return getBestMatch(borderSubset,ego->getX(),ego->getY(),ego->getPSI());
50  }
51  Border* getBestMatch(BorderSubSet* borderSubset,double x,double y,double psi)
52  {
53  if(borderSubset->size()==0)return 0;
54  if(borderSubset->size()==1)return *borderSubset->begin();
55 
56  Border* bestBorder = 0;
57  double bestValue = 9e99;
58  adoreMatrix<double,3,1> vpos;
59  vpos(0) = x;
60  vpos(1) = y;
61  vpos(2) = 0;
62  for(auto it = borderSubset->begin();it!=borderSubset->end();it++)
63  {
64  if(!m_borderSet->borderTypeValid(*it))
65  {
66  continue;
67  }
68  double value = 0;
69  double t,n;
71  Border* right = *it;
72 
73  //left border distance
74  if(left!=0)
75  {
76  double sleft = left->m_path->getPositionOfPoint(vpos(0),vpos(1),1,2,t,n);
77  value += adore::mad::norm2<double,3>(left->m_path->f(sleft)-vpos);
78  }
79  else
80  {
81  value += 100.0;
82  }
83 
84  //right border distance
85  double sright = right->m_path->getPositionOfPoint(vpos(0),vpos(1),1,2,t,n);
86  value += adore::mad::norm2<double,3>(right->m_path->f(sright)-vpos);
87 
88  //orientation
89  double right_orientation = std::atan2(right->m_path->dfidx(sright,1),right->m_path->dfidx(sright,0));
90  double dx = std::cos(right_orientation) - std::cos(psi);
91  double dy = std::sin(right_orientation) - std::sin(psi);
92  value += std::sqrt(dx*dx+dy*dy)*m_w_orientation;
93 
94  if(value<bestValue)
95  {
96  bestValue = value;
97  bestBorder = right;
98  }
99  }
100  return bestBorder;
101  }
102  };
103 
105  {
106  private:
109  LMSNearest m_nearestLaneStragey;//use nearest lane as a tie braking strategy
112  double m_a,m_b,m_c,m_d,m_w;//
113  public:
114  LMSContinuation(BorderSet* borderSet,bool delayedSwitching=false):m_nearestLaneStragey(borderSet),m_borderSet(borderSet)
115  {
116  m_delayedSwitching = delayedSwitching;
118  m_a = 1.1;
119  m_b = 1.7;
120  m_c = 0.5;
121  m_d = 0.3;
122  m_w = 1.7;
123  }
124  void setDimensions(double a,double b,double c,double d,double w)
125  {
126  m_a = a;m_b=b;m_c=c;m_d=d;m_w=w;
127  }
128  virtual Border* getBestMatch(BorderSubSet* bordersInRegion,adore::env::VehicleMotionState9d* ego)override
129  {
130  //if any of these points is inside a lane, then it is a preferred candidate
131  double p0X,p0Y,p1X,p1Y,p2X,p2Y,p3X,p3Y,p4X,p4Y;
132  double cpsi = std::cos(ego->getPSI());
133  double spsi = std::sin(ego->getPSI());
134  double rho = (m_a+m_b+m_c+m_d)*0.5;
135  double w2 = m_w*0.5;
136  p0X = ego->getX() + cpsi * (rho-m_d);
137  p0Y = ego->getY() + spsi * (rho-m_d);
138 
139 
141  {
142  p1X = p0X + (rho)*cpsi - (w2)*spsi;
143  p1Y = p0Y + (rho)*spsi + (w2)*cpsi;
144 
145  p2X = p0X + (rho)*cpsi - (-w2)*spsi;
146  p2Y = p0Y + (rho)*spsi + (-w2)*cpsi;
147 
148  p3X = p0X + (-rho)*cpsi - (w2)*spsi;
149  p3Y = p0Y + (-rho)*spsi + (w2)*cpsi;
150 
151  p4X = p0X + (-rho)*cpsi - (-w2)*spsi;
152  p4Y = p0Y + (-rho)*spsi + (-w2)*cpsi;
153  }
154  else
155  {
156  p1X = p0X - spsi*1.1;
157  p1Y = p0Y + cpsi*1.1;
158  p2X = p0X + spsi*1.1;
159  p2Y = p0Y - cpsi*1.1;
160  p3X = 0.0; // initialized to silence -Wmaybe-uninitialized warning
161  p3Y = 0.0; // initialized to silence -Wmaybe-uninitialized warning
162  p4X = 0.0; // initialized to silence -Wmaybe-uninitialized warning
163  p4Y = 0.0; // initialized to silence -Wmaybe-uninitialized warning
164  }
165 
166  if(bordersInRegion->size()==0)
167  {
169  return 0;
170  }
171  BorderSubSet decendants;//find decendents of previous
173  {
174  for(auto it = bordersInRegion->begin();it!=bordersInRegion->end();it++)
175  {
176  if(!m_borderSet->borderTypeValid(*it))
177  {
178  continue;
179  }
180  else if((*it)->m_id == m_lastMatch && m_delayedSwitching)
181  {
182  if( (*it)->isPointInsideLane(m_borderSet->getLeftNeighbor(*it),p0X,p0Y)
183  || (*it)->isPointInsideLane(m_borderSet->getLeftNeighbor(*it),p1X,p1Y)
184  || (*it)->isPointInsideLane(m_borderSet->getLeftNeighbor(*it),p2X,p2Y)
185  || (*it)->isPointInsideLane(m_borderSet->getLeftNeighbor(*it),p3X,p3Y)
186  || (*it)->isPointInsideLane(m_borderSet->getLeftNeighbor(*it),p4X,p4Y)
187  )
188  {
189  return *it;//still on the same lane
190  }
191  }
192  else if((*it)->m_id == m_lastMatch)
193  {
194  if( (*it)->isPointInsideLane(m_borderSet->getLeftNeighbor(*it),p0X,p0Y)
195  || (*it)->isPointInsideLane(m_borderSet->getLeftNeighbor(*it),p1X,p1Y)
196  || (*it)->isPointInsideLane(m_borderSet->getLeftNeighbor(*it),p2X,p2Y) )
197  {
198  return *it;//still on the same lane
199  }
200  }
201  else if((*it)->isSuccessorOf(m_lastMatch))
202  {
203  if( (*it)->isPointInsideLane(m_borderSet->getLeftNeighbor(*it),p0X,p0Y)
204  || (*it)->isPointInsideLane(m_borderSet->getLeftNeighbor(*it),p1X,p1Y)
205  || (*it)->isPointInsideLane(m_borderSet->getLeftNeighbor(*it),p2X,p2Y) )
206  {
207  decendants.push_back(*it);//vehicle is on a successor of the previous lane --> test, which successor is best
208  }
209  }
210  }
211  }
212 
213  if( decendants.size()>0 )
214  {
215  Border* best = m_nearestLaneStragey.getBestMatch(&decendants,ego);
216  m_lastMatch = best->m_id;
218  return best;
219  }
220  else
221  {
222  Border* best = m_nearestLaneStragey.getBestMatch(bordersInRegion,ego);
223  if( best!=0 )
224  {
225  m_lastMatch = best->m_id;
227  }
228  else
229  {
230  m_lastMatch_initialized = false;
231  }
232  return best;
233  }
234  }
235  void reset()
236  {
238  }
239  };
240 
242  {
243  private:
249  public:
250  LMSNavigation(BorderSet* borderSet, BorderCostMap* borderID2Cost)
251  :m_borderSet(borderSet),m_borderID2Cost(borderID2Cost),m_continuationStrategy(borderSet)
252  {
254  };
255 
256  virtual Border* getBestMatch(BorderSubSet* borderSubset,adore::env::VehicleMotionState9d* ego) override
257  {
258  if(borderSubset->size()==0)
259  {
261  return 0;
262  }
263  else
264  {
265  if(borderSubset->size()==1)return *borderSubset->begin();
266  }
267 
268  // copied from LMSContinuation - find candidates for decendents
269  BorderSubSet decendants;//find decendents of previous
271  {
272  for(auto it = borderSubset->begin();it!=borderSubset->end();it++)
273  {
274  if(!m_borderSet->borderTypeValid(*it))
275  {
276  continue;
277  }
278  if((*it)->m_id == m_lastMatch && (*it)->isPointInsideLane(m_borderSet->getLeftNeighbor(*it),ego->getX(),ego->getY()))
279  {
280  return *it;//still on the same lane
281  }
282  if((*it)->isSuccessorOf(m_lastMatch) && (*it)->isPointInsideLane(m_borderSet->getLeftNeighbor(*it),ego->getX(),ego->getY()))
283  {
284  decendants.push_back(*it);//vehicle is on a successor of the previous lane --> test, which successor is best
285  }
286  }
287  }
288 
289  // compare costs as tiebreaker
290  bool costSet = false;
291  Border* best = 0;
292  if( decendants.size()>0 )
293  {
294  double minCost = 0;
295  for(auto it = decendants.begin();it!=decendants.end();it++)
296  {
297  if(best == 0 && m_borderID2Cost->find((*it)->m_id) == m_borderID2Cost->end())
298  {
299  best = *it;
300  }
301  else
302  {
303  auto nextBorder2Cost = m_borderID2Cost->find((*it)->m_id);
304  if(nextBorder2Cost != m_borderID2Cost->end())
305  {
306  if(!costSet)
307  {
308  best = *it;
309  minCost = nextBorder2Cost->second.getCombinedCost();
310  costSet = true;
311  }
312  else if(minCost > nextBorder2Cost->second.getCombinedCost())
313  {
314  best = *it;
315  minCost = nextBorder2Cost->second.getCombinedCost();
316  }
317  }
318  }
319  }
320 
321  }
322  if(costSet)
323  {
324  m_lastMatch = best->m_id;
326  return best;
327  }
328  else
329  {
330  Border* best = m_continuationStrategy.getBestMatch(borderSubset,ego);
331  if( best!=0 )
332  {
333  m_lastMatch = best->m_id;
335  }
336  else
337  {
338  m_lastMatch_initialized = false;
339  }
340  return best;
341  }
342  }
343  };
344 
348  template <int N>
350  {
351  public:
352  bool m_isPointInside[N];//RR,FR,FL,RL
353  bool m_isReferenceInside;//center?
354  double s;//local coordinates of reference point, relative to right border: best fit parameter of right border function
355  double soff;//local coordinates of reference point, relative to right border: tangential offset
356  double noff;//local coordinates of reference point, relative to right border: normal offset
358  BorderPositioning(Border* bright, Border* bleft,double Xref,double Yref,double* X,double* Y)
359  {
360  s = bright->m_path->getPositionOfPoint(Xref,Yref,1,2,soff,noff);
361  for(int i=0;i<N;i++)
362  {
363  m_isPointInside[i] = bright->m_path->isPointEnclosed(bleft->m_path,X[i],Y[i],1,2,false,bright->getNeighborDirection()!=Border::OPPOSITE_DIRECTION);
364  }
365  m_isReferenceInside = bright->m_path->isPointEnclosed(bleft->m_path,Xref,Yref,1,2,false,bright->getNeighborDirection()!=Border::OPPOSITE_DIRECTION);
366  }
367  bool anyInside()
368  {
369  if(m_isReferenceInside)return true;
370  for(int i=0;i<N;i++)if(m_isPointInside[i])return true;
371  return false;
372  }
373  bool allInside()
374  {
375  if(!m_isReferenceInside)return false;
376  for(int i=0;i<N;i++)if(!m_isPointInside[i])return false;
377  return true;
378  }
380  {
381  return m_isReferenceInside;
382  }
383 
384  };
385 
386  }
387 
388  }
389 }
Definition: bordercostmap.h:31
Definition: lanematchingstrategy.h:350
double noff
Definition: lanematchingstrategy.h:356
BorderPositioning()
Definition: lanematchingstrategy.h:357
bool m_isPointInside[N]
Definition: lanematchingstrategy.h:352
BorderPositioning(Border *bright, Border *bleft, double Xref, double Yref, double *X, double *Y)
Definition: lanematchingstrategy.h:358
bool allInside()
Definition: lanematchingstrategy.h:373
double s
Definition: lanematchingstrategy.h:354
bool isReferenceInside()
Definition: lanematchingstrategy.h:379
double soff
Definition: lanematchingstrategy.h:355
bool m_isReferenceInside
Definition: lanematchingstrategy.h:353
bool anyInside()
Definition: lanematchingstrategy.h:367
efficiently store borders in boost R-tree
Definition: borderset.h:99
Border * getLeftNeighbor(Border *b)
Get left neighbor of a border.
Definition: borderset.h:1252
bool borderTypeValid(Border *b)
check whether border type is in allowed types of set
Definition: borderset.h:204
Definition: lanematchingstrategy.h:105
bool m_lastMatch_initialized
Definition: lanematchingstrategy.h:107
LMSNearest m_nearestLaneStragey
Definition: lanematchingstrategy.h:109
double m_a
Definition: lanematchingstrategy.h:112
bool m_delayedSwitching
Definition: lanematchingstrategy.h:111
double m_c
Definition: lanematchingstrategy.h:112
double m_d
Definition: lanematchingstrategy.h:112
void setDimensions(double a, double b, double c, double d, double w)
Definition: lanematchingstrategy.h:124
BorderSet * m_borderSet
Definition: lanematchingstrategy.h:110
double m_w
Definition: lanematchingstrategy.h:112
void reset()
Definition: lanematchingstrategy.h:235
virtual Border * getBestMatch(BorderSubSet *bordersInRegion, adore::env::VehicleMotionState9d *ego) override
Definition: lanematchingstrategy.h:128
double m_b
Definition: lanematchingstrategy.h:112
BorderID m_lastMatch
Definition: lanematchingstrategy.h:108
LMSContinuation(BorderSet *borderSet, bool delayedSwitching=false)
Definition: lanematchingstrategy.h:114
Definition: lanematchingstrategy.h:242
LMSContinuation m_continuationStrategy
Definition: lanematchingstrategy.h:248
BorderID m_lastMatch
Definition: lanematchingstrategy.h:246
BorderSet * m_borderSet
Definition: lanematchingstrategy.h:245
BorderCostMap * m_borderID2Cost
Definition: lanematchingstrategy.h:247
bool m_lastMatch_initialized
Definition: lanematchingstrategy.h:244
virtual Border * getBestMatch(BorderSubSet *borderSubset, adore::env::VehicleMotionState9d *ego) override
Definition: lanematchingstrategy.h:256
LMSNavigation(BorderSet *borderSet, BorderCostMap *borderID2Cost)
Definition: lanematchingstrategy.h:250
Definition: lanematchingstrategy.h:38
double m_w_orientation
Definition: lanematchingstrategy.h:41
virtual Border * getBestMatch(BorderSubSet *borderSubset, adore::env::VehicleMotionState9d *ego) override
Definition: lanematchingstrategy.h:47
BorderSet * m_borderSet
Definition: lanematchingstrategy.h:40
LMSNearest(BorderSet *borderSet)
Definition: lanematchingstrategy.h:43
Border * getBestMatch(BorderSubSet *borderSubset, double x, double y, double psi)
Definition: lanematchingstrategy.h:51
Definition: lanematchingstrategy.h:32
virtual Border * getBestMatch(BorderSubSet *borderSubset, adore::env::VehicleMotionState9d *ego)=0
double getPositionOfPoint(T px, T py, int d1, int d2, T &d_tangential_min, T &d_normal_min)
Definition: llinearpiecewisefunction.h:955
bool isPointEnclosed(LLinearPiecewiseFunctionM< T, n > *other, T px, T py, int d1, int d2, bool invert_this=false, bool invert_other=true)
Definition: llinearpiecewisefunction.h:1075
std::vector< Border * > BorderSubSet
Definition: borderset.h:92
@ right
Definition: indicator_hint.h:36
@ left
Definition: indicator_hint.h:35
interval< T > atan2(interval< T > y, interval< T > x)
Definition: intervalarithmetic.h:234
interval< T > cos(interval< T > x)
Definition: intervalarithmetic.h:225
void set(T *data, T value, int size)
Definition: adoremath.h:39
interval< T > sin(interval< T > x)
Definition: intervalarithmetic.h:204
x
Definition: adore_set_goal.py:30
y
Definition: adore_set_goal.py:31
w
Definition: adore_set_pose.py:40
Definition: areaofeffectconverter.h:20
This struct identifies a Border by the coordinates of the starting and the end point.
Definition: borderid.h:31
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
BorderID m_id
Definition: border.h:68
This struct holds the motion state of the vehicle in 9d.
Definition: vehiclemotionstate9d.h:39
double getX() const
Get the x-coordinate.
Definition: vehiclemotionstate9d.h:54
double getY() const
Get the y-coordinate.
Definition: vehiclemotionstate9d.h:60
double getPSI() const
Get the heading.
Definition: vehiclemotionstate9d.h:72