ADORe
ADORe is a modular open source software library and toolkit for decision making, planning, control and simulation of automated vehicles
ocroadbasedprediction_test.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 and API
13  ********************************************************************************/
14 #pragma once
15 
18 #include <unordered_set>
19 #include <vector>
20 
21 namespace adore
22 {
23  namespace env
24  {
45  class OCRoadBasedPrediction: public OCPredictionStrategy<traffic::Participant>
46  {
47  private:
48  double t_max_utc_;
49  double lat_precision_;
50  double lat_error_;
51  double lon_error_;
52  double v_max_;
53  double a_max_;
54  double a_min_;
55  double angle_error_max_;
56  double time_headway_;
57  double time_leeway_;
59  struct SearchState
60  {
61  public:
62  double s0_border_;
63  double s0_participant_;
65  int predecessorID_;
66  };
67 
68  public:
69  void setAngleErrorMax(double value){angle_error_max_=value;}
70  void setTMaxUTC(double value){t_max_utc_=value;}
71  void setLatPrecision(double value){lat_precision_=value;}
72  void setLatError(double value){lat_error_=value;}
73  void setLonError(double value){lon_error_=value;}
74  void setVMax(double value){v_max_=value;}
75  void setAMax(double value){a_max_=value;}
76  void setAMin(double value){a_min_=value;}
77  void setTimeHeadway(double value){time_headway_=value;}
78  void setTimeLeeway(double value){time_leeway_=value;}
80  :t_max_utc_(0.0),lat_precision_(0.2),lat_error_(0.0),lon_error_(0.0),
81  v_max_(9999.9),a_max_(0.0),a_min_(0.0),
82  time_headway_(2.0),
83  time_leeway_(1.0),
84  trafficMap_(trafficMap){}
85  virtual bool predict(const traffic::Participant& p, OccupancyCylinderPredictionSet& set)const override
86  {
87  const double b = p.getWidth()*0.5 + lat_error_;
88  const double r = b+lat_precision_;
89  const double ds = 2.0 * std::sqrt(r*r-b*b);
90  const double t0 = p.getObservationTime();
91  const double t1 = (std::max)(t0,t_max_utc_);
92  const double dt = t1-t0;
93  const double v = std::sqrt(p.getVx()*p.getVx()+p.getVy()*p.getVy());
94  const double l = p.getLength() + 2.0*lon_error_;
95  const double k = (int)std::ceil(l/ds);
96  const double s0 = -k*ds*0.5 + 0.5*ds;
97  const double s1 = +k*ds*0.5 + v*dt + 0.5*a_max_*dt*dt - 0.5*ds;
98  const double dx = std::cos(p.getYaw());
99  const double dy = std::sin(p.getYaw());
100  auto pos0 = p.getCenter();
103  RampPrediction ramp;
104  ramp.predict_s_tva(-l*0.5,v,a_min_,a_min_<0.0?0.0:v_max_,t0,0.1,t1,rear_end_s_tva);//lower bound: back of behicle with amin
105  ramp.predict_s_tva(+l*0.5,v,a_max_,a_max_<0.0?0.0:v_max_,t0,0.1,t1,front_end_s_tva);//upper bound: front of vehicle with amax
106 
107  //prediction for current shape and position
108  {
109  OccupancyCylinderPrediction prediction;
110  int predictionID = set.size();
112  center.getData()(1,0) = pos0(0)-l*0.5*dx;
113  center.getData()(2,0) = pos0(1)-l*0.5*dy;
114  center.getData()(3,0) = pos0(2);
115  center.getData()(0,1) = l;
116  center.getData()(1,1) = pos0(0)+l*0.5*dx;
117  center.getData()(2,1) = pos0(1)+l*0.5*dy;
118  center.getData()(3,1) = pos0(2);
119  // if(center.getData().nc()<2)continue;
120  //create a new prediction for the encountered border
121  prediction.trackingID_ = p.getTrackingID();
122  prediction.branchID_ = predictionID;
123  prediction.predecessorID_ = -1;
124  prediction.confidence_ = 1.0;
125  for(double s = ds*0.5;s<=l-ds*0.5;s+=ds)
126  {
127  auto pos = center.f(s);
128  double t0i,t1i;
129  //first time entering circle with front of vehicle
130  t0i = front_end_s_tva.f(adore::mad::bound(front_end_s_tva.limitLo(),s-ds*0.5,front_end_s_tva.limitHi()))(0);
131  t0i -= time_headway_;
132  //latest time exiting circle with back of vehicle
133  t1i = rear_end_s_tva.f(adore::mad::bound(rear_end_s_tva.limitLo(),s+ds*0.5,rear_end_s_tva.limitHi()))(0);
134  t1i += time_leeway_;
135  if(!std::isnan(r)&&!std::isnan(pos(0))&&!std::isnan(pos(1))&&!std::isnan(t0)&&!std::isnan(t1))
136  {
137  prediction.occupancy_.insert(adore::mad::OccupancyCylinder(r,pos(0),pos(1),t0i,t1i));
138  }
139  }
140  set.push_back(prediction);
141  }
142 
143 
144  //predict based on road-network
145  std::unordered_set<adore::env::BorderBased::BorderID,adore::env::BorderBased::BorderIDHasher> closed;
146  std::vector<SearchState> open;
147  //initialize open
148  for(auto itpair = trafficMap_->getParticipantToBorder().equal_range(p.getTrackingID());
149  itpair.first!=itpair.second;
150  itpair.first++)
151  {
152  auto it = itpair.first;
153  auto test_participantID = it->first;
154  auto borderID = it->second.first;
155  auto positioning = it->second.second;
156  if(test_participantID==p.getTrackingID() && positioning.anyInside())//start on border, if any point of vehicle is inside
157  {
158  SearchState x;
159  x.s0_border_ = positioning.s;
160  x.s0_participant_ = 0.0;
161  x.predecessorID_ = -1;
162  x.borderID_ = borderID;
163  //angle offset
164  //find two points on right border
165  auto bright = trafficMap_->getBorderSet()->getBorder(borderID);
166  if(bright==nullptr)continue;//no border found: prediction impossible
167  //compute orientation information:
168  const double lah = (std::max)(1.0,l*0.5);//look ahead/behind
169  if(positioning.s>=bright->m_path->limitHi()-0.1)continue;
170  const auto p0 = bright->m_path->f_bounded(positioning.s-lah);
171  const auto p1 = bright->m_path->f_bounded(positioning.s+lah);
172  const double pdx = p1(0)-p0(0); //path delta x
173  const double pdy = p1(1)-p0(1); //path delta y
174  if(pdx*pdx+pdy*pdy<0.01)continue;//very short border?
175  const double rpdx = pdx*dx + pdy*dy;//path delta x rotated in vehicle coordinates
176  const double rpdy = -pdx*dy + pdy*dx;//path delta y rotated in vehicle coordinates
177  if((std::abs)((std::atan2)(rpdy,rpdx))>angle_error_max_)continue;//orientation is wrong do not start on this border
178  open.push_back(x);
179  }
180  }
181  int branch_count = 0;
182  //process search states in open (depth first)
183  while(open.size()>0)
184  {
185  int predictionID = set.size();
186  SearchState x = open.back();
187  open.pop_back();
188  if(closed.find(x.borderID_)!=closed.end())continue;//do not explore twice
189  closed.insert(x.borderID_);
190  //find the center
192  auto right_border = trafficMap_->getBorderSet()->getBorder(x.borderID_);
194  // if(center.getData().nc()<2)continue;
195  //create a new prediction for the encountered border
196  OccupancyCylinderPrediction prediction;
197  prediction.trackingID_ = p.getTrackingID();
198  prediction.branchID_ = predictionID;
199  prediction.predecessorID_ = x.predecessorID_;
200  prediction.confidence_ = 1.0;
201  const double sx0 = x.s0_participant_;
202  const double dsmax = center.limitHi()-x.s0_border_;
203  const double sx1 = std::min(s1,sx0+dsmax);
204  const double s_max_border = center.limitHi();
205  if(center.getData().nc()>=2)for(double s = sx0;s<=sx1;s+=ds)
206  {
207  auto pos = center.f(s+x.s0_border_-x.s0_participant_);
208  auto pos_right = right->f(s+x.s0_border_-x.s0_participant_);
209  double r_dynamic = sqrt((pos(0)-pos_right(0))*(pos(0)-pos_right(0)) + (pos(1)-pos_right(1))*(pos(1)-pos_right(1)));
210 
211  // maximum between the r_dynamic and r
212 
213  double t0i,t1i;
214  //first time entering circle with front of vehicle
215  t0i = front_end_s_tva.f(adore::mad::bound(front_end_s_tva.limitLo(),s-ds*0.5,front_end_s_tva.limitHi()))(0);
216  t0i -= time_headway_;
217  //latest time exiting circle with back of vehicle
218  t1i = rear_end_s_tva.f(adore::mad::bound(rear_end_s_tva.limitLo(),s+ds*0.5,rear_end_s_tva.limitHi()))(0);
219  t1i += time_leeway_;
220  if(!std::isnan(r_dynamic)&&!std::isnan(pos(0))&&!std::isnan(pos(1))&&!std::isnan(t0)&&!std::isnan(t1))
221  {
222  prediction.occupancy_.insert(adore::mad::OccupancyCylinder(r_dynamic,pos(0),pos(1),t0i,t1i));
223  }
224  }
225  set.push_back(prediction);
226 
227  if(sx1<s1)
228  {
229  //find all successors of the border and add them to the open list
230  auto border = trafficMap_->getBorderSet()->getBorder(x.borderID_);
231  for(auto it = trafficMap_->getBorderSet()->getSuccessors(border);
232  it.current()!=it.end();
233  it.current()++)
234  {
235  auto nextBorder = it.current()->second;
236  if(border->isContinuousPredecessorOf(nextBorder))
237  {
238  SearchState xnext;
239  xnext.s0_border_ = 0.0;
240  xnext.s0_participant_ = sx1;
241  xnext.predecessorID_ = predictionID;
242  xnext.borderID_ = nextBorder->m_id;
243  open.push_back(xnext);
244  }
245 
246  }
247  }
248  predictionID++;
249  branch_count++;
250  }
251 
252  return branch_count>0;
253  }
254 
255 
256  };
257  }
258 }
adore::mad::LLinearPiecewiseFunctionM< double, 4 > getCenterline(const BorderID &id)
get the linear piecewise description of the centerline:
Definition: borderset.h:647
itCoordinate2Border getSuccessors(Border *b)
get an interator pair for all borders which follow after b
Definition: borderset.h:996
Border * getBorder(const BorderID &id) const
retrieve a border by ID
Definition: borderset.h:628
void setAngleErrorMax(double value)
Definition: ocroadbasedprediction_test.h:69
double time_headway_
Definition: ocroadbasedprediction.h:56
void setVMax(double value)
Definition: ocroadbasedprediction_test.h:74
void setAMax(double value)
Definition: ocroadbasedprediction_test.h:75
double lat_error_
Definition: ocroadbasedprediction.h:50
double t_max_utc_
Definition: ocroadbasedprediction.h:48
double v_max_
Definition: ocroadbasedprediction.h:52
void setAMin(double value)
Definition: ocroadbasedprediction_test.h:76
void setTimeHeadway(double value)
Definition: ocroadbasedprediction_test.h:77
OCRoadBasedPrediction(adore::env::traffic::TrafficMap *trafficMap)
Definition: ocroadbasedprediction_test.h:79
double a_min_
Definition: ocroadbasedprediction.h:54
void setLonError(double value)
Definition: ocroadbasedprediction_test.h:73
double lat_precision_
Definition: ocroadbasedprediction.h:49
void setLatError(double value)
Definition: ocroadbasedprediction_test.h:72
double angle_error_max_
Definition: ocroadbasedprediction.h:55
void setTimeLeeway(double value)
Definition: ocroadbasedprediction_test.h:78
double a_max_
Definition: ocroadbasedprediction.h:53
void setTMaxUTC(double value)
Definition: ocroadbasedprediction_test.h:70
double time_leeway_
Definition: ocroadbasedprediction.h:57
double lon_error_
Definition: ocroadbasedprediction.h:51
void setLatPrecision(double value)
Definition: ocroadbasedprediction_test.h:71
adore::env::traffic::TrafficMap * trafficMap_
Definition: ocroadbasedprediction.h:63
virtual bool predict(const traffic::Participant &p, OccupancyCylinderPredictionSet &set) const override
Definition: ocroadbasedprediction_test.h:85
Definition: velocityprediction.h:22
void predict_s_tva(double s0, double v0, double a0, double delay, double a1, double vbound, double t0, double dt, double t1, adore::mad::LLinearPiecewiseFunctionM< double, 3 > &result)
Definition: velocityprediction.h:24
Definition: trafficmap.h:36
adore::env::BorderBased::BorderSet * getBorderSet()
Get the border set.
Definition: trafficmap.h:63
const TParticipantToBorder & getParticipantToBorder() const
Get the participant to border map.
Definition: trafficmap.h:81
virtual CT f(DT x) const override
Definition: llinearpiecewisefunction.h:251
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
void insert(const VolumeType &volume)
Definition: vectorbasedvolumetree.h:90
std::vector< OccupancyCylinderPrediction > OccupancyCylinderPredictionSet
Definition: occupancycylinderprediction.h:40
@ right
Definition: indicator_hint.h:36
interval< T > atan2(interval< T > y, interval< T > x)
Definition: intervalarithmetic.h:234
interval< T > cos(interval< T > x)
Definition: intervalarithmetic.h:225
T bound(T lb, T value, T ub)
Definition: adoremath.h:569
T min(T a, T b, T c, T d)
Definition: adoremath.h:663
void set(T *data, T value, int size)
Definition: adoremath.h:39
interval< T > sin(interval< T > x)
Definition: intervalarithmetic.h:204
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
p0
Definition: adore_set_pose.py:32
r
Definition: adore_suppress_lanechanges.py:209
Definition: areaofeffectconverter.h:20
This struct identifies a Border by the coordinates of the starting and the end point.
Definition: borderid.h:31
T1 & current()
Definition: borderset.h:50
Definition: ocroadbasedprediction.h:65
double s0_border_
Definition: ocroadbasedprediction.h:67
int predecessorID_
Definition: ocroadbasedprediction.h:70
double s0_participant_
Definition: ocroadbasedprediction.h:68
adore::env::BorderBased::BorderID borderID_
Definition: ocroadbasedprediction.h:69
Definition: occupancycylinderprediction.h:27
int branchID_
Definition: occupancycylinderprediction.h:31
int trackingID_
Definition: occupancycylinderprediction.h:30
adore::mad::OccupancyCylinderTree occupancy_
Definition: occupancycylinderprediction.h:33
float confidence_
Definition: occupancycylinderprediction.h:34
int predecessorID_
Definition: occupancycylinderprediction.h:32
Struct for representing a participant in traffic.
Definition: participant.h:30
double getLength() const
Definition: participant.h:155
TTrackingID getTrackingID() const
Definition: participant.h:109
double getObservationTime() const
Definition: participant.h:112
const adoreMatrix< double, 3, 1 > & getCenter() const
Definition: participant.h:124
double getYaw() const
Definition: participant.h:154
double getWidth() const
Definition: participant.h:156
double getVy() const
Definition: participant.h:158
double getVx() const
Definition: participant.h:157
Definition: occupancycylinder.h:25