1 package org.opentrafficsim.road.gtu.lane.tactical.cacc;
2
3
4 import org.djunits.Throw;
5 import org.djunits.unit.AccelerationUnit;
6 import org.djunits.value.vdouble.scalar.Acceleration;
7 import org.djunits.value.vdouble.scalar.Length;
8 import org.djunits.value.vdouble.scalar.Speed;
9 import org.opentrafficsim.base.parameters.ParameterException;
10 import org.opentrafficsim.base.parameters.ParameterTypeAcceleration;
11 import org.opentrafficsim.base.parameters.ParameterTypeDouble;
12 import org.opentrafficsim.base.parameters.ParameterTypeDuration;
13 import org.opentrafficsim.base.parameters.ParameterTypeLength;
14 import org.opentrafficsim.base.parameters.ParameterTypes;
15 import org.opentrafficsim.base.parameters.Parameters;
16 import org.opentrafficsim.core.gtu.GTUType;
17 import org.opentrafficsim.core.gtu.perception.EgoPerception;
18 import org.opentrafficsim.core.gtu.plan.operational.OperationalPlanException;
19 import org.opentrafficsim.road.gtu.lane.LaneBasedGTU;
20 import org.opentrafficsim.road.gtu.lane.perception.DownstreamNeighborsIterable;
21 import org.opentrafficsim.road.gtu.lane.perception.InfrastructureLaneChangeInfo;
22 import org.opentrafficsim.road.gtu.lane.perception.LanePerception;
23 import org.opentrafficsim.road.gtu.lane.perception.PerceptionCollectable;
24 import org.opentrafficsim.road.gtu.lane.perception.RelativeLane;
25 import org.opentrafficsim.road.gtu.lane.perception.categories.InfrastructurePerception;
26 import org.opentrafficsim.road.gtu.lane.perception.headway.HeadwayGTU;
27
28
29
30
31
32
33
34
35
36
37
38 public class CaccController implements LongitudinalController
39 {
40
41
42 public static final ParameterTypeDuration T_SYSTEM_CACC = CaccParameters.T_SYSTEM_CACC;
43
44 public static final ParameterTypeDuration T_SYSTEM_ACC = CaccParameters.T_SYSTEM_ACC;
45
46 public static final ParameterTypeDouble K = CaccParameters.K;
47
48 public static final ParameterTypeDouble K_A = CaccParameters.K_A;
49
50 public static final ParameterTypeDouble K_V = CaccParameters.K_V;
51
52 public static final ParameterTypeDouble K_D = CaccParameters.K_D;
53
54 public static final ParameterTypeAcceleration A_MIN = CaccParameters.A_MIN;
55
56 public static final ParameterTypeAcceleration A_MAX = CaccParameters.A_MAX;
57
58 public static final ParameterTypeDouble R_MIN = CaccParameters.R_MIN;
59
60 public static final ParameterTypeLength STANDSTILL = CaccParameters.STANDSTILL;
61
62
63
64 private GTUType caccGTUType = null;
65
66
67 private Platoon platoon;
68
69 private Speed setSpeed;
70
71
72
73
74
75 public void setPlatoon(final Platoon platoon)
76 {
77 this.platoon = platoon;
78 }
79
80
81
82
83
84 public void setCACCGTUType(final GTUType gtuType)
85 {
86 this.caccGTUType = gtuType;
87 }
88
89
90
91
92
93
94
95 @Override
96 public Acceleration calculateAcceleration(final LaneBasedGTU gtu) throws OperationalPlanException, ParameterException
97 {
98 Parameters parameters = gtu.getParameters();
99 LanePerception perception = gtu.getTacticalPlanner().getPerception();
100 CaccPerceptionCategory caccPerception = perception.getPerceptionCategory(CaccPerceptionCategory.class);
101 EgoPerception<?, ?> ego = perception.getPerceptionCategory(EgoPerception.class);
102
103 DownstreamNeighborsIterable leaders = caccPerception.getLeaders(RelativeLane.CURRENT);
104 DownstreamNeighborsIterable leadersLeft = caccPerception.getLeaders(RelativeLane.LEFT);
105 DownstreamNeighborsIterable leadersRight = caccPerception.getLeaders(RelativeLane.RIGHT);
106 Speed newSetSpeed;
107
108 if (leaders.isEmpty() || (this.platoon != null && !this.platoon.isInPlatoon(leaders.first().getId())))
109 {
110
111 newSetSpeed = parameters.getParameter(CaccParameters.SET_SPEED);
112 }
113 else
114 {
115 newSetSpeed = parameters.getParameter(CaccParameters.SET_SPEED).plus(Speed.instantiateSI(10.0));
116 }
117
118 this.setSpeed = Speed.min(newSetSpeed, gtu.getMaximumSpeed());
119
120
121 Acceleration a = followLeader(leaders, parameters, ego);
122
123 for (DownstreamNeighborsIterableption/DownstreamNeighborsIterable.html#DownstreamNeighborsIterable">DownstreamNeighborsIterable ldrs : new DownstreamNeighborsIterable[] {leadersLeft, leadersRight})
124 {
125 if (ldrs != null && !ldrs.isEmpty() && this.platoon != null && this.platoon.isInPlatoon(ldrs.first().getId()))
126 {
127 a = Acceleration.min(a, followLeader(ldrs, parameters, ego));
128 }
129 }
130
131
132
133 Length remainingDist = null;
134 for (InfrastructureLaneChangeInfo ili : perception.getPerceptionCategory(InfrastructurePerception.class)
135 .getInfrastructureLaneChangeInfo(RelativeLane.CURRENT))
136 {
137 if (remainingDist == null || remainingDist.gt(ili.getRemainingDistance()))
138 {
139 remainingDist = ili.getRemainingDistance();
140 }
141 }
142 if (remainingDist != null)
143 {
144 Speed speed = ego.getSpeed();
145 Acceleration bCrit = parameters.getParameter(ParameterTypes.BCRIT);
146 remainingDist = remainingDist.minus(parameters.getParameter(STANDSTILL));
147
148 remainingDist = remainingDist.minus(Length.instantiateSI(10));
149 Length remainingDistStart = remainingDist.minus(Length.instantiateSI(10));
150 if (remainingDistStart.le0())
151 {
152 if (speed.gt0())
153 {
154 a = Acceleration.min(a, bCrit.neg());
155 }
156 else
157 {
158 a = Acceleration.min(a, Acceleration.ZERO);
159 }
160 }
161 else
162 {
163 Acceleration bMin = new Acceleration(.5 * speed.si * speed.si / remainingDist.si, AccelerationUnit.SI);
164 if (bMin.ge(bCrit))
165 {
166 a = Acceleration.min(a, bMin.neg());
167 }
168 }
169 }
170
171 return a;
172
173 }
174
175
176
177
178
179
180
181
182
183 private Acceleration followLeader(final PerceptionCollectable<HeadwayGTU, LaneBasedGTU> leaders,
184 final Parameters parameters, final EgoPerception<?, ?> ego) throws ParameterException
185 {
186
187 HeadwayGTU leader = leaders == null || leaders.isEmpty() ? null : leaders.first();
188
189
190 Acceleration amin = parameters.getParameter(A_MIN);
191 Acceleration amax = parameters.getParameter(A_MAX);
192 double k = parameters.getParameter(K);
193 double ka = parameters.getParameter(K_A);
194 double kv = parameters.getParameter(K_V);
195 double kd = parameters.getParameter(K_D);
196 double rmin = parameters.getParameter(R_MIN);
197 Length standstill = parameters.getParameter(STANDSTILL);
198
199 Throw.whenNull(this.caccGTUType, "Oops, forgot to set caccGTUType?");
200 if (leader != null && leader.getGtuType().isOfType(this.caccGTUType))
201 {
202
203
204
205 Speed v = ego.getSpeed();
206 Acceleration ap = leader.getAcceleration();
207 Speed vp = leader.getSpeed();
208 Length r = leader.getDistance();
209 double tsystem;
210
211
212 if (this.platoon != null && this.platoon.isInPlatoon(leader.getId()))
213 {
214 tsystem = parameters.getParameter(T_SYSTEM_CACC).doubleValue();
215 }
216 else
217 {
218 tsystem = parameters.getParameter(T_SYSTEM_ACC).doubleValue();
219 }
220
221
222 double rsystem = (tsystem * v.si) + standstill.si;
223 double rsafe = rsystem;
224 double rref = Math.max(rmin, Math.min(rsafe, rsystem));
225
226
227 double av = k * (this.setSpeed.si - v.si) + kd * (r.si - rref);
228 double ad = ka * ap.si + kv * (vp.si - v.si) + kd * (r.si - rref);
229 double aCACC;
230
231 if (ap.si > 0)
232 {
233 aCACC = ad;
234 }
235 else
236 {
237 aCACC = Math.min(av, ad);
238 }
239
240 return Acceleration.instantiateSI(aCACC < amax.si ? (aCACC > amin.si ? aCACC : amin.si) : amax.si);
241
242 }
243 else
244 {
245
246
247
248
249
250 Speed v = ego.getSpeed();
251 Double r;
252 Speed vp;
253
254
255 double tsystem = parameters.getParameter(T_SYSTEM_ACC).doubleValue();
256
257
258 double rsystem = tsystem * v.si + standstill.si;
259 double rref = Math.max(rmin, rsystem);
260
261
262 double ad;
263 if (leader == null)
264 {
265 ad = Double.POSITIVE_INFINITY;
266 }
267 else
268 {
269 r = leader.getDistance().si - standstill.si;
270 vp = leader.getSpeed();
271 ad = (kv * (vp.si - v.si)) + (kd * (r - rref));
272 }
273
274
275 double av = k * (this.setSpeed.si - v.si);
276
277 double aACC = Math.min(av, ad);
278 return Acceleration.instantiateSI(aACC < amax.si ? (aACC > amin.si ? aACC : amin.si) : amax.si);
279 }
280 }
281
282 }