ADORe
ADORe is a modular open source software library and toolkit for decision making, planning, control and simulation of automated vehicles
mrmplanner.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 API and implementation
13  ********************************************************************************/
14 
15 #pragma once
16 #include "lateralplanner.h"
18 
19 namespace adore
20 {
21  namespace fun
22  {
30  template<int K,int P>
32  {
33  public:
34  static const int N = 3;//number of states
35  static const int R = 1;//number of inputs
38  private:
45  double jmax_;
46  double tstall_;
47  double astall_;
48  double amin_;
51  public:// setters for parameters
52  void setJMax(double value){jmax_=value;}
53  void setTStall(double value){tstall_=value;}
54  void setAStall(double value){astall_=value;}
55  void setAMin(double value){amin_=value;}
56  void setSecondAttempt(bool value){second_attempt_=value;}
57 
58  public:
61  adore::params::APVehicle* apvehicle,
63  :lateralPlanner_(lfv,aplat,apvehicle,aptrajectory),
64  roadCoordinates_(lfv,apvehicle,aptrajectory),
65  aptraj_(aptrajectory),apvehicle_(apvehicle)
66  {
67  jmax_ = 1.0;
68  amin_ = -3.0;
69  tstall_ = 1.0;
70  astall_ = -0.25;
71  second_attempt_ = true;
72  //initialize the longitudinal plan function [t0,tend]->(s,s',s'',s''')
73  longitudinal_plan_.getData().set_size(5,K*P+1);
74  dlib::set_rowm(longitudinal_plan_.getData(),0) = adore::mad::linspace(0.0,lateralPlanner_.getTend(),K*P+1);
76  }
77 
79  {
81  }
82 
84  double amin_medium_brake_trapezoidal(double v0, double a0, double jmin)const
85  {
86  return -std::sqrt(0.5*a0*a0-v0*jmin);
87  }
89  void t_short_brake_trapezoidal(double v0,double a0,double jmin, double t3)
90  {
91  t3 = std::sqrt(a0/jmin-2.0*v0/jmin)-a0/jmin;
92  }
94  void t_medium_brake_trapezoidal(double v0,double a0, double jmin, double& t1, double &t3)const
95  {
96  const double amin = amin_medium_brake_trapezoidal(v0,a0,jmin);
97  t1 = (amin-a0)/jmin;
98  t3 = amin/jmin;
99  }
101  void t_long_brake_trapezoidal(double v0,double a0,double amin,double jmin,double& t1, double& t2,double &t3)const
102  {
103  t1 = (amin-a0)/jmin;
104  t3 = amin/jmin;
105  const double dv1 = 0.5 * t1 * (amin-a0) + t1 * a0;
106  const double dv3 = 0.5 * t3 * amin;
107  t2 = -(v0+dv1+dv3)/amin;
108  }
110  int brake_case_trapezoidal(double v0,double a0,double amin,double jmin)const
111  {
112  a0 = std::min(0.0,a0);
113  if(-(a0/jmin)*(0.5*a0)>=v0)return 0; //short brake: reaching v=0 before a0 can be reduced to zero
114  if(amin_medium_brake_trapezoidal(v0,a0,jmin)>=amin)return 1; //medium brake: ramp down acceleration until a1>amin, ramp up to zero
115  return 2; //long brake: ramp down acceleration until amin, constant acceleration, ramp up to zero
116  }
118  void brake_params_trapezoidal(double v0,double a0,double amin,double jmin,double& a1,double& t1,double& t2,double& t3)
119  {
120  switch(brake_case_trapezoidal(v0,a0,amin,jmin))
121  {
122  case 0:
123  a1 = a0;
124  t1 = 0.0;
125  t2 = 0.0;
126  t_short_brake_trapezoidal(v0,a0,jmin,t3);
127  break;
128  case 1:
129  a1 = amin_medium_brake_trapezoidal(v0,a0,jmin);
130  t_medium_brake_trapezoidal(v0,a0,jmin,t1,t3);
131  t2 = 0.0;
132  break;
133  case 2:
134  a1 = amin;
135  t_long_brake_trapezoidal(v0,a0,amin,jmin,t1,t2,t3);
136  break;
137  }
138  }
139  double a_brake_trapezoidal(double t,double a0,double a1,double jmin,double t1,double t2,double t3)
140  {
141  if(t<=t1)
142  {
143  return a0 + t*jmin;
144  }
145  if(t<=t1+t2)
146  {
147  return a1;
148  }
149  if(t<=t1+t2+t3)
150  {
151  return a1-jmin*(t-t1-t2);
152  }
153  return 0.0;
154  }
155  double j_brake_trapezoidal(double t,double jmin,double t1,double t2,double t3)
156  {
157  if(t<=t1)
158  {
159  return jmin;
160  }
161  if(t<=t1+t2)
162  {
163  return 0.0;
164  }
165  if(t<=t1+t2+t3)
166  {
167  return -jmin;
168  }
169  return 0.0;
170  }
171 
175  virtual void compute(const VehicleMotionState9d& initial_state)
176  {
177  if(!roadCoordinates_.isValid())return;
178 
179  auto rc = roadCoordinates_.toRoadCoordinates(initial_state);
180  double s0 = rc.s0;
181  double ds0 = rc.s1;
182  double dds0 = adore::mad::bound(amin_,rc.s2,0.0);//the current acceleration is set to 0 if a>0 to achieve a quicker reaction
183  double s0_offset = s0;
184  double t0_offset = initial_state.getTime();
185 
186  //fill the longitudinal plan function [t0,tend]->(s,s',s'',s''')
187  //(longitudinal plan only depends on initial conditions)
188  double s = s0-s0_offset;
189  double ds = ds0;
190  double dds = dds0;
191  longitudinal_plan_.getData()(1,0) = s;
192  longitudinal_plan_.getData()(2,0) = ds;
193  longitudinal_plan_.getData()(3,0) = dds;
194 
195  double t1,t2,t3,a1;
196  brake_params_trapezoidal(ds0,dds0,amin_,-jmax_,a1,t1,t2,t3);
197  int version = 1;
198 
199  for(int k=1;k<longitudinal_plan_.getData().nc();k++)
200  {
201  double ti = longitudinal_plan_.getData()(0,k-1);
202  double tk = longitudinal_plan_.getData()(0,k);
203  double dt = tk-ti;
204  double jset;
205  if(version==0)
206  {
207  double aset = ds>0.0?(ti<tstall_?astall_:amin_):0.0;
208  jset = adore::mad::bound(-jmax_,(aset-dds)/dt,jmax_);
209  s = std::max(s,s+dt*ds+0.5*dt*dt*dds+1.0/6.0*dt*dt*dt*jset);
210  ds = std::max(0.0,ds+dds*dt+0.5*dt*dt*jset);
211  dds = dds + dt * jset;
212  }
213  else if(version==1)
214  {
215  jset = j_brake_trapezoidal(ti,-jmax_,t1,t2,t3);
216  s = std::max(s,s+dt*ds+0.5*dt*dt*dds+1.0/6.0*dt*dt*dt*jset);
217  ds = std::max(0.0,ds+dds*dt+0.5*dt*dt*jset);
218  dds = a_brake_trapezoidal(tk,dds0,a1,-jmax_,t1,t2,t3);
219  }
220  longitudinal_plan_.getData()(1,k) = s;
221  longitudinal_plan_.getData()(2,k) = ds;
222  longitudinal_plan_.getData()(3,k) = dds;
223  longitudinal_plan_.getData()(4,k-1) = jset;
224  longitudinal_plan_.getData()(4,k) = 0.0;
225  }
226  //postprocess constraints for longitudinal plan
227  postproc_.getInformationSet().update(t0_offset,s0_offset,ds0);
229 
230 
232  {
233  //compute lateral plan
234  lateralPlanner_.compute(initial_state,&longitudinal_plan_,t0_offset,s0_offset);
235  }
236 
237  if(!hasValidPlan() && second_attempt_)
238  {
239  second_attempt_ = false;
240  double tmp = tstall_;
241  tstall_ = 0.0;
242  compute(initial_state);
243  second_attempt_ = true;
244  tstall_ = tmp;
245  }
246  }
247 
249  {
250  return longitudinal_plan_;
251  }
253  {
254  return lateralPlanner_.getLateralPlan();
255  }
260  {
261  return lateralPlanner_;
262  }
266  virtual bool hasValidPlan()const
267  {
268  return longitudinal_plan_valid_ && lateralPlanner_.hasValidPlan();
269  }
273  virtual const SetPointRequest* getSetPointRequest()const
274  {
275  return lateralPlanner_.getSetPointRequest();
276  }
280  virtual double getCPUTime()const
281  {
282  return 0.0;
283  }
284  };
285  }
286 }
Definition: anominalplanner.h:28
TInformationSet & getInformationSet()
Definition: informationsetpostprocessing.h:38
bool isLongitudinalPlanValid(double t0_offset, double s0_offset, adore::mad::LLinearPiecewiseFunctionM< double, K > *longitudinal_plan)
Definition: informationsetpostprocessing.h:50
Definition: lateralplanner.h:41
Definition: mrmplanner.h:32
double amin_
Definition: mrmplanner.h:48
void t_medium_brake_trapezoidal(double v0, double a0, double jmin, double &t1, double &t3) const
times required for ramp down and ramp up during medium brake
Definition: mrmplanner.h:94
virtual const SetPointRequest * getSetPointRequest() const
Definition: mrmplanner.h:273
LateralPlanner< K, P > lateralPlanner_
Definition: mrmplanner.h:39
static const int R
Definition: mrmplanner.h:35
double a_brake_trapezoidal(double t, double a0, double a1, double jmin, double t1, double t2, double t3)
Definition: mrmplanner.h:139
RoadCoordinateConverter roadCoordinates_
Definition: mrmplanner.h:42
void brake_params_trapezoidal(double v0, double a0, double amin, double jmin, double &a1, double &t1, double &t2, double &t3)
general parameters
Definition: mrmplanner.h:118
adore::params::APTrajectoryGeneration * aptraj_
Definition: mrmplanner.h:43
adore::params::APVehicle * apvehicle_
Definition: mrmplanner.h:44
void setAMin(double value)
Definition: mrmplanner.h:55
MRMPlanner(adore::view::ALane *lfv, adore::params::APLateralPlanner *aplat, adore::params::APVehicle *apvehicle, adore::params::APTrajectoryGeneration *aptrajectory)
Definition: mrmplanner.h:59
void t_long_brake_trapezoidal(double v0, double a0, double amin, double jmin, double &t1, double &t2, double &t3) const
times required for ramp down, constant and ramp up during long brake
Definition: mrmplanner.h:101
TPostProcessConstraints::TInformationSet & getInformationSet()
Definition: mrmplanner.h:78
bool longitudinal_plan_valid_
Definition: mrmplanner.h:50
double jmax_
Definition: mrmplanner.h:45
void setTStall(double value)
Definition: mrmplanner.h:53
InformationSetPostProcessing< 4, 2 > TPostProcessConstraints
Definition: mrmplanner.h:37
adore::mad::LLinearPiecewiseFunctionM< double, N+R > TPartialPlan
Definition: mrmplanner.h:36
void t_short_brake_trapezoidal(double v0, double a0, double jmin, double t3)
times required for ramp up during short brake
Definition: mrmplanner.h:89
static const int N
Definition: mrmplanner.h:34
virtual double getCPUTime() const
Definition: mrmplanner.h:280
double tstall_
Definition: mrmplanner.h:46
TPostProcessConstraints postproc_
Definition: mrmplanner.h:41
double j_brake_trapezoidal(double t, double jmin, double t1, double t2, double t3)
Definition: mrmplanner.h:155
virtual void compute(const VehicleMotionState9d &initial_state)
Definition: mrmplanner.h:175
void setJMax(double value)
Definition: mrmplanner.h:52
LateralPlanner< K, P > & getOffsetSolver()
Definition: mrmplanner.h:259
double amin_medium_brake_trapezoidal(double v0, double a0, double jmin) const
minimum acceleration that can be achieved before inverting jerk to end with v=0 and a=0
Definition: mrmplanner.h:84
TPartialPlan & getLateralPlan()
Definition: mrmplanner.h:252
TPartialPlan longitudinal_plan_
Definition: mrmplanner.h:40
bool second_attempt_
Definition: mrmplanner.h:49
int brake_case_trapezoidal(double v0, double a0, double amin, double jmin) const
Definition: mrmplanner.h:110
void setAStall(double value)
Definition: mrmplanner.h:54
void setSecondAttempt(bool value)
Definition: mrmplanner.h:56
virtual bool hasValidPlan() const
Definition: mrmplanner.h:266
TPartialPlan & getLongitudinalPlan()
Definition: mrmplanner.h:248
double astall_
Definition: mrmplanner.h:47
virtual void update(double t0, double s0, double ds0) override
Definition: anominalplannerinformation.h:209
Definition: roadcoordinates.h:46
RoadCoordinates toRoadCoordinates(const PlanarVehicleState10d &x, bool log=false)
Definition: roadcoordinates.h:156
bool isValid()
Definition: roadcoordinates.h:128
Definition: setpointrequest.h:35
adoreMatrix< T, n+1, 0 > & getData()
Definition: llinearpiecewisefunction.h:147
abstract class containing parameters related to configuring the lateral planner
Definition: ap_lateral_planner.h:26
abstract class containing parameters to configure the behaviour of trajectory generation
Definition: ap_trajectory_generation.h:26
abstract class for vehicle configuration related paremeters
Definition: ap_vehicle.h:29
Definition: alane.h:28
adoreMatrix< T, 1, n > linspace(T x0, T x1)
Definition: adoremath.h:91
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
Definition: areaofeffectconverter.h:20
This struct holds the motion state of the vehicle in 9d.
Definition: vehiclemotionstate9d.h:39
double getTime() const
Get the time.
Definition: vehiclemotionstate9d.h:48
double s0
Definition: roadcoordinates.h:37