ADORe
ADORe is a modular open source software library and toolkit for decision making, planning, control and simulation of automated vehicles
lanechangegeometry.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
14  * Matthias Nichting
15  * Thomas Lobig - refactoring
16  ********************************************************************************/
17 
18 #pragma once
19 // TODO refactor alangechangeview to have LEFT RIGHT enum in a seperate FILE!
21 #include <adore/mad/adoremath.h>
23 #include <unordered_set>
27 namespace adore
28 {
29 namespace env
30 {
31 namespace BorderBased
32 {
38 {
39 
40 public:
45  typedef std::unordered_set<BorderBased::Node *, BorderBased::NodeHasher> TBorderSubSet;
68  double m_vehicle_width;
69  bool m_view_valid;
71 public:
77  {
78  m_vehicle_width = 1.8;
79  }
86  bool isValid() const
87  {
88  return m_view_valid;
89  }
95  {
96  for (auto it = m_borderSubSet.begin(); it != m_borderSubSet.end(); it++)
97  {
98  delete *it;
99  }
100  m_borderSubSet.clear();
101  m_borderSequence.clear();
102  m_innerBorderSequence.clear();
103  }
104  template <int lfg_a, int lfg_b>
118  int lfg_adjacency_i_start, adore::view::ALaneChangeView::direction direction, double review_distance,
119  double preview_distance, double adjacency_lower_limit = 0.0)
120  {
121  m_view_valid = false;
122  m_direction = direction;
123  clearGeometry();
124  m_lfg_adjacency_b0 = 0;
125  m_lfg_adjacency_b1 = 0;
128 
129  bool open = false;
130  Border *neighbor;
131  double s = 0.0; // initialized to silence -Wmaybe-uninitialized warning
132  int i = 0;
133  for (auto it = lfg->begin(); it != lfg->end(); it++, i++)
134  {
135  if (i >= lfg_adjacency_i_start)
136  {
137  if (direction == adore::view::ALaneChangeView::LEFT)
138  {
139  neighbor = borderSet->getLeftNeighbor(*it);
140  if ((*it)->getNeighborDirection() == Border::OPPOSITE_DIRECTION)
141  {
142  neighbor = 0;
143  }
144  else if (neighbor != 0 && !borderSet->borderTypeValid(neighbor))
145  {
146  neighbor = 0;
147  }
148  }
149  else
150  {
151  neighbor = borderSet->getRightNeighbor(*it);
152  if (neighbor != 0 && !borderSet->borderTypeValid(neighbor))
153  {
154  neighbor = 0;
155  }
156  }
157  if (open)
158  {
159  // closing found
160  if (neighbor == 0 || !m_lfg_adjacency_bn1->m_border->isPredecessorOf(neighbor->m_id))
161  {
162  m_adjacency_s1 = s;
163  open = false;
164  // is this the correct adjacency? if it finishes before lower limit, a reset is necessary
165  if (m_adjacency_s1 > adjacency_lower_limit)
166  {
167  break; // break the for loop, adjacency is defined
168  }
169  else
170  {
171  // in the following remove previously found opening and replace it with borders of lfv
172  m_borderSequence.clear();
174  m_innerBorderSequence.end());
175  }
176  }
177  // closing not yet found
178  else
179  {
180  m_lfg_adjacency_b1 = *it;
181  m_lfg_adjacency_i1 = i;
182  BorderBased::Node *node = new BorderBased::Node(neighbor);
183  node->setG(s);
184  node->setPredecessor(m_lfg_adjacency_bn1);
185  m_borderSubSet.emplace(node);
187 
188  s += neighbor->getLength();
189  }
190  }
191  else
192  {
193  // find the opening
194  if (neighbor != 0)
195  {
196  bool neighboringLaneExists;
197  if (direction == adore::view::ALaneChangeView::LEFT)
198  {
199  neighboringLaneExists =
200  borderSet->hasLeftNeighbor(neighbor) && neighbor->getNeighborDirection() == Border::SAME_DIRECTION;
201  }
202  else
203  {
204  neighboringLaneExists = true;
205  }
206  if (neighboringLaneExists)
207  {
208  open = true;
209  m_lfg_adjacency_b0 = *it;
210  m_lfg_adjacency_b1 = *it;
211  m_lfg_adjacency_i0 = i;
212  m_lfg_adjacency_i1 = i;
213 
214  double n;
215  lfg->toRelativeCoordinates(neighbor->m_id.m_first.m_X, neighbor->m_id.m_first.m_Y, m_adjacency_s0, n);
216  s = m_adjacency_s0;
217  BorderBased::Node *node = new BorderBased::Node(neighbor);
218  node->setG(s);
219  m_borderSubSet.emplace(node);
222 
223  s += neighbor->getLength();
224  }
225  }
226  }
227  // extract the outer border before and during adjacency
228  if (open)
229  {
230  m_borderSequence.push_back(neighbor);
231  }
232  else
233  {
234  m_borderSequence.push_back(*it);
235  }
236  }
237  else
238  {
239  m_borderSequence.push_back(*it);
240  }
241  m_innerBorderSequence.push_back(*it);
242  }
243  if (open)
244  m_adjacency_s1 = s;
246 
247  if (m_lfg_adjacency_bn0 != 0 && m_lfg_adjacency_bn1 != 0)
248  {
249  BASFollowStraight basFollowStraight(m_lfg_adjacency_bn1->m_border, borderSet,
250  std::max(0.0, preview_distance - s));
252  bool inverted;
253  Border *current;
254  basFollowStraight.getNextBorder(current, inverted);
255  for (basFollowStraight.getNextBorder(current, inverted); current != 0;
256  basFollowStraight.getNextBorder(current, inverted))
257  {
258  m_borderSequence.push_back(current);
259  m_innerBorderSequence.push_back(current);
260 
261  BorderBased::Node *node = new BorderBased::Node(current);
262  node->setG(lastNode->g() + lastNode->m_border->getLength());
263  node->setPredecessor(lastNode);
264  m_borderSubSet.emplace(node);
265  lastNode = node;
266  m_s_viewing_distance = node->g() + node->m_border->getLength();
267  }
268 
269  BorderGraph graph(borderSet);
270  std::vector<BorderBased::Node *> open_list;
271  open_list.push_back(m_lfg_adjacency_bn0);
272  std::unordered_set<Border *> closed_set;
273 
274  while (open_list.size() > 0)
275  {
276  BorderBased::Node *parent = open_list.back();
277  open_list.pop_back();
278  closed_set.emplace(parent->m_border);
279  auto expansion = graph.getPredecessors(parent, false, false);
280  while (expansion.hasMore())
281  {
282  BorderBased::Node *child = new BorderBased::Node(expansion.getNext());
283  if (closed_set.find(child->m_border) == closed_set.end() &&
284  m_borderSubSet.find(child) == m_borderSubSet.end() &&
286  {
287  // remember border for the selection of relevant traffic participants
288  m_borderSubSet.emplace(child);
289  // explore graph against driving direction, up to review_distance depth
290  if (m_adjacency_s0 - child->g() < review_distance)
291  {
292  open_list.push_back(child);
293  }
294  }
295  else
296  {
297  delete child;
298  }
299  }
300  }
301 
302  BorderAccumulator leftBA, rightBA, centerBorderBA;
303  if (direction == adore::view::ALaneChangeView::LEFT)
304  {
305  BASNeighbor basNeighbor(&m_borderSequence, borderSet, BASNeighbor::LEFT);
306  leftBA.append(&basNeighbor);
307 
308  for (auto it = m_borderSequence.begin(); it != m_borderSequence.end(); it++)
309  {
310  centerBorderBA.append(*it, false);
311  }
312 
313  for (auto it = m_innerBorderSequence.begin(); it != m_innerBorderSequence.end(); it++)
314  {
315  rightBA.append(*it, false);
316  }
317  }
318  else
319  {
320  BASNeighbor basNeighbor(&m_innerBorderSequence, borderSet, BASNeighbor::LEFT);
321  leftBA.append(&basNeighbor);
322 
323  BASNeighbor basNeighbor2(&m_borderSequence, borderSet, BASNeighbor::LEFT);
324  centerBorderBA.append(&basNeighbor2);
325 
326  for (auto it = m_borderSequence.begin(); it != m_borderSequence.end(); it++)
327  {
328  rightBA.append(*it, false);
329  }
330  }
331 
334  centerBorderBA.defineFunction(m_centerBorder_fct);
335  double max_lane_change_distance = 10.0;
336 
338  &m_leftBorder_fct, max_lane_change_distance, lfg->getOffsetOfLeftBorderFct());
340  &m_rightBorder_fct, max_lane_change_distance, lfg->getOffsetOfRightBorderFct());
341  if (direction == adore::view::ALaneChangeView::LEFT)
342  {
344  &m_centerBorder_fct, max_lane_change_distance, lfg->getOffsetOfRightBorderFct());
345  }
346  else
347  {
349  &m_centerBorder_fct, max_lane_change_distance, lfg->getOffsetOfLeftBorderFct());
350  }
351 
352  bool width_open = false;
353  m_s_lane_width_open = 0.0;
354  m_s_lane_width_closed = 10000.0;
355  auto border =
357  auto data = m_centerBorderDistance_fct.getData();
358  for (int i = 0; i < data.nc(); i++)
359  {
360  double d = -(std::abs)(data(1, i)) + (std::abs)(border->fi(data(0, i), 0)) - m_vehicle_width;
361  if (width_open)
362  {
363  if (d < 0)
364  {
365  m_s_lane_width_closed = data(0, i);
366  break;
367  }
368  }
369  else
370  {
371  if (d > 0)
372  {
373  m_s_lane_width_open = data(0, i);
374  width_open = true;
375  }
376  }
377  }
378 
380  }
381  }
382 
389  {
390  return &m_innerBorderSequence;
391  }
398  {
399  return &m_borderSequence;
400  }
407  {
408  if(m_direction == adore::view::ALaneChangeView::direction::LEFT)
409  return &m_borderSequence;
410  return &m_innerBorderSequence;
411  }
418  {
419  if(m_direction == adore::view::ALaneChangeView::direction::RIGHT)
420  return &m_borderSequence;
421  return &m_innerBorderSequence;
422  }
423 
424 
430  double getViewingDistance() const
431  {
432  return m_s_viewing_distance;
433  }
440  {
441  return m_direction;
442  }
448  double getProgressOfWidthOpen() const
449  {
450  return m_s_lane_width_open;
451  }
458  {
459  return m_s_lane_width_closed;
460  }
467  double getOffsetOfLeftBorder(double s)
468  {
470  return m_leftBorderDistance_fct(s);
471  }
478  double getOffsetOfRightBorder(double s)
479  {
481  return m_rightBorderDistance_fct(s);
482  }
489  double getOffsetOfCenterBorder(double s)
490  {
491  return m_centerBorderDistance_fct(s);
492  }
498  double getProgressOfGateOpen() const
499  {
500  return m_adjacency_s0;
501  }
507  double getProgressOfGateClosed() const
508  {
509  return m_adjacency_s1;
510  }
511 };
512 } // namespace BorderBased
513 } // namespace env
514 } // namespace adore
This class chooses the straightest successor of a border until an upper limit on distance is reached.
Definition: borderaccumulator.h:78
virtual void getNextBorder(Border *&border, bool &inverted) override
Get the next border.
Definition: borderaccumulator.h:97
This class choses the left/right neighbors of a border sequence.
Definition: borderaccumulator.h:231
@ LEFT
Definition: borderaccumulator.h:242
This class collects a sequence of borders, according to chosen BorderAccumulationStrategy.
Definition: borderaccumulator.h:293
void append(Border *current, bool inverted)
‍**
Definition: borderaccumulator.h:353
void defineFunction(function_type &f)
Create (with new) a function, which contains all accumulated border paths.
Definition: borderaccumulator.h:395
Definition: bordergraph.h:213
Expansion getPredecessors(Node *n, bool left_neighbors=true, bool right_neighbors=true, bool allow_direction_inversion=false)
Definition: bordergraph.h:346
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
Border * getLeftNeighbor(Border *b)
Get left neighbor of a border.
Definition: borderset.h:1252
bool hasLeftNeighbor(Border *b)
checks whether left border exists for a border
Definition: borderset.h:1267
bool borderTypeValid(Border *b)
check whether border type is in allowed types of set
Definition: borderset.h:204
A class with a geometry description of a lane next to the current lane.
Definition: lanechangegeometry.h:38
bool isValid() const
Check whether the LaneChangeGeometry is valid.
Definition: lanechangegeometry.h:86
double m_s_viewing_distance
Definition: lanechangegeometry.h:55
adore::view::ALaneChangeView::direction getLCDirection() const
Get the direction of the LaneChangeGeometry.
Definition: lanechangegeometry.h:439
double m_vehicle_width
Definition: lanechangegeometry.h:68
Border * m_lfg_adjacency_b0
Definition: lanechangegeometry.h:60
BorderBased::Node * m_lfg_adjacency_bn0
Definition: lanechangegeometry.h:62
bool m_view_valid
Definition: lanechangegeometry.h:69
int m_lfg_adjacency_i0
Definition: lanechangegeometry.h:56
function_type_xyz m_rightBorder_fct
Definition: lanechangegeometry.h:47
double getProgressOfWidthClosed() const
Get the s-coordinate where the lane stops to have the required width.
Definition: lanechangegeometry.h:457
function_type_xyz m_reference_fct
Definition: lanechangegeometry.h:49
double getProgressOfGateOpen() const
Get the s-coordinate where the lane starts to be in direct adjacency to the LaneFollowingGeometry whi...
Definition: lanechangegeometry.h:498
TBorderSubSet m_borderSubSet
Definition: lanechangegeometry.h:64
BorderBased::BAContainer m_borderSequence
Definition: lanechangegeometry.h:65
function_type_xyz m_centerBorder_fct
Definition: lanechangegeometry.h:48
LaneChangeGeometry()
Construct a new LaneChangeGeometry object.
Definition: lanechangegeometry.h:76
function_type_xyz m_leftBorder_fct
Definition: lanechangegeometry.h:46
double getProgressOfWidthOpen() const
Get the s-coordinate where the lane reaches the required width.
Definition: lanechangegeometry.h:448
double getOffsetOfRightBorder(double s)
Get the offset of the right border at a certain position.
Definition: lanechangegeometry.h:478
double getOffsetOfCenterBorder(double s)
Get the offset of the center border at a certain position.
Definition: lanechangegeometry.h:489
int m_lfg_adjacency_i1
Definition: lanechangegeometry.h:57
BorderSubSet * getInnerBorders()
Get the sequence of inner Borders.
Definition: lanechangegeometry.h:388
double m_adjacency_s0
Definition: lanechangegeometry.h:58
adore::view::ALaneChangeView::direction m_direction
Definition: lanechangegeometry.h:67
adore::mad::function_type_scalar function_type_scalar
Definition: lanechangegeometry.h:43
function_type_scalar m_rightBorderDistance_fct
Definition: lanechangegeometry.h:51
double getProgressOfGateClosed() const
Get the s-coordinate where the lane ends to be in direct adjacency to the LaneFollowingGeometry whith...
Definition: lanechangegeometry.h:507
BorderSubSet * getRightBorders()
Get the sequence of inner Borders.
Definition: lanechangegeometry.h:417
void update(LaneFollowingGeometry< lfg_a, lfg_b > *lfg, BorderSet *borderSet, adore::env::VehicleMotionState9d *ego, int lfg_adjacency_i_start, adore::view::ALaneChangeView::direction direction, double review_distance, double preview_distance, double adjacency_lower_limit=0.0)
update the road geometry
Definition: lanechangegeometry.h:117
void clearGeometry()
Clear the LaneChangeGeometry.
Definition: lanechangegeometry.h:94
double getViewingDistance() const
Get the viewing distance.
Definition: lanechangegeometry.h:430
double m_s_lane_width_closed
Definition: lanechangegeometry.h:54
double m_adjacency_s1
Definition: lanechangegeometry.h:59
BorderSubSet * getLeftBorders()
Get the sequence of inner Borders.
Definition: lanechangegeometry.h:406
function_type_scalar m_centerBorderDistance_fct
Definition: lanechangegeometry.h:52
BorderBased::BAContainer m_innerBorderSequence
Definition: lanechangegeometry.h:66
function_type_scalar m_leftBorderDistance_fct
Definition: lanechangegeometry.h:50
double m_s_lane_width_open
Definition: lanechangegeometry.h:53
std::unordered_set< BorderBased::Node *, BorderBased::NodeHasher > TBorderSubSet
Definition: lanechangegeometry.h:45
BorderBased::Node * m_lfg_adjacency_bn1
Definition: lanechangegeometry.h:63
Border * m_lfg_adjacency_b1
Definition: lanechangegeometry.h:61
double getOffsetOfLeftBorder(double s)
Get the offset of the left border at a certain position.
Definition: lanechangegeometry.h:467
BorderSubSet * getOuterBorders()
Get the sequence of outer Borders.
Definition: lanechangegeometry.h:397
A class with a geometry description of the current lane.
Definition: lanefollowinggeometry.h:41
void toRelativeCoordinates(double Xg, double Yg, double &s, double &n)
Transform from euclidian to relative coordinates.
Definition: lanefollowinggeometry.h:770
BAContainer::iterator begin()
Get the begin()-iterator of the BAContainer of the right borders.
Definition: lanefollowinggeometry.h:692
BAContainer::iterator end()
Get the end()-iterator of the BAContainer of the right borders.
Definition: lanefollowinggeometry.h:701
function_type_scalar * getOffsetOfLeftBorderFct()
Get the function that holds the offset of left borders.
Definition: lanefollowinggeometry.h:732
function_type2d * getCenterlineNormal()
Get the Centerline Normal object.
Definition: lanefollowinggeometry.h:660
function_type_xyz * getCenterline()
Get the centerline of the lane.
Definition: lanefollowinggeometry.h:650
function_type_scalar * getOffsetOfRightBorderFct()
Get the function that holds the offset of right borders.
Definition: lanefollowinggeometry.h:752
virtual DT limitLo() const override
Definition: llinearpiecewisefunction.h:264
adoreMatrix< T, n+1, 0 > & getData()
Definition: llinearpiecewisefunction.h:147
virtual DT limitHi() const override
Definition: llinearpiecewisefunction.h:259
direction
Definition: alanechangeview.h:42
@ LEFT
Definition: alanechangeview.h:43
std::vector< Border * > BorderSubSet
Definition: borderset.h:92
std::vector< Border * > BAContainer
Definition: borderaccumulator.h:35
adore::mad::LLinearPiecewiseFunctionM< double, 3 > function_type_xyz
Definition: linearfunctiontypedefs.h:22
adore::mad::LLinearPiecewiseFunctionM< double, 1 > function_type_scalar
Definition: linearfunctiontypedefs.h:24
T bound(T lb, T value, T ub)
Definition: adoremath.h:569
void defineDistanceMap2d(LLinearPiecewiseFunctionM< T, n_dfun > *dfun, int id, LLinearPiecewiseFunctionM< T, n_base > *base, LLinearPiecewiseFunctionM< T, n_normal > *normal, LLinearPiecewiseFunctionM< T, n_target > *target, T guard, LLinearPiecewiseFunctionM< T, 1 > *guardfun=0)
Definition: llinearpiecewisefunction.h:1405
adore::mad::LLinearPiecewiseFunctionM< double, 2 > function_type2d
Definition: linearfunctiontypedefs.h:23
adoreMatrix< T, N, M > max(adoreMatrix< T, N, M > a, const adoreMatrix< T, N, M > &b)
Definition: adoremath.h:686
Definition: areaofeffectconverter.h:20
Coordinate m_first
Definition: borderid.h:32
The border struct contains data of the smallest.
Definition: border.h:62
@ OPPOSITE_DIRECTION
Definition: border.h:507
@ SAME_DIRECTION
Definition: border.h:507
bool isPredecessorOf(const BorderID &successorID)
Check whether border is a direct predecessor of another border.
Definition: border.h:333
bool isContinuousPredecessorOf(Border *other)
Check whether the border is a continuous predecessor of another border.
Definition: border.h:363
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 m_Y
Definition: coordinate.h:35
double m_X
Definition: coordinate.h:35
Definition: bordergraph.h:28
double g() const
Definition: bordergraph.h:71
Border * m_border
Definition: bordergraph.h:30
This struct holds the motion state of the vehicle in 9d.
Definition: vehiclemotionstate9d.h:39
ControlErrorNode node
Definition: test_control_error_node.cpp:124