ADORe
ADORe is a modular open source software library and toolkit for decision making, planning, control and simulation of automated vehicles
lanechangegaps.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
19 namespace adore
20 {
21 namespace env
22 {
26  class AGapRating
27  {
28  public:
29  virtual void rate(GapData& gap,double s_ego,double ds_ego)=0;
30  };
35  {
36  public:
37  virtual void rate(GapData& gap,double s_ego,double ds_ego)override
38  {
39  double ds = adore::mad::bound(gap.s_chase+gap.s_anchor,s_ego,gap.s_lead+gap.s_anchor)-s_ego;
40  gap.rating = ds*ds;
41  gap.feasible = std::abs(ds)<100.0 && gap.s_chase<gap.s_gate_closure;
42  }
43  };
44 
49  {
50  private:
55  public:
57  {
58  anchor_offset_ = 10.0;
60  }
62  {
63  setGapRating(nullptr);
64  }
66  {
67  return data_;
68  }
69  void setGapRating(AGapRating* gaprating)
70  {
71  if(gaprating_!=nullptr)delete gaprating_;
72  gaprating_=gaprating;
73  }
75  {
76  data_.clear();
77  if(lcv_->getSourceLane()==nullptr||!lcv_->getSourceLane()->isValid())return;
78  if(lcv_->getTargetLane()==nullptr||!lcv_->getTargetLane()->isValid())return;
79  double s_ego,n_ego;
80  lcv_->getTargetLane()->toRelativeCoordinates(ego.getX(),ego.getY(),s_ego,n_ego);
81  for(int i=0;i<=queue.size();i++)
82  {
83  const adore::view::TrafficObject* lead = i<queue.size()?&queue[i]:nullptr;
84  const adore::view::TrafficObject* chase = i>0?&queue[i-1]:nullptr;
85  GapData gap;
86  gap.lead_exists = lead!=nullptr;
87  gap.chase_exists = chase!=nullptr;
88 
89  if(!gap.lead_exists && !gap.chase_exists)//anchor according to ego
90  {
92  if(gap.s_anchor==s_ego)//if ego is in gate region, a velocity can be assigned
93  {
94  gap.anchor_vt = ego.getvx();
95  }
96  gap.t_obs = ego.getTime();
97  gap.s_lead = 1.0e4;
98  gap.v_lead = 1.0e3;
99  gap.s_chase = -1.0e4;
100  gap.v_chase = 0.0;
101  }
102  else if(!gap.lead_exists)//anchor to front of chase vehicle
103  {
104  gap.s_anchor = chase->getCurrentProgress() + chase->getLength()*0.5 + anchor_offset_;
105  gap.s_lead = 1.0e4;
106  gap.v_lead = 1.0e3;
107  gap.s_chase = -anchor_offset_;
108  gap.v_chase = chase->getCurrentSpeed();
109  gap.anchor_vt = chase->getCurrentSpeed();
110  gap.t_obs = chase->getObservationTime();
111  }
112  else if(!gap.chase_exists)
113  {
114  gap.s_anchor = lead->getCurrentProgress() - lead->getLength()*0.5 - anchor_offset_;
115  gap.s_lead = anchor_offset_;
116  gap.v_lead = lead->getCurrentSpeed();
117  gap.s_chase = -1.0e4;
118  gap.v_chase = 0.0;
119  gap.anchor_vt = lead->getCurrentSpeed();
120  gap.t_obs = lead->getObservationTime();
121  }
122  else
123  {
124  gap.s_anchor = lead->getCurrentProgress()-lead->getLength()*0.5 - anchor_offset_;
125  if(chase->getCurrentProgress()+chase->getLength()*0.5>gap.s_anchor)
126  {
127  gap.s_anchor = (lead->getCurrentProgress()-lead->getLength()*0.5
128  + chase->getCurrentProgress()+chase->getLength()*0.5)*0.5;
129  }
130  gap.s_lead = lead->getCurrentProgress()-lead->getLength()*0.5-gap.s_anchor;
131  gap.s_chase = chase->getCurrentProgress()+chase->getLength()*0.5-gap.s_anchor;
132  gap.v_lead = lead->getCurrentSpeed();
133  gap.v_chase = chase->getCurrentSpeed();
134  gap.anchor_vt = lead->getCurrentSpeed();
135  gap.t_obs = lead->getObservationTime();
136  }
137 
140  double sin = std::sin(lcv_->getTargetLane()->getHeading(gap.s_anchor));
141  double cos = std::cos(lcv_->getTargetLane()->getHeading(gap.s_anchor));
142  gap.anchor_dX = cos*gap.anchor_vt;
143  gap.anchor_dY = sin*gap.anchor_vt;
146  gaprating_->rate(gap,s_ego,ego.getvx());
147  if(gap.feasible)data_.push_back(gap);
148  }
149  }
150  };
151 }
152 }
Definition: lanechangegaps.h:27
virtual void rate(GapData &gap, double s_ego, double ds_ego)=0
Definition: lanechangegaps.h:35
virtual void rate(GapData &gap, double s_ego, double ds_ego) override
Definition: lanechangegaps.h:37
Definition: lanechangegaps.h:49
AGapRating * gaprating_
Definition: lanechangegaps.h:54
double anchor_offset_
Definition: lanechangegaps.h:53
~LaneChangeGaps()
Definition: lanechangegaps.h:61
LaneChangeGaps(adore::view::ALaneChangeView *lcv)
Definition: lanechangegaps.h:56
void setGapRating(AGapRating *gaprating)
Definition: lanechangegaps.h:69
adore::view::ALaneChangeView * lcv_
Definition: lanechangegaps.h:52
GapQueue & getData()
Definition: lanechangegaps.h:65
GapQueue data_
Definition: lanechangegaps.h:51
void computeGaps(const adore::view::TrafficQueue &queue, const adore::env::VehicleMotionState9d &ego)
Definition: lanechangegaps.h:74
Definition: alanechangeview.h:27
virtual double getProgressOfGateOpen() const =0
virtual double getOffsetOfDestinationOuterBorder(double s)=0
virtual ALane * getTargetLane()=0
virtual double getProgressOfGateClosed() const =0
virtual double getOffsetOfSeparatingBorder(double s)=0
virtual ALane * getSourceLane()=0
virtual void toRelativeCoordinates(double xe, double ye, double &s, double &n)=0
virtual void toEucledianCoordinates(double s, double n, double &xe, double &ye, double &ze)=0
virtual bool isValid() const =0
virtual double getHeading(double s)=0
std::vector< GapData > GapQueue
Definition: gapdata.h:53
interval< T > cos(interval< T > x)
Definition: intervalarithmetic.h:225
T bound(T lb, T value, T ub)
Definition: adoremath.h:569
interval< T > sin(interval< T > x)
Definition: intervalarithmetic.h:204
std::vector< TrafficObject > TrafficQueue
Definition: trafficobject.h:183
Definition: areaofeffectconverter.h:20
Definition: gapdata.h:27
double v_lead
Definition: gapdata.h:43
bool lead_exists
Definition: gapdata.h:39
double anchor_dY
Definition: gapdata.h:35
double s_lead
Definition: gapdata.h:41
bool chase_exists
Definition: gapdata.h:40
double s_gate_opening
Definition: gapdata.h:46
double anchor_Z
Definition: gapdata.h:33
double anchor_X
Definition: gapdata.h:31
bool feasible
Definition: gapdata.h:30
double v_chase
Definition: gapdata.h:44
double anchor_dX
Definition: gapdata.h:34
double anchor_Y
Definition: gapdata.h:32
double rating
Definition: gapdata.h:29
double anchor_vt
Definition: gapdata.h:37
double t_obs
Definition: gapdata.h:38
double s_chase
Definition: gapdata.h:42
double s_anchor
Definition: gapdata.h:45
double s_gate_closure
Definition: gapdata.h:47
This struct holds the motion state of the vehicle in 9d.
Definition: vehiclemotionstate9d.h:39
double getX() const
Get the x-coordinate.
Definition: vehiclemotionstate9d.h:54
double getvx() const
Get the longitudinal velocity.
Definition: vehiclemotionstate9d.h:78
double getY() const
Get the y-coordinate.
Definition: vehiclemotionstate9d.h:60
double getTime() const
Get the time.
Definition: vehiclemotionstate9d.h:48
Definition: trafficobject.h:27
double getCurrentSpeed() const
Definition: trafficobject.h:69
double getLength() const
Definition: trafficobject.h:118
double getCurrentProgress() const
Definition: trafficobject.h:62
double getObservationTime() const
Definition: trafficobject.h:125