ADORe
ADORe is a modular open source software library and toolkit for decision making, planning, control and simulation of automated vehicles
lanechangeborders.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 #pragma once
18 #include <vector>
19 
20 namespace adore
21 {
22 namespace env
23 {
24 namespace BorderBased
25 {
30 {
31  private:
35  double lookahead_;
36  double lookbehind_;
41  bool valid_;
43  public:
44  std::vector<Border*> gate_target_borders_;
45  std::vector<Border*> gate_source_borders_;
46  std::vector<Border*> downstream_borders_;
47  std::vector<Border*> upstream_borders_;
48  public:
49  LaneChangeBorders(bool lc_direction_left,BorderSet* borderSet,BorderCostMap* borderCostMap)
50  :lc_direction_left_(lc_direction_left),borderSet_(borderSet),borderCostMap_(borderCostMap),
51  valid_(false)
52  {
53  lookahead_ = 100.0;
54  lookbehind_ = 100.0;
56  }
58  {
59  return lc_direction_left_;
60  }
61  bool isValid()
62  {
63  return valid_;
64  }
66  {
67  return progress_in_gate_;
68  }
70  {
71  return remaining_in_gate_;
72  }
74  {
75  return downstream_length_;
76  }
78  {
79  return distance_to_current_;
80  }
81  double getLookAhead()
82  {
83  return lookahead_;
84  }
85  void setLookAhead(double value)
86  {
87  lookahead_ = value;
88  }
89  double getLookBehind()
90  {
91  return lookbehind_;
92  }
93  void setLookBehind(double value)
94  {
95  lookbehind_ = value;
96  }
97  void setConinueOnIncreasingCost(bool value)
98  {
100  }
102  {
103  //query navigation cost at end of gate
104  auto q0 = borderCostMap_->find((*gate_source_borders_.rbegin())->m_id);
105  auto q1 = borderCostMap_->find((*gate_target_borders_.rbegin())->m_id);
107  if(!(q0==borderCostMap_->end()))
108  {
109  cost0 = q0->second;
110  }
112  if(!(q1==borderCostMap_->end()))
113  {
114  cost1 = q1->second;
115  }
116  return cost1.getCombinedCost()-cost0.getCombinedCost();
117  }
126  template<typename Iterator>
127  void update(double position_on_current,Iterator current_lf, Iterator first_lf, Iterator last_lf)
128  {
129  gate_target_borders_.clear();
130  gate_source_borders_.clear();
131  downstream_borders_.clear();
132  upstream_borders_.clear();
133  progress_in_gate_ = 0.0;
134  remaining_in_gate_ = 0.0;
135  downstream_length_ = 0.0;
136  distance_to_current_ = 0.0;
137  valid_ = true;
138 
139  // std::cout<<"lane change view "<<(lc_direction_left_?"left":"right")<<":\n";
140 
141  auto gate_region = getGateRegion(current_lf, first_lf, last_lf);
142  if(gate_region.first==std::next(last_lf)||gate_region.second==std::next(last_lf))return;//no region has been found: done
143 
144  //measure distances
145  double d = 0.0;
146  for(Iterator it = gate_region.first;it!=std::next(gate_region.second);it++)
147  {
148  if(it==current_lf)
149  {
150  progress_in_gate_ = position_on_current + d;
151  }
152  d += (*it)->getLength();
153  if(it==gate_region.second)
154  {
156  }
157  }
158  d = 0.0;
159  for(Iterator it = first_lf;it!=std::next(last_lf);it++)
160  {
161  if(it==current_lf)break;
162  d += (*it)->getLength();
163  }
164  distance_to_current_ = d + position_on_current;
165  // std::cout<<"position_on_current="<<position_on_current<<std::endl;
166  // std::cout<<"progress_in_gate_="<<progress_in_gate_<<std::endl;
167  // std::cout<<"remaining_in_gate_="<<remaining_in_gate_<<std::endl;
168 
169 
170  int upstream_count = 0;
171  int gate_source_count = 0;
172  int gate_target_count = 0;
173  int downstream_count = 0;
174  //collect borders: upstream/source
175  for(Iterator it = first_lf;it!=gate_region.first;it++)
176  {
177  upstream_borders_.push_back(*it);
178  upstream_count ++;
179  }
180  //collect borders: gate/source+target
181  for(Iterator it = gate_region.first;it!=std::next(gate_region.second);it++)
182  {
183  gate_source_borders_.push_back(*it);
184  gate_source_count ++;
185  auto adjacent_lane = borderSet_->getLaneChangeTarget(*it,lc_direction_left_);
186  if(adjacent_lane.first==nullptr||adjacent_lane.second==nullptr)return;
187  gate_target_borders_.push_back(adjacent_lane.second);
188  gate_target_count ++;
189  }
190  //collect borders: downstream/target
191  double max_distance = (std::max)(0.0,lookahead_ - remaining_in_gate_ + gate_target_borders_.back()->getLength());
194  Border* b;
195  bool inverted = false;
196  bas.getNextBorder(b,inverted);
197  for(bas.getNextBorder(b,inverted);b!=nullptr;bas.getNextBorder(b,inverted))
198  {
199  downstream_borders_.push_back(b);
201  downstream_count ++;
202  }
203  // std::cout<<"upstream_count="<<upstream_count<<std::endl;
204  // std::cout<<"gate_source_count="<<gate_source_count<<std::endl;
205  // std::cout<<"gate_target_count="<<gate_target_count<<std::endl;
206  // std::cout<<"downstream_count="<<downstream_count<<std::endl;
207  valid_ = true;
208  }
209 
222  template <typename Iterator>
223  std::pair<Iterator,Border*> findBestGateEntryPoint(Iterator first_lf, Iterator last_lf)
224  {
225  std::pair<Iterator,Border*> result = std::make_pair(last_lf,nullptr);
227  for(Iterator current = first_lf;current!=std::next(last_lf,1);current++)
228  {
229  Border* currentBorder = *current;
230  auto adjacent_lane = borderSet_->getLaneChangeTarget(currentBorder,lc_direction_left_);
231  if(adjacent_lane.first==nullptr||adjacent_lane.second==nullptr)
232  {
233  continue;
234  }
235  if(!borderSet_->borderTypeValid(adjacent_lane.second))
236  {
237  continue;
238  }
239 
240  auto cost_query = borderCostMap_->find(adjacent_lane.second->m_id);
242  if(!(cost_query==borderCostMap_->end()))
243  {
244  cost = cost_query->second;
245  }
246  //for available navigation cost, take the first gate for tie-braking
247  //for unavailable navigation cost, replace initializer and use last gate (avoid lane changes)
248  if(cost<best_cost || (cost==best_cost && cost.getCombinedCost()==adore::env::NavigationCost::maximum_cost()))
249  {
250  result = std::make_pair(current,adjacent_lane.second);
251  best_cost = cost;
252  }
253  }
254  return result;
255  }
256 
268  template<typename Iterator>
269  std::pair<Iterator,Iterator> getGateRegion(Iterator current_lf, Iterator first_lf, Iterator last_lf)
270  {
271  auto best_gate = findBestGateEntryPoint<Iterator>(current_lf,last_lf);//best_gate: (source lane iterator, target lane Border*)
272  if(best_gate.second==nullptr)return std::make_pair(std::next(last_lf,1),std::next(last_lf,1));//no gate available
273  auto gate_start = best_gate;
274  for(Iterator candidate = std::prev(best_gate.first);candidate!=std::prev(first_lf);candidate--)//search upstream of best_gate entry point
275  {
276  auto lct = borderSet_->getLaneChangeTarget(*candidate,lc_direction_left_);
277  if(lct.first==nullptr||lct.second==nullptr)break;//no lane change target -> beyond start of gate
278  if(!borderSet_->borderTypeValid(lct.second))break;// lane change target invalid -> beyond start of gate
279  if(lct.second->isContinuousPredecessorOf(gate_start.second))
280  {
281  gate_start.first = candidate;
282  gate_start.second = lct.second;
283  }
284  else
285  {
286  break;
287  }
288  }
289  auto gate_end = best_gate;
290  for(Iterator candidate = std::next(best_gate.first);candidate!=std::next(last_lf);candidate++)//search downstream of best_gate entry point
291  {
292  auto lct = borderSet_->getLaneChangeTarget(*candidate,lc_direction_left_);
293  if(lct.first==nullptr||lct.second==nullptr)break;//no lane change target -> beyond end of gate
294  if(!borderSet_->borderTypeValid(lct.second))break;// lane change target invalid -> beyond start of gate
295  if(lct.second->isContinuousSuccessorOf(gate_end.second))
296  {
297  gate_end.first = candidate;
298  gate_end.second = lct.second;
299  }
300  else
301  {
302  break;
303  }
304  }
305  return std::make_pair(gate_start.first,gate_end.first);
306  }
307 
308 };
309 }
310 }
311 }
This class chooses the successor with the lowest cost until an upper limit on distance is reached.
Definition: borderaccumulator.h:136
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
Definition: bordercostmap.h:31
efficiently store borders in boost R-tree
Definition: borderset.h:99
std::pair< Border *, Border * > getLaneChangeTarget(Border *current_right, bool direction_left)
computes a pair of borders (left,right) suitable for lane-changing from given source lane
Definition: borderset.h:1336
bool borderTypeValid(Border *b)
check whether border type is in allowed types of set
Definition: borderset.h:204
Selects Borders from BorderSet required for LaneChangeView construction.
Definition: lanechangeborders.h:30
std::pair< Iterator, Iterator > getGateRegion(Iterator current_lf, Iterator first_lf, Iterator last_lf)
computes the gate region for a lane change Supply iterators for a list or vector of Border*,...
Definition: lanechangeborders.h:269
void setLookAhead(double value)
Definition: lanechangeborders.h:85
std::vector< Border * > gate_target_borders_
Definition: lanechangeborders.h:44
bool lc_direction_left_
Definition: lanechangeborders.h:34
void update(double position_on_current, Iterator current_lf, Iterator first_lf, Iterator last_lf)
collects all borders relevant for lane change view in object-variable vectors
Definition: lanechangeborders.h:127
double downstream_length_
Definition: lanechangeborders.h:39
double getRemainingInGate()
Definition: lanechangeborders.h:69
bool isLCDirectionLeft()
Definition: lanechangeborders.h:57
std::vector< Border * > gate_source_borders_
Definition: lanechangeborders.h:45
double lookahead_
Definition: lanechangeborders.h:35
bool isValid()
Definition: lanechangeborders.h:61
double getLookAhead()
Definition: lanechangeborders.h:81
double getLookBehind()
Definition: lanechangeborders.h:89
BorderCostMap * borderCostMap_
Definition: lanechangeborders.h:33
double getNavCostIncrease()
Definition: lanechangeborders.h:101
bool continueOnIncreasingCost_
Definition: lanechangeborders.h:42
double progress_in_gate_
Definition: lanechangeborders.h:37
BorderSet * borderSet_
Definition: lanechangeborders.h:32
std::vector< Border * > upstream_borders_
Definition: lanechangeborders.h:47
LaneChangeBorders(bool lc_direction_left, BorderSet *borderSet, BorderCostMap *borderCostMap)
Definition: lanechangeborders.h:49
std::vector< Border * > downstream_borders_
Definition: lanechangeborders.h:46
double lookbehind_
Definition: lanechangeborders.h:36
double getDistanceToCurrent()
Definition: lanechangeborders.h:77
double distance_to_current_
Definition: lanechangeborders.h:40
void setConinueOnIncreasingCost(bool value)
Definition: lanechangeborders.h:97
bool valid_
Definition: lanechangeborders.h:41
double getProgressInGate()
Definition: lanechangeborders.h:65
double remaining_in_gate_
Definition: lanechangeborders.h:38
std::pair< Iterator, Border * > findBestGateEntryPoint(Iterator first_lf, Iterator last_lf)
computes the best gate entry point for a lane change Supply iterators for lists or vectors of Border*...
Definition: lanechangeborders.h:223
void setLookBehind(double value)
Definition: lanechangeborders.h:93
double getDownstreamLength()
Definition: lanechangeborders.h:73
adoreMatrix< T, N, M > max(adoreMatrix< T, N, M > a, const adoreMatrix< T, N, M > &b)
Definition: adoremath.h:686
Definition: areaofeffectconverter.h:20
The border struct contains data of the smallest.
Definition: border.h:62
double getLength()
Get the length of the border.
Definition: border.h:703
Struct to organize navigation cost.
Definition: navigationcost.h:26
static double maximum_cost()
Definition: navigationcost.h:30
double getCombinedCost()
Get combined cost.
Definition: navigationcost.h:76