99 std::string
getName()
const{
return "coercion";}
116 virtual std::string
getName()
const override{
return "TTC_nom";}
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;
136 double cost = (ttc_max-ttc)*(ttc_max-ttc)/ttc_max/ttc_max;
154 virtual std::string
getName()
const override{
return "AverageProgressLoss";}
157 const double tmax = 10.0;
159 double distance = 0.0;
163 double dt = sp.tEnd-sp.tStart;
164 double v = sp.x0ref.getvx();
169 return std::max(0.0,1.0 - distance/(vmax*tmax));
189 virtual std::string
getName()
const override{
return "MinimumNavigationCostOnLane";}
195 catch(
const std::exception& e)
231 if(lane==
nullptr)
return 1.0e99;
232 double X0 = spr.
setPoints.begin()->x0ref.getX();
233 double Y0 = spr.
setPoints.begin()->x0ref.getY();
240 double interval_end = lane->
getSMax();
244 catch(
const std::exception& e)
246 std::cerr << e.what() <<
'\n';
267 virtual std::string
getName()
const override{
return "NormalizedNavigationCost";}
273 catch(
const std::exception& e)
291 double v0 = std::abs(spr.
setPoints.begin()->x0ref.getvx());
292 double t0 = spr.
setPoints.begin()->tStart;
293 double t1 = spr.
setPoints.rbegin()->tEnd;
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;
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;
329 lane->toRelativeCoordinates(X0,Y0,s0,n0);
330 lane->toRelativeCoordinates(X1,Y1,s1,n1);
332 catch(
const std::exception& e)
334 std::cerr <<
"error in coordinate transformation\n";
335 std::cerr << e.what() <<
'\n';
343 double tmp;lane->boundNavigationCost(interval_start,interval_end,
p0,tmp);
347 tmp;lane->boundNavigationCost(interval_start,interval_end,p1,tmp);
350 return adore::mad::bound(0.0,(optimal_progress-actual_progress)/optimal_progress,1.0);
352 catch(
const std::exception& e)
354 std::cerr <<
"error in cost bounding\n";
355 std::cerr << e.what() <<
'\n';
366 virtual std::string
getName()
const override{
return "LongitudinalAccelerationCost";}
370 for(
const auto& setPoint : spr.
setPoints)
372 value += (setPoint.tEnd-setPoint.tStart) * setPoint.x0ref.getAx()* setPoint.x0ref.getAx();
380 virtual std::string
getName()
const override{
return "LongitudinalJerkCost";}
384 for(
const auto& setPoint : spr.
setPoints)
386 value += (setPoint.tEnd-setPoint.tStart) * setPoint.x0ref.getDAx()* setPoint.x0ref.getDAx();
395 std::vector<adore::fun::ASPRCost*>
c_;
396 std::vector<double>
w_;
410 for(
unsigned int i=0;i<
std::min(
c_.size(),
w_.size());i++)
412 value +=
w_[i] *
c_[i]->getCost(spr);
416 virtual std::string
getName()
const override{
return "weightedSum";}
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
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