1 package org.opentrafficsim.road.gtu.lane.tactical.util.lmrs;
2
3 import static org.opentrafficsim.core.gtu.behavioralcharacteristics.AbstractParameterType.Check.UNITINTERVAL;
4
5 import java.io.Serializable;
6 import java.util.HashSet;
7 import java.util.LinkedHashSet;
8 import java.util.Set;
9 import java.util.SortedMap;
10 import java.util.TreeMap;
11
12 import org.djunits.unit.AccelerationUnit;
13 import org.djunits.value.vdouble.scalar.Acceleration;
14 import org.djunits.value.vdouble.scalar.Duration;
15 import org.djunits.value.vdouble.scalar.Length;
16 import org.djunits.value.vdouble.scalar.Speed;
17 import org.djunits.value.vdouble.scalar.Time;
18 import org.opentrafficsim.core.Throw;
19 import org.opentrafficsim.core.gtu.GTUException;
20 import org.opentrafficsim.core.gtu.TurnIndicatorStatus;
21 import org.opentrafficsim.core.gtu.behavioralcharacteristics.BehavioralCharacteristics;
22 import org.opentrafficsim.core.gtu.behavioralcharacteristics.ParameterException;
23 import org.opentrafficsim.core.gtu.behavioralcharacteristics.ParameterTypeDouble;
24 import org.opentrafficsim.core.gtu.behavioralcharacteristics.ParameterTypes;
25 import org.opentrafficsim.core.gtu.plan.operational.OperationalPlanException;
26 import org.opentrafficsim.core.network.LateralDirectionality;
27 import org.opentrafficsim.core.network.NetworkException;
28 import org.opentrafficsim.road.gtu.lane.LaneBasedGTU;
29 import org.opentrafficsim.road.gtu.lane.perception.LanePerception;
30 import org.opentrafficsim.road.gtu.lane.perception.RelativeLane;
31 import org.opentrafficsim.road.gtu.lane.perception.categories.InfrastructurePerception;
32 import org.opentrafficsim.road.gtu.lane.perception.categories.NeighborsPerception;
33 import org.opentrafficsim.road.gtu.lane.perception.headway.AbstractHeadwayGTU;
34 import org.opentrafficsim.road.gtu.lane.plan.operational.SimpleOperationalPlan;
35 import org.opentrafficsim.road.gtu.lane.plan.operational.LaneOperationalPlanBuilder.LaneChange;
36 import org.opentrafficsim.road.gtu.lane.tactical.following.CarFollowingModel;
37 import org.opentrafficsim.road.gtu.lane.tactical.util.CarFollowingUtil;
38 import org.opentrafficsim.road.network.speed.SpeedLimitInfo;
39 import org.opentrafficsim.road.network.speed.SpeedLimitProspect;
40
41
42
43
44
45
46
47
48
49
50
51 public final class LmrsUtil
52 {
53
54
55 public static final ParameterTypeDouble DFREE = new ParameterTypeDouble("dFree", "Free lane change desire threshold.",
56 0.365, UNITINTERVAL)
57 {
58
59 private static final long serialVersionUID = 20160413L;
60
61 public void check(final double value, final BehavioralCharacteristics bc) throws ParameterException
62 {
63 if (bc.contains(DSYNC))
64 {
65 Throw.when(value >= bc.getParameter(DSYNC), ParameterException.class,
66 "Value of dFree is above or equal to dSync.");
67 }
68 if (bc.contains(DCOOP))
69 {
70 Throw.when(value >= bc.getParameter(DCOOP), ParameterException.class,
71 "Value of dFree is above or equal to dCoop.");
72 }
73 }
74 };
75
76
77 public static final ParameterTypeDouble DSYNC = new ParameterTypeDouble("dSync",
78 "Synchronized lane change desire threshold.", 0.577, UNITINTERVAL)
79 {
80
81 private static final long serialVersionUID = 20160413L;
82
83 public void check(final double value, final BehavioralCharacteristics bc) throws ParameterException
84 {
85 if (bc.contains(DFREE))
86 {
87 Throw.when(value <= bc.getParameter(DFREE), ParameterException.class,
88 "Value of dSync is below or equal to dFree.");
89 }
90 if (bc.contains(DCOOP))
91 {
92 Throw.when(value >= bc.getParameter(DCOOP), ParameterException.class,
93 "Value of dSync is above or equal to dCoop.");
94 }
95 }
96 };
97
98
99 public static final ParameterTypeDouble DCOOP = new ParameterTypeDouble("dCoop",
100 "Cooperative lane change desire threshold.", 0.788, UNITINTERVAL)
101 {
102
103 private static final long serialVersionUID = 20160413L;
104
105 public void check(final double value, final BehavioralCharacteristics bc) throws ParameterException
106 {
107 if (bc.contains(DFREE))
108 {
109 Throw.when(value <= bc.getParameter(DFREE), ParameterException.class,
110 "Value of dCoop is below or equal to dFree.");
111 }
112 if (bc.contains(DSYNC))
113 {
114 Throw.when(value <= bc.getParameter(DSYNC), ParameterException.class,
115 "Value of dCoop is below or equal to dSync.");
116 }
117 }
118 };
119
120
121 public static final ParameterTypeDouble DLEFT = new ParameterTypeDouble("dLeft", "Left lane change desire.", 0);
122
123
124 public static final ParameterTypeDouble DRIGHT = new ParameterTypeDouble("dLeft", "Left lane change desire.", 0);
125
126
127
128
129 private LmrsUtil()
130 {
131
132 }
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150 @SuppressWarnings("checkstyle:parameternumber")
151 public static SimpleOperationalPlan determinePlan(final LaneBasedGTU gtu, final Time startTime,
152 final LmrsStatus lmrsStatus, final CarFollowingModel carFollowingModel, final LaneChange laneChange,
153 final LanePerception perception, final LinkedHashSet<MandatoryIncentive> mandatoryIncentives,
154 final LinkedHashSet<VoluntaryIncentive> voluntaryIncentives) throws GTUException, NetworkException,
155 ParameterException, OperationalPlanException
156 {
157
158
159 perception.perceive();
160 SpeedLimitProspect slp =
161 perception.getPerceptionCategory(InfrastructurePerception.class).getSpeedLimitProspect(RelativeLane.CURRENT);
162 SpeedLimitInfo sli = slp.getSpeedLimitInfo(Length.ZERO);
163 BehavioralCharacteristics bc = gtu.getBehavioralCharacteristics();
164
165
166 Set<AbstractHeadwayGTU> leaders =
167 perception.getPerceptionCategory(NeighborsPerception.class).getFirstLeaders(LateralDirectionality.NONE);
168 for (AbstractHeadwayGTU leader : leaders)
169 {
170 if (!lmrsStatus.getLastLeaders().contains(leader))
171 {
172
173
174
175
176
177
178
179 Duration tLead = leader.getDistance().minus(bc.getParameter(ParameterTypes.S0)).divideBy(gtu.getSpeed());
180 if (tLead.lt(bc.getParameter(ParameterTypes.T)))
181 {
182 bc.setParameter(ParameterTypes.T, tLead);
183 }
184 }
185 }
186 lmrsStatus.setLastLeaders(leaders);
187
188
189 Speed speed = gtu.getSpeed();
190 Acceleration a =
191 CarFollowingUtil.followLeaders(carFollowingModel, bc, speed, sli, perception.getPerceptionCategory(
192 NeighborsPerception.class).getLeaders(RelativeLane.CURRENT));
193
194
195 LateralDirectionality initiatedLaneChange;
196 if (laneChange.isChangingLane())
197 {
198
199 RelativeLane tar = laneChange.getTargetLane();
200 initiatedLaneChange = tar.getLateralDirectionality();
201 Acceleration aTar =
202 CarFollowingUtil.followLeaders(carFollowingModel, bc, speed, sli, perception.getPerceptionCategory(
203 NeighborsPerception.class).getLeaders(tar));
204 a = Acceleration.min(a, aTar);
205
206 }
207 else
208 {
209
210 exponentialHeadwayRelaxation(bc);
211
212
213 Desire desire = getLaneChangeDesire(bc, perception, carFollowingModel, mandatoryIncentives, voluntaryIncentives);
214
215
216 boolean acceptLeft =
217 acceptGap(perception, bc, sli, carFollowingModel, desire.getLeft(), speed, LateralDirectionality.LEFT);
218 boolean acceptRight =
219 acceptGap(perception, bc, sli, carFollowingModel, desire.getRight(), speed, LateralDirectionality.RIGHT);
220
221
222 double dFree = bc.getParameter(DFREE);
223 double dSync = bc.getParameter(DSYNC);
224 double dCoop = bc.getParameter(DCOOP);
225
226 TurnIndicatorStatus turnIndicatorStatus;
227 if (desire.leftIsLargerOrEqual() && desire.getLeft() >= dFree && acceptLeft)
228 {
229
230 initiatedLaneChange = LateralDirectionality.LEFT;
231 turnIndicatorStatus = TurnIndicatorStatus.LEFT;
232 Duration tRel =
233 Duration.interpolate(bc.getParameter(ParameterTypes.TMAX), bc.getParameter(ParameterTypes.TMIN), desire
234 .getLeft());
235 if (tRel.lt(bc.getParameter(ParameterTypes.T)))
236 {
237 bc.setParameter(ParameterTypes.T, tRel);
238 }
239
240 }
241 else if (!desire.leftIsLargerOrEqual() && desire.getRight() >= dFree && acceptRight)
242 {
243
244 initiatedLaneChange = LateralDirectionality.RIGHT;
245 turnIndicatorStatus = TurnIndicatorStatus.RIGHT;
246 Duration tRel =
247 Duration.interpolate(bc.getParameter(ParameterTypes.TMAX), bc.getParameter(ParameterTypes.TMIN), desire
248 .getRight());
249 if (tRel.lt(bc.getParameter(ParameterTypes.T)))
250 {
251 bc.setParameter(ParameterTypes.T, tRel);
252 }
253
254 }
255 else
256 {
257 initiatedLaneChange = null;
258 turnIndicatorStatus = TurnIndicatorStatus.NONE;
259 }
260
261
262 Acceleration aSync;
263 if (initiatedLaneChange == null)
264 {
265
266 if (desire.leftIsLargerOrEqual() && desire.getLeft() >= dSync)
267 {
268 aSync =
269 synchronize(perception, bc, sli, carFollowingModel, desire.getLeft(), speed,
270 LateralDirectionality.LEFT);
271 a = Acceleration.min(a, aSync);
272 }
273 else if (!desire.leftIsLargerOrEqual() && desire.getRight() >= dSync)
274 {
275 aSync =
276 synchronize(perception, bc, sli, carFollowingModel, desire.getRight(), speed,
277 LateralDirectionality.RIGHT);
278 a = Acceleration.min(a, aSync);
279 }
280
281 if (desire.leftIsLargerOrEqual() && desire.getLeft() >= dCoop)
282 {
283
284 turnIndicatorStatus = TurnIndicatorStatus.LEFT;
285 }
286 else if (!desire.leftIsLargerOrEqual() && desire.getRight() >= dCoop)
287 {
288
289 turnIndicatorStatus = TurnIndicatorStatus.RIGHT;
290 }
291 }
292 gtu.setTurnIndicatorStatus(turnIndicatorStatus);
293
294
295 aSync = cooperate(perception, bc, sli, carFollowingModel, speed, LateralDirectionality.LEFT);
296 a = Acceleration.min(a, aSync);
297 aSync = cooperate(perception, bc, sli, carFollowingModel, speed, LateralDirectionality.RIGHT);
298 a = Acceleration.min(a, aSync);
299
300 }
301
302 return new SimpleOperationalPlan(a, initiatedLaneChange);
303
304 }
305
306
307
308
309
310
311 private static void exponentialHeadwayRelaxation(final BehavioralCharacteristics bc) throws ParameterException
312 {
313 double ratio = bc.getParameter(ParameterTypes.DT).si / bc.getParameter(ParameterTypes.TAU).si;
314 bc.setParameter(ParameterTypes.T, Duration.interpolate(bc.getParameter(ParameterTypes.T), bc
315 .getParameter(ParameterTypes.TMAX), ratio <= 1.0 ? ratio : 1.0));
316 }
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334 private static Desire getLaneChangeDesire(final BehavioralCharacteristics behavioralCharacteristics,
335 final LanePerception perception, final CarFollowingModel carFollowingModel,
336 final LinkedHashSet<MandatoryIncentive> mandatoryIncentives,
337 final LinkedHashSet<VoluntaryIncentive> voluntaryIncentives) throws ParameterException, GTUException,
338 OperationalPlanException
339 {
340
341 double dSync = behavioralCharacteristics.getParameter(DSYNC);
342 double dCoop = behavioralCharacteristics.getParameter(DCOOP);
343
344
345 double dLeftMandatory = Double.NEGATIVE_INFINITY;
346 double dRightMandatory = Double.NEGATIVE_INFINITY;
347 Desire mandatoryDesire = new Desire(dLeftMandatory, dRightMandatory);
348 for (MandatoryIncentive incentive : mandatoryIncentives)
349 {
350 Desire d = incentive.determineDesire(behavioralCharacteristics, perception, carFollowingModel, mandatoryDesire);
351 dLeftMandatory = d.getLeft() > dLeftMandatory ? d.getLeft() : dLeftMandatory;
352 dRightMandatory = d.getRight() > dRightMandatory ? d.getRight() : dRightMandatory;
353 mandatoryDesire = new Desire(dLeftMandatory, dRightMandatory);
354 }
355
356
357 double dLeftVoluntary = 0;
358 double dRightVoluntary = 0;
359 Desire voluntaryDesire = new Desire(dLeftVoluntary, dRightVoluntary);
360 for (VoluntaryIncentive incentive : voluntaryIncentives)
361 {
362 Desire d =
363 incentive.determineDesire(behavioralCharacteristics, perception, carFollowingModel, mandatoryDesire,
364 voluntaryDesire);
365 dLeftVoluntary += d.getLeft();
366 dRightVoluntary += d.getRight();
367 voluntaryDesire = new Desire(dLeftVoluntary, dRightVoluntary);
368 }
369
370
371 double thetaLeft = 0;
372 if (dLeftMandatory <= dSync || dLeftMandatory * dLeftVoluntary >= 0)
373 {
374
375 thetaLeft = 1;
376 }
377 else if (dSync < dLeftMandatory && dLeftMandatory < dCoop && dLeftMandatory * dLeftVoluntary < 0)
378 {
379
380 thetaLeft = (dCoop - Math.abs(dLeftMandatory)) / (dCoop - dSync);
381 }
382 double thetaRight = 0;
383 if (dRightMandatory <= dSync || dRightMandatory * dRightVoluntary >= 0)
384 {
385
386 thetaRight = 1;
387 }
388 else if (dSync < dRightMandatory && dRightMandatory < dCoop && dRightMandatory * dRightVoluntary < 0)
389 {
390
391 thetaRight = (dCoop - Math.abs(dRightMandatory)) / (dCoop - dSync);
392 }
393
394 return new Desire(dLeftMandatory + thetaLeft * dLeftVoluntary, dRightMandatory + thetaRight * dRightVoluntary);
395
396 }
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411 private static boolean acceptGap(final LanePerception perception, final BehavioralCharacteristics bc,
412 final SpeedLimitInfo sli, final CarFollowingModel cfm, final double desire, final Speed ownSpeed,
413 final LateralDirectionality lat) throws ParameterException, OperationalPlanException
414 {
415 Acceleration b = bc.getParameter(ParameterTypes.B);
416 if (perception.getPerceptionCategory(InfrastructurePerception.class).getLegalLaneChangePossibility(
417 RelativeLane.CURRENT, LateralDirectionality.LEFT).si > 0)
418 {
419 Acceleration aFollow = new Acceleration(Double.POSITIVE_INFINITY, AccelerationUnit.SI);
420 for (AbstractHeadwayGTU follower : perception.getPerceptionCategory(NeighborsPerception.class).getFirstFollowers(
421 lat))
422 {
423 Acceleration a =
424 singleAcceleration(follower.getDistance(), follower.getSpeed(), ownSpeed, desire, follower
425 .getBehavioralCharacteristics(), follower.getSpeedLimitInfo(), follower.getCarFollowingModel());
426 aFollow = Acceleration.min(aFollow, a);
427 }
428 Acceleration aSelf = new Acceleration(Double.POSITIVE_INFINITY, AccelerationUnit.SI);
429 for (AbstractHeadwayGTU leader : perception.getPerceptionCategory(NeighborsPerception.class).getFirstLeaders(lat))
430 {
431 Acceleration a = singleAcceleration(leader.getDistance(), ownSpeed, leader.getSpeed(), desire, bc, sli, cfm);
432 aSelf = Acceleration.min(aSelf, a);
433 }
434 Acceleration threshold = b.multiplyBy(-desire);
435 return aFollow.ge(threshold) && aSelf.ge(threshold);
436 }
437 return false;
438 }
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453 private static Acceleration synchronize(final LanePerception perception, final BehavioralCharacteristics bc,
454 final SpeedLimitInfo sli, final CarFollowingModel cfm, final double desire, final Speed ownSpeed,
455 final LateralDirectionality lat) throws ParameterException, OperationalPlanException
456 {
457 Acceleration b = bc.getParameter(ParameterTypes.B);
458 Acceleration a = new Acceleration(Double.POSITIVE_INFINITY, AccelerationUnit.SI);
459 for (AbstractHeadwayGTU leader : perception.getPerceptionCategory(NeighborsPerception.class).getFirstLeaders(lat))
460 {
461 Acceleration aSingle =
462 singleAcceleration(leader.getDistance(), ownSpeed, leader.getSpeed(), desire, bc, sli, cfm);
463 a = Acceleration.min(a, aSingle);
464 }
465 return Acceleration.max(a, b.multiplyBy(-1));
466 }
467
468
469
470
471
472
473
474
475
476
477
478
479
480 private static Acceleration cooperate(final LanePerception perception, final BehavioralCharacteristics bc,
481 final SpeedLimitInfo sli, final CarFollowingModel cfm, final Speed ownSpeed, final LateralDirectionality lat)
482 throws ParameterException, OperationalPlanException
483 {
484 Acceleration b = bc.getParameter(ParameterTypes.B);
485 Acceleration a = new Acceleration(Double.POSITIVE_INFINITY, AccelerationUnit.SI);
486 for (AbstractHeadwayGTU leader : perception.getPerceptionCategory(NeighborsPerception.class).getFirstLeaders(lat))
487 {
488 if ((lat == LateralDirectionality.LEFT && leader.isRightTurnIndicatorOn())
489 || (lat == LateralDirectionality.RIGHT && leader.isLeftTurnIndicatorOn()))
490 {
491 BehavioralCharacteristics bc2 = leader.getBehavioralCharacteristics();
492 double desire =
493 lat == LateralDirectionality.LEFT && bc2.contains(DRIGHT) ? bc2.getParameter(DRIGHT)
494 : lat == LateralDirectionality.RIGHT && bc2.contains(DLEFT) ? bc2.getParameter(DLEFT) : 0;
495 Acceleration aSingle =
496 singleAcceleration(leader.getDistance(), ownSpeed, leader.getSpeed(), desire, bc, sli, cfm);
497 a = Acceleration.min(a, aSingle);
498 }
499 }
500 return Acceleration.max(a, b.multiplyBy(-1));
501 }
502
503
504
505
506
507
508
509
510
511
512
513
514
515 private static Acceleration singleAcceleration(final Length distance, final Speed followerSpeed,
516 final Speed leaderSpeed, final double desire, final BehavioralCharacteristics bc, final SpeedLimitInfo sli,
517 final CarFollowingModel cfm) throws ParameterException
518 {
519
520 bc.setParameter(ParameterTypes.T, Duration.interpolate(bc.getParameter(ParameterTypes.TMAX), bc
521 .getParameter(ParameterTypes.TMIN), desire));
522
523 SortedMap<Length, Speed> leaders = new TreeMap<>();
524 leaders.put(distance, leaderSpeed);
525 Acceleration a = cfm.followingAcceleration(bc, followerSpeed, sli, leaders);
526
527 bc.resetParameter(ParameterTypes.T);
528 return a;
529 }
530
531
532
533
534
535
536
537
538
539
540
541
542 public static class LmrsStatus implements Serializable
543 {
544
545
546 private static final long serialVersionUID = 20160811L;
547
548
549 private Set<AbstractHeadwayGTU> lastLeaders = new HashSet<>();
550
551
552
553
554 public final Set<AbstractHeadwayGTU> getLastLeaders()
555 {
556 return new HashSet<>(this.lastLeaders);
557 }
558
559
560
561
562 public final void setLastLeaders(final Set<AbstractHeadwayGTU> lastLeaders)
563 {
564 this.lastLeaders.clear();
565 this.lastLeaders.addAll(lastLeaders);
566 }
567
568
569 @Override
570 public final String toString()
571 {
572 return "LmrsStatus";
573 }
574
575 }
576
577 }