ADORe
ADORe is a modular open source software library and toolkit for decision making, planning, control and simulation of automated vehicles
area_of_effect_provider.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 implementation
13  ********************************************************************************/
14 
15 
16 #pragma once
17 
18 #include <adore/env/afactory.h>
21 #include <adore/params/afactory.h>
22 
23 
24 namespace adore
25 {
26 namespace apps
27 {
33 {
34  private:
42  double resolution_;
43  double lookbehind_;
44  std::vector<double> s_;
45  public:
46  AreaOfEffectProvider(double resolution = 1.0):
47  resolution_(resolution),lookbehind_(10.0)
48  {
49  for(int i=0;i<=13;i++)s_.push_back(0.0);//initialize with zeros
50  writer_aoe_ = EnvFactory::get()->getAreaOfEffectWriter();//initialize output writer
51  writer_aoi_ = EnvFactory::get()->getAreaOfInterestWriter();//initialize output writer
52  xreader_ = EnvFactory::get()->getVehicleMotionStateReader();//initialize vehicle state reader
53  params_ = ParamsFactory::get()->getPrediction();//initialize prediction parameters
54  }
55  void run()
56  {
57  if(!xreader_->hasData())return;
59  xreader_->getData(x);
62  adore::view::ALane* source = view->getCurrentLane();
63  adore::view::ALane* leftTarget = nullptr;
64  adore::view::ALane* rightTarget = nullptr;
65  if( view->getRightLaneChange()!=nullptr
66  && view->getRightLaneChange()->getTargetLane()->isValid())
67  {
68  rightTarget = view->getRightLaneChange()->getTargetLane();
69  }
70  if( view->getLeftLaneChange()!=nullptr
71  && view->getLeftLaneChange()->getTargetLane()->isValid())
72  {
73  leftTarget = view->getLeftLaneChange()->getTargetLane();
74  }
75 
76 
77  adore::env::AreaOfEffect aoe;//area of effect
78  adore::env::AreaOfEffect aoi;//reduced size, area of interest
79  double n_shrink_aoi = params_->get_area_of_interest_shrink();//size in m to reduce by
80  double n_shrink_aoe = params_->get_area_of_effect_shrink();//size in m to reduce by
81  if(view->getCurrentLane()->isValid())
82  {
83  computeSValues(&three_lanes_,x.getX(),x.getY());
84  sampleLane(source,s_[0],s_[1],aoe,n_shrink_aoe);
85  sampleLane(source,s_[0],s_[1],aoi,n_shrink_aoi);
86  if(rightTarget!=nullptr && rightTarget->isValid())
87  {
88  sampleLane(rightTarget,s_[2],s_[4],aoe,n_shrink_aoe);
89  sampleLane(rightTarget,s_[4],s_[5],aoe,n_shrink_aoe);
90  sampleLane(rightTarget,s_[2],s_[4],aoi,n_shrink_aoi);
91  sampleLane(rightTarget,s_[4],s_[5],aoi,n_shrink_aoi);
92  }
93  sampleLane(source,s_[6],s_[7],aoe,n_shrink_aoe);
94  sampleLane(source,s_[7],s_[8],aoe,n_shrink_aoe);
95  sampleLane(source,s_[6],s_[7],aoi,n_shrink_aoi);
96  sampleLane(source,s_[7],s_[8],aoi,n_shrink_aoi);
97  if(leftTarget!=nullptr && leftTarget->isValid())
98  {
99  sampleLane(leftTarget,s_[9],s_[10],aoe,n_shrink_aoe);
100  sampleLane(leftTarget,s_[10],s_[12],aoe,n_shrink_aoe);
101  sampleLane(leftTarget,s_[9],s_[10],aoi,n_shrink_aoi);
102  sampleLane(leftTarget,s_[10],s_[12],aoi,n_shrink_aoi);
103  }
104  sampleLane(source,s_[13],s_[0],aoe,n_shrink_aoe);
105  sampleLane(source,s_[13],s_[0],aoi,n_shrink_aoi);
106  }
107  writer_aoe_->write(aoe);
108  writer_aoi_->write(aoi);
109  }
110 
111  void computeSValues(adore::view::AThreeLaneView* view,double X0,double Y0)
112  {
113  double X,Y,Z,s,n;
114  auto source = view->getCurrentLane();
115  source->toRelativeCoordinates(X0,Y0,s,n);
116  double smin = std::max(s-lookbehind_,source->getSMin());
117  //ccw list of s values
118  s_[0] = smin;
119  s_[1] = s_[0];
120  s_[2] = 0.0;
121  s_[3] = 0.0;
122  s_[4] = 0.0;
123  s_[5] = 0.0;
124  s_[6] = s_[0];
125  s_[7] = source->getSMax();
126  s_[8] = s_[7];
127  s_[9] = 0.0;
128  s_[10] = 0.0;
129  s_[11] = 0.0;
130  s_[12] = 0.0;
131  s_[13] = s_[7];
132  if( view->getRightLaneChange()!=nullptr
133  && view->getRightLaneChange()->getTargetLane()->isValid())
134  {
135  auto lc = view->getRightLaneChange();
136  auto target = lc->getTargetLane();
137 
138  s_[1] = std::max(smin,lc->getProgressOfGateOpen());
139  //s2: translate gate open to target lane progress
140  source->toEucledianCoordinates(s_[1],source->getOffsetOfRightBorder(s_[1]),X,Y,Z);
141  target->toRelativeCoordinates(X,Y,s,n);
142  s_[2] = s;
143 
144  target->toRelativeCoordinates(X0,Y0,s,n);
145  s_[3] = std::max(s-lookbehind_,target->getSMin());
146  s_[4] = target->getSMax();
147 
148 
149  s_[6] = std::max(smin,lc->getProgressOfGateClosed());
150  //s5: translate gate closed to target lane progress
151  source->toEucledianCoordinates(s_[6],source->getOffsetOfRightBorder(s_[6]),X,Y,Z);
152  target->toRelativeCoordinates(X,Y,s,n);
153  s_[5] = s;
154  }
155  if( view->getLeftLaneChange()!=nullptr
156  && view->getLeftLaneChange()->getTargetLane()->isValid())
157  {
158  auto lc = view->getLeftLaneChange();
159  auto target = lc->getTargetLane();
160 
161  s_[13] = std::max(smin,lc->getProgressOfGateOpen());
162  //s12: translate gate open to target lane progress
163  source->toEucledianCoordinates(s_[13],source->getOffsetOfLeftBorder(s_[13]),X,Y,Z);
164  target->toRelativeCoordinates(X,Y,s,n);
165  s_[12] = s;
166 
167  target->toRelativeCoordinates(X0,Y0,s,n);
168  s_[11] = std::max(s-lookbehind_,target->getSMin());
169  s_[10] = target->getSMax();
170 
171 
172  s_[8] = std::max(smin,lc->getProgressOfGateClosed());
173  //s9: translate gate closed to target lane progress
174  source->toEucledianCoordinates(s_[8],source->getOffsetOfLeftBorder(s_[8]),X,Y,Z);
175  target->toRelativeCoordinates(X,Y,s,n);
176  s_[9] = s;
177  }
178 
179  }
180 
181  void sampleLane(adore::view::ALane* lane,double s0,double s1,adore::env::AreaOfEffect& area,double n_shrink_aoi = 0.0)
182  {
183  if(s0==s1)return;
184  bool inverted = s0>s1;
185  double n,X,Y,Z,lasts;
186  for(double s=s0;
187  (!inverted && s<=s1) || (inverted && s>=s1);
188  s+=(inverted?-1.0:1.0)*resolution_)
189  {
190  lasts = s;
191  n = inverted?lane->getOffsetOfLeftBorder(s)-n_shrink_aoi:lane->getOffsetOfRightBorder(s)+n_shrink_aoi;
192  lane->toEucledianCoordinates(s,n,X,Y,Z);
193  area.push_back(std::make_pair(X,Y));
194  }
195  if(lasts!=s1)
196  {
197  n = inverted?lane->getOffsetOfLeftBorder(s1)-n_shrink_aoi:lane->getOffsetOfRightBorder(s1)+n_shrink_aoi;
198  lane->toEucledianCoordinates(s1,n,X,Y,Z);
199  area.push_back(std::make_pair(X,Y));
200  }
201  }
202 };
203 
204 }
205 }
Definition: area_of_effect_provider.h:33
adore::env::AFactory::TVehicleMotionStateReader * xreader_
Definition: area_of_effect_provider.h:39
adore::params::APPrediction * params_
Definition: area_of_effect_provider.h:41
void run()
Definition: area_of_effect_provider.h:55
adore::env::ThreeLaneViewDecoupled three_lanes_
Definition: area_of_effect_provider.h:40
std::vector< double > s_
Definition: area_of_effect_provider.h:44
void computeSValues(adore::view::AThreeLaneView *view, double X0, double Y0)
Definition: area_of_effect_provider.h:111
adore::env::AFactory::TAreaOfEffectWriter * writer_aoi_
Definition: area_of_effect_provider.h:38
adore::env::AFactory::TAreaOfEffectWriter * writer_aoe_
Definition: area_of_effect_provider.h:37
double lookbehind_
Definition: area_of_effect_provider.h:43
AreaOfEffectProvider(double resolution=1.0)
Definition: area_of_effect_provider.h:46
double resolution_
Definition: area_of_effect_provider.h:42
void sampleLane(adore::view::ALane *lane, double s0, double s1, adore::env::AreaOfEffect &area, double n_shrink_aoi=0.0)
Definition: area_of_effect_provider.h:181
virtual TAreaOfEffectWriter * getAreaOfInterestWriter()=0
virtual TVehicleMotionStateReader * getVehicleMotionStateReader()=0
virtual TAreaOfEffectWriter * getAreaOfEffectWriter()=0
Utility class to simplify factory access.
Definition: afactory.h:220
static adore::env::AFactory * get()
Definition: afactory.h:236
Definition: threelaneviewdecoupled.h:32
void update()
Definition: threelaneviewdecoupled.h:373
Definition: com_patterns.h:68
virtual void getData(T &value)=0
virtual bool hasData() const =0
Definition: com_patterns.h:97
virtual void write(const T &value)=0
virtual APPrediction * getPrediction() const =0
parameter interface for parameters related to prediction
Definition: ap_prediction.h:28
virtual double get_area_of_effect_shrink() const =0
filtering out all static objects not inside area of effect
virtual double get_area_of_interest_shrink() const =0
distinction between clutter and static traffic objects: how far into road has object to extend to be ...
Utility class to simplify factory access.
Definition: afactory.h:87
static adore::params::AFactory * get()
Definition: afactory.h:103
virtual ALane * getTargetLane()=0
Definition: alane.h:28
virtual void toRelativeCoordinates(double xe, double ye, double &s, double &n)=0
virtual double getOffsetOfRightBorder(double s)=0
virtual void toEucledianCoordinates(double s, double n, double &xe, double &ye, double &ze)=0
virtual bool isValid() const =0
virtual double getOffsetOfLeftBorder(double s)=0
Definition: athreelaneview.h:30
virtual ALane * getCurrentLane()=0
virtual ALaneChangeView * getLeftLaneChange()=0
virtual ALaneChangeView * getRightLaneChange()=0
std::vector< std::pair< double, double > > AreaOfEffect
Definition: areaofeffect.h:20
adoreMatrix< T, N, M > max(adoreMatrix< T, N, M > a, const adoreMatrix< T, N, M > &b)
Definition: adoremath.h:686
x
Definition: adore_set_goal.py:30
Definition: areaofeffectconverter.h:20
This struct holds the motion state of the vehicle in 9d.
Definition: vehiclemotionstate9d.h:39