ADORe
ADORe is a modular open source software library and toolkit for decision making, planning, control and simulation of automated vehicles
platoon_controller.h
Go to the documentation of this file.
1 
2 /********************************************************************************
3 
4 * Copyright (C) 2017-2020 German Aerospace Center (DLR)
5 * Eclipse ADORe, Automated Driving Open Research https://eclipse.org/adore
6 *
7 * This program and the accompanying materials are made available under the
8 * terms of the Eclipse Public License 2.0 which is available at
9 * http://www.eclipse.org/legal/epl-2.0.
10 *
11 * SPDX-License-Identifier: EPL-2.0
12 *
13 
14 * Contributors:
15 * Abhishek Joshi {Reza Dariani}
16 *
17 * //Platoon controller suggested velocity for ego-vehicle.
18 ********************************************************************************/
19 
20 #pragma once
22 #include <adore/fun/nlpqlp_planner/planner_data.h>
23 #include <deque>
24 
25 //TO DO Abhishek
26 
27 //#include <PlatoonController_Feedback.h>
28 
29 //#include <PlatoonController_h_inf.h>
30 
31 
32 namespace adore
33 
34 {
35 
36  namespace fun
37 
38  {
39 
41 
42  {
43 
44 
45 
46  public:
47  double error_preceding ;
48  double error_leader;
51  double currentTime;
52  Planner_data data;
53 
54  //std::deque<double> plot_time;
55  // static const int NumPlotPoints = 100;//
56 
57 
58 
59  //communication delay is assumed to be 1.
60 
62 
63  {
64  this->data = data;
65  this->cup=cup;
66  this->MAX_VELOCITY = data.MAX_VELOCITY;
67  k1 = data.platoon_controller_kd; //D preceding
68  k2 = data.platoon_controller_kp; //P preceding
69  kI = data.platoon_controller_ki;
70  k3 = 0.0001; //D leader
71  k4 = 0.002; //P leader
72  desiredTimeGap = 1.5;
73  timeDifference = 0.1;
74  pl = 1;
75  pp = 1;
78  error_I = 0.;
80  };
81 
82 
84 
86  {
87  //double safetyMarginEgo = 1.0; //
88  //double safetyMarginCU = 1.0;
89  correction = 1.0;
90  currentTime = egoStates.getTime();
91  double vx = egoStates.getvx();
92  vx = std::max(vx,3.);
93  control_preceding = 0.;
94  control_leader = 0.;
95  error_preceding = 0.0;
96  error_leader = 0.0;
97  double preceding_v = MAX_VELOCITY;
98  double leader_v = MAX_VELOCITY;
100  double preceding_s_behind_bumper ;
101  double preceding_s_front_bumper ;
102  double leader_s_behind_bumper ;
103  double precedingPolicy_s;
104 
105  if(cup->precedingExists())
106  {
107  preceding_v = cup->getPreceding().MCM.currentTrajectory.v[0];
108  preceding_s_behind_bumper = cup->getPreceding().progress_rear_side;
109  preceding_s_front_bumper = cup->getPreceding().progress_front_side;
110  precedingPolicy_s = (desiredTimeGap * vx) + cup->getEgoProgressFrontSide();
111  error_preceding = preceding_s_behind_bumper - precedingPolicy_s;
112  if(error_preceding<-0.5) {correction = 0.; error_I=0.0;}
116 
117  }
118 
119 
120  if(cup->leaderExists() && !cup->leaderIsPreceding())
121  {
122  //desiredTimeGapWithLeader = 3.5*desiredTimeGap;
123  leader_v = cup->getLeader().MCM.currentTrajectory.v[0];
124  leader_s_behind_bumper = cup->getLeader().progress_rear_side;
125  double leaderPolicy_s = (desiredTimeGap * preceding_v) + (preceding_s_front_bumper);
126  error_leader = leader_s_behind_bumper - leaderPolicy_s;
129  //std::cout<<"\n: "<<leaderPolicy_s<<"\t"<< leader_s_behind_bumper<<"\t"<<error_leader;
130  // std::cout<<"\n"<<error_leader<<"\t"<<control_leader;
131  }
132 
134  //std::cout<<"\n velocity from controller "<<suggested_velocity;
138  if(data.print_platooning) std::cout<<"\n"<<error_preceding<<"\t"<<suggested_velocity;
139 
140  }
141 
142 
143  double getVelocity() {return suggested_velocity;}
144  double filter(double v)
145  {
146  double _v = std::floor(v);
147  double rem = v - _v;
148  rem = (std::floor(rem*10.))/10.;
149  return _v + rem;
150 
151  }
152 
153  private:
154  // adore::view::platoonView* p_view;
155  double correction;
157  double MAX_VELOCITY;
158  double kI;
159  double kp; // Preceding gap error controller
160  double kl; // Leader gap error controller
161  double pl; // Preceding car-following policy - can be set by user
162  double pp; // Leading car-following policy - can be set by user
163  double desiredTimeGap; //has to be set by user
164  double desiredTimeGapWithLeader; // distance between ego and leader / velocity of the ego
165  double timeDifference; //has to be set by user
166  double k1,k2,k3,k4; //tuning knobs of kp (k1,k2) and kl (k3,k4) controllers
167  double v1,v2,v3; // v1 = velocity from kp controller; v2 = target velocity of preceding vehicle; v3 = velocity of kl controller
168  double velocity; //final velocity suggested by controllers
174  double error_I;
175 
176  };
177 
178  }
179 
180 }
Definition: cooperativeusersprocess.h:27
bool precedingExists()
Definition: cooperativeusersprocess.h:68
double getEgoProgressFrontSide()
Definition: cooperativeusersprocess.h:75
CooperativeUser getLeader()
Definition: cooperativeusersprocess.h:66
bool leaderExists()
Definition: cooperativeusersprocess.h:69
CooperativeUser getPreceding()
Definition: cooperativeusersprocess.h:65
bool leaderIsPreceding()
Definition: cooperativeusersprocess.h:71
Definition: platoon_controller.h:42
adore::env::CooperativeUsersProcess * cup
Definition: platoon_controller.h:156
double v2
Definition: platoon_controller.h:167
double control_preceding
Definition: platoon_controller.h:49
double suggested_velocity
Definition: platoon_controller.h:173
double filter(double v)
Definition: platoon_controller.h:144
double error_I
Definition: platoon_controller.h:174
double previousErrorToLeader
Definition: platoon_controller.h:169
double velocity
Definition: platoon_controller.h:168
double v3
Definition: platoon_controller.h:167
double kI
Definition: platoon_controller.h:158
double kp
Definition: platoon_controller.h:159
double previousErrorToPrecedor
Definition: platoon_controller.h:171
double error_preceding
Definition: platoon_controller.h:47
~PlatoonController()
Definition: platoon_controller.h:83
void update(adore::fun::VehicleMotionState9d &egoStates)
Definition: platoon_controller.h:85
double correction
Definition: platoon_controller.h:155
Planner_data data
Definition: platoon_controller.h:52
double kl
Definition: platoon_controller.h:160
double currentTime
Definition: platoon_controller.h:51
double MAX_VELOCITY
Definition: platoon_controller.h:157
double k4
Definition: platoon_controller.h:166
double k3
Definition: platoon_controller.h:166
double timeDifference
Definition: platoon_controller.h:165
double k1
Definition: platoon_controller.h:166
double currentErrorToLeader
Definition: platoon_controller.h:170
double desiredTimeGap
Definition: platoon_controller.h:163
double pp
Definition: platoon_controller.h:162
double error_leader
Definition: platoon_controller.h:48
double control_leader
Definition: platoon_controller.h:50
double pl
Definition: platoon_controller.h:161
double getVelocity()
Definition: platoon_controller.h:143
PlatoonController(adore::env::CooperativeUsersProcess *cup, Planner_data data)
Definition: platoon_controller.h:61
double desiredTimeGapWithLeader
Definition: platoon_controller.h:164
double v1
Definition: platoon_controller.h:167
double k2
Definition: platoon_controller.h:166
double currentErrorToPrecedor
Definition: platoon_controller.h:172
adoreMatrix< T, N, M > max(adoreMatrix< T, N, M > a, const adoreMatrix< T, N, M > &b)
Definition: adoremath.h:686
Definition: areaofeffectconverter.h:20
std::vector< double > v
Definition: cooperativeusersprediction.h:39
struct adore::env::CooperativeUserPrediction::@0 currentTrajectory
CooperativeUserPrediction MCM
Definition: cooperativeusersprocess.h:33
double progress_front_side
Definition: cooperativeusersprocess.h:36
double progress_rear_side
Definition: cooperativeusersprocess.h:37
This struct holds the motion state of the vehicle in 9d.
Definition: vehiclemotionstate9d.h:39
double getvx() const
Get the longitudinal velocity.
Definition: vehiclemotionstate9d.h:78
double getTime() const
Get the time.
Definition: vehiclemotionstate9d.h:48