ADORe
ADORe is a modular open source software library and toolkit for decision making, planning, control and simulation of automated vehicles
basicsetpointrequestevaluators.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ß - collision detection, navigation cost, acceleration costs
13  ********************************************************************************/
14 
15 #pragma once
21 #include <adore/params/afactory.h>
22 
23 
24 namespace adore
25 {
26  namespace fun
27  {
32  {
33  private:
38  public:
43  )
44  :predictionView_(predictionView),pvehicle_(pvehicle),ptac_(ptac),ppred_(ppred)
45  {}
46  virtual bool isValid(const adore::fun::SetPointRequest& spr)const override
47  {
48  double front_buffer_space = ptac_->getCollisionDetectionFrontBufferSpace();
49  double lateral_precision = ptac_->getCollisionDetectionLateralPrecision();
50  SetPointRequestSwath spr_swath(
51  pvehicle_->get_a()+pvehicle_->get_b()+pvehicle_->get_c()+pvehicle_->get_d()+front_buffer_space,
53  pvehicle_->get_d(),//spr reference point at rear axle
54  lateral_precision);
59  spr_swath.append_cylinder_swath_linear(spr,space,true);
61  }
62  };
63 
68  {
69  private:
74  public:
79  )
80  :predictionView_(predictionView),pvehicle_(pvehicle),ptac_(ptac),ppred_(ppred)
81  {}
82  virtual bool isValid(const adore::fun::SetPointRequest& spr)const override
83  {
84  double front_buffer_space = ptac_->getCollisionDetectionFrontBufferSpace();
85  double lateral_precision = ptac_->getCollisionDetectionLateralPrecision();
86  SetPointRequestSwath spr_swath(
87  pvehicle_->get_a()+pvehicle_->get_b()+pvehicle_->get_c()+pvehicle_->get_d()+front_buffer_space,
89  pvehicle_->get_d(),//spr reference point at rear axle
90  lateral_precision);
96  spr_swath.append_cylinder_swath_linear(spr,space,true);
98  }
99  std::string getName()const{return "coercion";}
100  };
101 
106  class SPRTTCNominal: public ASPRCost
107  {
108  private:
111  public:
114  :predictionView_(predictionView),pvehicle_(pvehicle)
115  {}
116  virtual std::string getName()const override{return "TTC_nom";}
117  virtual double getCost(const adore::fun::SetPointRequest& spr)const override
118  {
119  if(spr.setPoints.size()==0)return 0.0;
120  double t0 = spr.setPoints.begin()->tStart;
121  double ttc_max = 10.0;
122  double tmax = t0 + ttc_max;
123  double front_buffer_space = 4.0;
124  double lateral_precision = 0.05;
125  SetPointRequestSwath spr_swath(
126  pvehicle_->get_a()+pvehicle_->get_b()+pvehicle_->get_c()+pvehicle_->get_d()+front_buffer_space,
128  pvehicle_->get_d(),//spr reference point at rear axle
129  lateral_precision);
130  spr_swath.setLonError(0.0);
131  spr_swath.setLatError(0.0);
133  spr_swath.append_cylinder_swath_linear(spr,space);
134  double tmin = predictionView_->getExpectedCollisionTime(space,tmax);
135  double ttc = std::max(0.0,tmin-t0);
136  double cost = (ttc_max-ttc)*(ttc_max-ttc)/ttc_max/ttc_max;
137  return cost;
138  // return ttc;
139  }
140 
141  };
142 
143 
148  {
149  private:
151 
152  public:
154  virtual std::string getName()const override{return "AverageProgressLoss";}
155  virtual double getCost(const adore::fun::SetPointRequest& spr)const override
156  {
157  const double tmax = 10.0;
158  double vmax = std::max(1.0,params_->getGlobalSpeedLimit());
159  double distance = 0.0;
160  double time = 0.0;
161  for(auto& sp:spr.setPoints)
162  {
163  double dt = sp.tEnd-sp.tStart;
164  double v = sp.x0ref.getvx();
165  distance += v*dt;
166  time += dt;
167  if(time>tmax)break;
168  }
169  return std::max(0.0,1.0 - distance/(vmax*tmax));
170  }
171 
172  };
173 
178  {
179  private:
183  int laneID_;
184 
185  public:
187  :threeLaneView_(threeLaneView),params_(params),max_overshoot_(50.0),laneID_(laneID)
188  {}
189  virtual std::string getName()const override{return "MinimumNavigationCostOnLane";}
190  virtual double getCost(const adore::fun::SetPointRequest& spr)const override
191  {
192  try{
193  return nav_cost_of_lane(spr,laneID_);
194  }
195  catch(const std::exception& e)
196  {
197  std::cout<<e.what();
198  // throw(e);
199  return 1.0;
200  }
201  }
206  double nav_cost_of_lane(const adore::fun::SetPointRequest& spr,int laneid)const
207  {
208  adore::view::ALane* lane=nullptr;
209  switch(laneid)
210  {
211  case 0:
212  lane = threeLaneView_->getCurrentLane();
213  break;
214  case 1:
215  if(threeLaneView_->getLeftLaneChange()!=nullptr
218  {
220  }
221  break;
222  case -1:
223  if(threeLaneView_->getRightLaneChange()!=nullptr
226  {
228  }
229  break;
230  }
231  if(lane==nullptr)return 1.0e99;
232  double X0 = spr.setPoints.begin()->x0ref.getX();
233  double Y0 = spr.setPoints.begin()->x0ref.getY();
234  double s0,n0,p0;
235  lane->toRelativeCoordinates(X0,Y0,s0,n0);
236 
237  try
238  {
239  double interval_start = adore::mad::bound(lane->getSMin(),s0-max_overshoot_,lane->getSMax());
240  double interval_end = lane->getSMax();
241  double tmp;lane->boundNavigationCost(interval_start,interval_end,p0,tmp);
242  return p0;
243  }
244  catch(const std::exception& e)
245  {
246  std::cerr << e.what() << '\n';
247  }
248 
249  return 1e99;
250  }
251  };
256  {
257  private:
261  int laneID_;
262 
263  public:
265  :threeLaneView_(threeLaneView),params_(params),max_overshoot_(50.0),laneID_(laneID)
266  {}
267  virtual std::string getName()const override{return "NormalizedNavigationCost";}
268  virtual double getCost(const adore::fun::SetPointRequest& spr)const override
269  {
270  try{
272  }
273  catch(const std::exception& e)
274  {
275  std::cout<<e.what();
276  // throw(e);
277  return 1.0;
278  }
279  }
280 
285  double normalized_nav_cost_simple(const adore::fun::SetPointRequest& spr,int laneid)const
286  {
287  double fmax = 1.0;
288  if(spr.setPoints.size()==0)return fmax;
289  double vmax = params_->getGlobalSpeedLimit();
290  double amax = params_->getAccLonUB();
291  double v0 = std::abs(spr.setPoints.begin()->x0ref.getvx());
292  double t0 = spr.setPoints.begin()->tStart;
293  double t1 = spr.setPoints.rbegin()->tEnd;
294  double dt = t1-t0;
295  double ta = std::min(dt,std::abs(vmax-v0)/amax);
296  double optimal_progress = v0*ta + 0.5*amax*ta*ta + (dt-ta)*vmax;
297 
298  adore::view::ALane* lane=nullptr;
299  switch(laneid)
300  {
301  case 0:
302  lane = threeLaneView_->getCurrentLane();
303  break;
304  case 1:
305  if(threeLaneView_->getLeftLaneChange()!=nullptr
308  {
310  }
311  break;
312  case -1:
313  if(threeLaneView_->getRightLaneChange()!=nullptr
316  {
318  }
319  break;
320  }
321  if(lane==nullptr)return 1.0;
322  double X0 = spr.setPoints.begin()->x0ref.getX();
323  double Y0 = spr.setPoints.begin()->x0ref.getY();
324  double X1 = spr.setPoints.rbegin()->x0ref.getX();
325  double Y1 = spr.setPoints.rbegin()->x0ref.getY();
326  double s0,n0,s1,n1,p0,p1;
327  try
328  {
329  lane->toRelativeCoordinates(X0,Y0,s0,n0);
330  lane->toRelativeCoordinates(X1,Y1,s1,n1);
331  }
332  catch(const std::exception& e)
333  {
334  std::cerr << "error in coordinate transformation\n";
335  std::cerr << e.what() << '\n';
336  }
337 
338  try
339  {
340 
341  double interval_start = adore::mad::bound(0.0,s0-max_overshoot_,lane->getSMax());
342  double interval_end = adore::mad::bound(0.0,s0,lane->getSMax());
343  double tmp;lane->boundNavigationCost(interval_start,interval_end,p0,tmp);
344 
345  interval_start = adore::mad::bound(0.0,s0-max_overshoot_,lane->getSMax());
346  interval_end = adore::mad::bound(0.0,s1,lane->getSMax());//here: evaluate until s1
347  tmp;lane->boundNavigationCost(interval_start,interval_end,p1,tmp);
348 
349  double actual_progress = std::max(0.0,p0-p1);
350  return adore::mad::bound(0.0,(optimal_progress-actual_progress)/optimal_progress,1.0);
351  }
352  catch(const std::exception& e)
353  {
354  std::cerr << "error in cost bounding\n";
355  std::cerr << e.what() << '\n';
356  }
357 
358  return 1.0;
359  }
360 
361  };
362 
364  {
365  public:
366  virtual std::string getName()const override{return "LongitudinalAccelerationCost";}
367  virtual double getCost(const adore::fun::SetPointRequest& spr)const override
368  {
369  double value = 0.0;
370  for(const auto& setPoint : spr.setPoints)
371  {
372  value += (setPoint.tEnd-setPoint.tStart) * setPoint.x0ref.getAx()* setPoint.x0ref.getAx();
373  }
374  return value;
375  }
376  };
378  {
379  public:
380  virtual std::string getName()const override{return "LongitudinalJerkCost";}
381  virtual double getCost(const adore::fun::SetPointRequest& spr)const override
382  {
383  double value = 0.0;
384  for(const auto& setPoint : spr.setPoints)
385  {
386  value += (setPoint.tEnd-setPoint.tStart) * setPoint.x0ref.getDAx()* setPoint.x0ref.getDAx();
387  }
388  return value;
389  }
390  };
391 
393  {
394  private:
395  std::vector<adore::fun::ASPRCost*> c_;
396  std::vector<double> w_;
397 
398  public:
399  void add(double wi,ASPRCost* ci)
400  {
401  w_.push_back(wi);
402  c_.push_back(ci);
403  }
407  virtual double getCost(const adore::fun::SetPointRequest& spr)const override
408  {
409  double value = 0.0;
410  for(unsigned int i=0;i<std::min(c_.size(),w_.size());i++)
411  {
412  value += w_[i] * c_[i]->getCost(spr);
413  }
414  return value;
415  }
416  virtual std::string getName()const override{return "weightedSum";}
417  };
418 
423  {
424  private:
427  public:
429  {
430  big_distance_ = 5.0;
431  small_velocity_ = 1;
432  }
433  virtual bool isValid(const adore::fun::SetPointRequest& spr)const override
434  {
435  if(spr.setPoints.size()==0)return false;
436  if(spr.setPoints[0].x0ref.getvx()>small_velocity_)return true;
437  double ddmax = 0.0;
438  double x0 = spr.setPoints[0].x0ref.getX();
439  double y0 = spr.setPoints[0].x0ref.getY();
440  for(int i=1;i<spr.setPoints.size();i++)
441  {
442  double x = spr.setPoints[i].x0ref.getX();
443  double y = spr.setPoints[i].x0ref.getY();
444  ddmax = (std::max)(ddmax,(x-x0)*(x-x0)+(y-y0)*(y-y0));
445  }
446  return ddmax>big_distance_*big_distance_;
447  }
448  };
449 
450  }
451 }
Definition: setpointrequestevaluation.h:26
Definition: setpointrequestevaluation.h:34
Definition: basicsetpointrequestevaluators.h:423
RestartEffort()
Definition: basicsetpointrequestevaluators.h:428
double big_distance_
Definition: basicsetpointrequestevaluators.h:425
double small_velocity_
Definition: basicsetpointrequestevaluators.h:426
virtual bool isValid(const adore::fun::SetPointRequest &spr) const override
Definition: basicsetpointrequestevaluators.h:433
Definition: basicsetpointrequestevaluators.h:148
adore::params::APTacticalPlanner * params_
Definition: basicsetpointrequestevaluators.h:150
SPRAverageProgressLoss(adore::params::APTacticalPlanner *params)
Definition: basicsetpointrequestevaluators.h:153
virtual double getCost(const adore::fun::SetPointRequest &spr) const override
Definition: basicsetpointrequestevaluators.h:155
virtual std::string getName() const override
Definition: basicsetpointrequestevaluators.h:154
Definition: basicsetpointrequestevaluators.h:32
adore::view::ATrafficPredictionView * predictionView_
Definition: basicsetpointrequestevaluators.h:34
adore::params::APVehicle * pvehicle_
Definition: basicsetpointrequestevaluators.h:35
adore::params::APTacticalPlanner * ptac_
Definition: basicsetpointrequestevaluators.h:36
SPRInvariantCollisionFreedom(adore::view::ATrafficPredictionView *predictionView, adore::params::APVehicle *pvehicle=adore::params::ParamsFactoryInstance::get() ->getVehicle(), adore::params::APTacticalPlanner *ptac=adore::params::ParamsFactoryInstance::get() ->getTacticalPlanner(), adore::params::APPrediction *ppred=adore::params::ParamsFactoryInstance::get() ->getPrediction())
Definition: basicsetpointrequestevaluators.h:39
adore::params::APPrediction * ppred_
Definition: basicsetpointrequestevaluators.h:37
virtual bool isValid(const adore::fun::SetPointRequest &spr) const override
Definition: basicsetpointrequestevaluators.h:46
Definition: basicsetpointrequestevaluators.h:393
std::vector< double > w_
Definition: basicsetpointrequestevaluators.h:396
void add(double wi, ASPRCost *ci)
Definition: basicsetpointrequestevaluators.h:399
virtual std::string getName() const override
Definition: basicsetpointrequestevaluators.h:416
virtual double getCost(const adore::fun::SetPointRequest &spr) const override
Definition: basicsetpointrequestevaluators.h:407
std::vector< adore::fun::ASPRCost * > c_
Definition: basicsetpointrequestevaluators.h:395
Definition: basicsetpointrequestevaluators.h:364
virtual std::string getName() const override
Definition: basicsetpointrequestevaluators.h:366
virtual double getCost(const adore::fun::SetPointRequest &spr) const override
Definition: basicsetpointrequestevaluators.h:367
Definition: basicsetpointrequestevaluators.h:378
virtual double getCost(const adore::fun::SetPointRequest &spr) const override
Definition: basicsetpointrequestevaluators.h:381
virtual std::string getName() const override
Definition: basicsetpointrequestevaluators.h:380
Definition: basicsetpointrequestevaluators.h:178
double nav_cost_of_lane(const adore::fun::SetPointRequest &spr, int laneid) const
minimum cost of given lane, independent of trajectory
Definition: basicsetpointrequestevaluators.h:206
virtual std::string getName() const override
Definition: basicsetpointrequestevaluators.h:189
SPRNavigationCostOnLane(adore::view::AThreeLaneView *threeLaneView, adore::params::APTacticalPlanner *params, int laneID)
Definition: basicsetpointrequestevaluators.h:186
virtual double getCost(const adore::fun::SetPointRequest &spr) const override
Definition: basicsetpointrequestevaluators.h:190
int laneID_
Definition: basicsetpointrequestevaluators.h:183
double max_overshoot_
Definition: basicsetpointrequestevaluators.h:182
adore::params::APTacticalPlanner * params_
Definition: basicsetpointrequestevaluators.h:181
adore::view::AThreeLaneView * threeLaneView_
Definition: basicsetpointrequestevaluators.h:180
Definition: basicsetpointrequestevaluators.h:68
adore::params::APTacticalPlanner * ptac_
Definition: basicsetpointrequestevaluators.h:72
adore::params::APVehicle * pvehicle_
Definition: basicsetpointrequestevaluators.h:71
std::string getName() const
Definition: basicsetpointrequestevaluators.h:99
virtual bool isValid(const adore::fun::SetPointRequest &spr) const override
Definition: basicsetpointrequestevaluators.h:82
adore::params::APPrediction * ppred_
Definition: basicsetpointrequestevaluators.h:73
SPRNonCoercive(adore::view::ATrafficPredictionView *predictionView, adore::params::APVehicle *pvehicle=adore::params::ParamsFactoryInstance::get() ->getVehicle(), adore::params::APTacticalPlanner *ptac=adore::params::ParamsFactoryInstance::get() ->getTacticalPlanner(), adore::params::APPrediction *ppred=adore::params::ParamsFactoryInstance::get() ->getPrediction())
Definition: basicsetpointrequestevaluators.h:75
adore::view::ATrafficPredictionView * predictionView_
Definition: basicsetpointrequestevaluators.h:70
Definition: basicsetpointrequestevaluators.h:256
adore::view::AThreeLaneView * threeLaneView_
Definition: basicsetpointrequestevaluators.h:258
double normalized_nav_cost_simple(const adore::fun::SetPointRequest &spr, int laneid) const
simplified version of normalized_nav_cost: lane is given
Definition: basicsetpointrequestevaluators.h:285
SPRNormalizedNavigationCost(adore::view::AThreeLaneView *threeLaneView, adore::params::APTacticalPlanner *params, int laneID)
Definition: basicsetpointrequestevaluators.h:264
virtual double getCost(const adore::fun::SetPointRequest &spr) const override
Definition: basicsetpointrequestevaluators.h:268
adore::params::APTacticalPlanner * params_
Definition: basicsetpointrequestevaluators.h:259
int laneID_
Definition: basicsetpointrequestevaluators.h:261
double max_overshoot_
Definition: basicsetpointrequestevaluators.h:260
virtual std::string getName() const override
Definition: basicsetpointrequestevaluators.h:267
Definition: basicsetpointrequestevaluators.h:107
adore::view::ATrafficPredictionView * predictionView_
Definition: basicsetpointrequestevaluators.h:109
virtual double getCost(const adore::fun::SetPointRequest &spr) const override
Definition: basicsetpointrequestevaluators.h:117
adore::params::APVehicle * pvehicle_
Definition: basicsetpointrequestevaluators.h:110
virtual std::string getName() const override
Definition: basicsetpointrequestevaluators.h:116
SPRTTCNominal(adore::view::ATrafficPredictionView *predictionView, adore::params::APVehicle *pvehicle=adore::params::ParamsFactoryInstance::get() ->getVehicle())
Definition: basicsetpointrequestevaluators.h:112
Definition: setpointrequestswath.h:26
void append_cylinder_swath_linear(const SetPointRequest &spr, adore::mad::OccupancyCylinderTree &prediction, bool terminal=false)
Definition: setpointrequestswath.h:55
void setLonError(double value)
Definition: setpointrequestswath.h:42
void setAccelerationErrorSlow(double value)
Definition: setpointrequestswath.h:44
void setDuration(double value)
Definition: setpointrequestswath.h:43
void setLatError(double value)
Definition: setpointrequestswath.h:41
Definition: setpointrequest.h:35
std::vector< SetPoint > setPoints
Definition: setpointrequest.h:38
Definition: occupancycylinder.h:84
parameter interface for parameters related to prediction
Definition: ap_prediction.h:28
virtual double get_roadbased_prediction_duration() const =0
prediction duration for objects that can be matched to road
abstract classs containing parameters to configure aspects and constraints of the tactical planner
Definition: ap_tactical_planner.h:25
virtual double getAccLonUB() const =0
virtual double getGlobalSpeedLimit() const =0
virtual double getCollisionDetectionLateralError() const =0
virtual double getCollisionDetectionFrontBufferSpace() const =0
virtual double getCollisionDetectionLongitudinalError() const =0
virtual double getCollisionDetectionLateralPrecision() const =0
virtual double getNominalSwathAccelerationError() const =0
abstract class for vehicle configuration related paremeters
Definition: ap_vehicle.h:29
virtual double get_b() const =0
rear axle to cog
virtual double get_bodyWidth() const =0
virtual double get_d() const =0
rear border to rear axle
virtual double get_a() const =0
cog to front axle
virtual double get_c() const =0
front axle to front border
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 getSMin() const =0
virtual double getSMax() const =0
virtual void boundNavigationCost(double s0, double s1, double &cmin, double &cmax)=0
virtual bool isValid() const =0
Definition: athreelaneview.h:30
virtual ALane * getCurrentLane()=0
virtual ALaneChangeView * getLeftLaneChange()=0
virtual ALaneChangeView * getRightLaneChange()=0
Definition: atrafficpredictionview.h:27
virtual bool overlapsExpectedBehavior(const adore::mad::OccupancyCylinderTree &space) const =0
virtual bool overlapsEmergencyBehavior(const adore::mad::OccupancyCylinderTree &space) const =0
virtual double getExpectedCollisionTime(const adore::mad::OccupancyCylinderTree &space, double guard) const =0
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
adoreMatrix< T, N, M > max(adoreMatrix< T, N, M > a, const adoreMatrix< T, N, M > &b)
Definition: adoremath.h:686
x0
Definition: adore_set_goal.py:25
x
Definition: adore_set_goal.py:30
y0
Definition: adore_set_goal.py:26
y
Definition: adore_set_goal.py:31
p0
Definition: adore_set_pose.py:32
Definition: areaofeffectconverter.h:20