1 package org.opentrafficsim.road.gtu.lane.tactical.util.lmrs;
2
3 import java.util.Iterator;
4 import java.util.Map;
5 import java.util.SortedSet;
6
7 import org.djunits.value.vdouble.scalar.Acceleration;
8 import org.djunits.value.vdouble.scalar.Duration;
9 import org.djunits.value.vdouble.scalar.Length;
10 import org.djunits.value.vdouble.scalar.Speed;
11 import org.djunits.value.vdouble.scalar.Time;
12 import org.djutils.exceptions.Try;
13 import org.opentrafficsim.base.parameters.ParameterException;
14 import org.opentrafficsim.base.parameters.ParameterTypeAcceleration;
15 import org.opentrafficsim.base.parameters.ParameterTypeDuration;
16 import org.opentrafficsim.base.parameters.ParameterTypes;
17 import org.opentrafficsim.base.parameters.Parameters;
18 import org.opentrafficsim.core.gtu.GtuException;
19 import org.opentrafficsim.core.gtu.TurnIndicatorIntent;
20 import org.opentrafficsim.core.gtu.perception.EgoPerception;
21 import org.opentrafficsim.core.gtu.plan.operational.OperationalPlanException;
22 import org.opentrafficsim.core.network.LateralDirectionality;
23 import org.opentrafficsim.core.network.NetworkException;
24 import org.opentrafficsim.road.gtu.lane.LaneBasedGtu;
25 import org.opentrafficsim.road.gtu.lane.perception.InfrastructureLaneChangeInfo;
26 import org.opentrafficsim.road.gtu.lane.perception.LanePerception;
27 import org.opentrafficsim.road.gtu.lane.perception.PerceptionCollectable;
28 import org.opentrafficsim.road.gtu.lane.perception.RelativeLane;
29 import org.opentrafficsim.road.gtu.lane.perception.categories.InfrastructurePerception;
30 import org.opentrafficsim.road.gtu.lane.perception.categories.IntersectionPerception;
31 import org.opentrafficsim.road.gtu.lane.perception.categories.neighbors.NeighborsPerception;
32 import org.opentrafficsim.road.gtu.lane.perception.headway.HeadwayConflict;
33 import org.opentrafficsim.road.gtu.lane.perception.headway.HeadwayGtu;
34 import org.opentrafficsim.road.gtu.lane.perception.headway.HeadwayTrafficLight;
35 import org.opentrafficsim.road.gtu.lane.plan.operational.LaneChange;
36 import org.opentrafficsim.road.gtu.lane.plan.operational.SimpleOperationalPlan;
37 import org.opentrafficsim.road.gtu.lane.tactical.Synchronizable;
38 import org.opentrafficsim.road.gtu.lane.tactical.following.CarFollowingModel;
39 import org.opentrafficsim.road.gtu.lane.tactical.util.CarFollowingUtil;
40 import org.opentrafficsim.road.gtu.lane.tactical.util.ConflictUtil;
41 import org.opentrafficsim.road.gtu.lane.tactical.util.ConflictUtil.ConflictPlans;
42 import org.opentrafficsim.road.network.lane.conflict.Conflict;
43 import org.opentrafficsim.road.network.speed.SpeedLimitInfo;
44 import org.opentrafficsim.road.network.speed.SpeedLimitProspect;
45
46
47
48
49
50
51
52
53
54
55 public final class LmrsUtil implements LmrsParameters
56 {
57
58
59 public static final ParameterTypeDuration DT = ParameterTypes.DT;
60
61
62 public static final ParameterTypeDuration TMIN = ParameterTypes.TMIN;
63
64
65 public static final ParameterTypeDuration T = ParameterTypes.T;
66
67
68 public static final ParameterTypeDuration TMAX = ParameterTypes.TMAX;
69
70
71 public static final ParameterTypeDuration TAU = ParameterTypes.TAU;
72
73
74 public static final ParameterTypeAcceleration BCRIT = ParameterTypes.BCRIT;
75
76
77
78
79 private LmrsUtil()
80 {
81
82 }
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100 @SuppressWarnings({"checkstyle:parameternumber", "checkstyle:methodlength"})
101 public static SimpleOperationalPlan determinePlan(final LaneBasedGtu gtu, final Time startTime,
102 final CarFollowingModel carFollowingModel, final LaneChange laneChange, final LmrsData lmrsData,
103 final LanePerception perception, final Iterable<MandatoryIncentive> mandatoryIncentives,
104 final Iterable<VoluntaryIncentive> voluntaryIncentives)
105 throws GtuException, NetworkException, ParameterException, OperationalPlanException
106 {
107
108 InfrastructurePerception infra = perception.getPerceptionCategory(InfrastructurePerception.class);
109 SpeedLimitProspect slp = infra.getSpeedLimitProspect(RelativeLane.CURRENT);
110 SpeedLimitInfo sli = slp.getSpeedLimitInfo(Length.ZERO);
111 Parameters params = gtu.getParameters();
112 EgoPerception<?, ?> ego = perception.getPerceptionCategory(EgoPerception.class);
113 Speed speed = ego.getSpeed();
114 NeighborsPerception neighbors = perception.getPerceptionCategory(NeighborsPerception.class);
115 PerceptionCollectable<HeadwayGtu, LaneBasedGtu> leaders = neighbors.getLeaders(RelativeLane.CURRENT);
116
117
118 Acceleration a;
119 if (lmrsData.isHumanLongitudinalControl())
120 {
121 lmrsData.getTailgating().tailgate(perception, params);
122 if (!leaders.isEmpty() && lmrsData.isNewLeader(leaders.first()))
123 {
124 initHeadwayRelaxation(params, leaders.first());
125 }
126 a = gtu.getCarFollowingAcceleration();
127 }
128 else
129 {
130 a = Acceleration.POS_MAXVALUE;
131 }
132
133
134 LateralDirectionality initiatedLaneChange;
135 TurnIndicatorIntent turnIndicatorStatus = TurnIndicatorIntent.NONE;
136 if (laneChange.isChangingLane())
137 {
138 initiatedLaneChange = LateralDirectionality.NONE;
139 if (lmrsData.isHumanLongitudinalControl())
140 {
141 RelativeLane secondLane = laneChange.getSecondLane(gtu);
142 PerceptionCollectable<HeadwayGtu, LaneBasedGtu> secondLeaders = neighbors.getLeaders(secondLane);
143 Acceleration aSecond = carFollowingModel.followingAcceleration(params, speed, sli, secondLeaders);
144 if (!secondLeaders.isEmpty() && lmrsData.isNewLeader(secondLeaders.first()))
145 {
146 initHeadwayRelaxation(params, secondLeaders.first());
147 }
148 a = Acceleration.min(a, aSecond);
149 }
150 }
151 else
152 {
153
154
155 Desire desire = getLaneChangeDesire(params, perception, carFollowingModel, mandatoryIncentives, voluntaryIncentives,
156 lmrsData.getDesireMap());
157
158
159 double dFree = params.getParameter(DFREE);
160 initiatedLaneChange = LateralDirectionality.NONE;
161 turnIndicatorStatus = TurnIndicatorIntent.NONE;
162 if (desire.leftIsLargerOrEqual() && desire.left() >= dFree)
163 {
164 if (acceptLaneChange(perception, params, sli, carFollowingModel, desire.left(), speed, a,
165 LateralDirectionality.LEFT, lmrsData.getGapAcceptance(), laneChange))
166 {
167
168 initiatedLaneChange = LateralDirectionality.LEFT;
169 turnIndicatorStatus = TurnIndicatorIntent.LEFT;
170 params.setParameter(DLC, desire.left());
171 setDesiredHeadway(params, desire.left());
172 leaders = neighbors.getLeaders(RelativeLane.LEFT);
173 if (!leaders.isEmpty())
174 {
175
176
177 lmrsData.isNewLeader(leaders.first());
178 }
179 a = Acceleration.min(a, carFollowingModel.followingAcceleration(params, speed, sli,
180 neighbors.getLeaders(RelativeLane.LEFT)));
181 }
182 }
183 else if (!desire.leftIsLargerOrEqual() && desire.right() >= dFree)
184 {
185 if (acceptLaneChange(perception, params, sli, carFollowingModel, desire.right(), speed, a,
186 LateralDirectionality.RIGHT, lmrsData.getGapAcceptance(), laneChange))
187 {
188
189 initiatedLaneChange = LateralDirectionality.RIGHT;
190 turnIndicatorStatus = TurnIndicatorIntent.RIGHT;
191 params.setParameter(DLC, desire.right());
192 setDesiredHeadway(params, desire.right());
193 leaders = neighbors.getLeaders(RelativeLane.RIGHT);
194 if (!leaders.isEmpty())
195 {
196
197
198 lmrsData.isNewLeader(leaders.first());
199 }
200 a = Acceleration.min(a, carFollowingModel.followingAcceleration(params, speed, sli,
201 neighbors.getLeaders(RelativeLane.RIGHT)));
202 }
203 }
204 if (!initiatedLaneChange.isNone())
205 {
206 SortedSet<InfrastructureLaneChangeInfo> set = infra.getInfrastructureLaneChangeInfo(RelativeLane.CURRENT);
207 if (!set.isEmpty())
208 {
209 Length boundary = null;
210 for (InfrastructureLaneChangeInfo info : set)
211 {
212 int n = info.getRequiredNumberOfLaneChanges();
213 if (n > 1)
214 {
215 Length thisBoundary = info.getRemainingDistance()
216 .minus(Synchronization.requiredBufferSpace(speed, info.getRequiredNumberOfLaneChanges(),
217 params.getParameter(ParameterTypes.LOOKAHEAD),
218 params.getParameter(ParameterTypes.T0), params.getParameter(ParameterTypes.LCDUR),
219 params.getParameter(DCOOP)));
220 if (thisBoundary.le0())
221 {
222 thisBoundary = info.getRemainingDistance().divide(info.getRequiredNumberOfLaneChanges());
223 }
224 boundary = boundary == null || thisBoundary.si < boundary.si ? thisBoundary : boundary;
225 }
226 }
227 laneChange.setBoundary(boundary);
228 }
229 params.setParameter(DLEFT, 0.0);
230 params.setParameter(DRIGHT, 0.0);
231 }
232 else
233 {
234 params.setParameter(DLEFT, desire.left());
235 params.setParameter(DRIGHT, desire.right());
236 }
237
238
239 Acceleration aSync;
240
241
242 double dSync = params.getParameter(DSYNC);
243 if (desire.leftIsLargerOrEqual() && desire.left() >= dSync)
244 {
245 Synchronizable.State state;
246 if (desire.left() >= params.getParameter(DCOOP))
247 {
248
249 turnIndicatorStatus = TurnIndicatorIntent.LEFT;
250 state = Synchronizable.State.INDICATING;
251 }
252 else
253 {
254 state = Synchronizable.State.SYNCHRONIZING;
255 }
256 aSync = lmrsData.getSynchronization().synchronize(perception, params, sli, carFollowingModel, desire.left(),
257 LateralDirectionality.LEFT, lmrsData, laneChange, initiatedLaneChange);
258 a = applyAcceleration(a, aSync, lmrsData, state);
259 }
260 else if (!desire.leftIsLargerOrEqual() && desire.right() >= dSync)
261 {
262 Synchronizable.State state;
263 if (desire.right() >= params.getParameter(DCOOP))
264 {
265
266 turnIndicatorStatus = TurnIndicatorIntent.RIGHT;
267 state = Synchronizable.State.INDICATING;
268 }
269 else
270 {
271 state = Synchronizable.State.SYNCHRONIZING;
272 }
273 aSync = lmrsData.getSynchronization().synchronize(perception, params, sli, carFollowingModel, desire.right(),
274 LateralDirectionality.RIGHT, lmrsData, laneChange, initiatedLaneChange);
275 a = applyAcceleration(a, aSync, lmrsData, state);
276 }
277 else
278 {
279 lmrsData.setSynchronizationState(Synchronizable.State.NONE);
280 }
281
282
283 aSync = lmrsData.getCooperation().cooperate(perception, params, sli, carFollowingModel, LateralDirectionality.LEFT,
284 desire);
285 a = applyAcceleration(a, aSync, lmrsData, Synchronizable.State.COOPERATING);
286 aSync = lmrsData.getCooperation().cooperate(perception, params, sli, carFollowingModel, LateralDirectionality.RIGHT,
287 desire);
288 a = applyAcceleration(a, aSync, lmrsData, Synchronizable.State.COOPERATING);
289
290
291 exponentialHeadwayRelaxation(params);
292
293 }
294 lmrsData.finalizeStep();
295
296 SimpleOperationalPlan simplePlan = new SimpleOperationalPlan(a, params.getParameter(DT), initiatedLaneChange);
297 if (turnIndicatorStatus.isLeft())
298 {
299 simplePlan.setIndicatorIntentLeft();
300 }
301 else if (turnIndicatorStatus.isRight())
302 {
303 simplePlan.setIndicatorIntentRight();
304 }
305 return simplePlan;
306
307 }
308
309
310
311
312
313
314
315
316
317 private static Acceleration applyAcceleration(final Acceleration a, final Acceleration aNew, final LmrsData lmrsData,
318 final Synchronizable.State state)
319 {
320 if (a.si < aNew.si)
321 {
322 return a;
323 }
324 lmrsData.setSynchronizationState(state);
325 return aNew;
326 }
327
328
329
330
331
332
333
334 private static void initHeadwayRelaxation(final Parameters params, final HeadwayGtu leader) throws ParameterException
335 {
336 Double dlc = leader.getParameters().getParameterOrNull(DLC);
337 if (dlc != null)
338 {
339 setDesiredHeadway(params, dlc);
340 }
341
342 }
343
344
345
346
347
348
349 private static void exponentialHeadwayRelaxation(final Parameters params) throws ParameterException
350 {
351 double ratio = params.getParameter(DT).si / params.getParameter(TAU).si;
352 params.setParameter(T,
353 Duration.interpolate(params.getParameter(T), params.getParameter(TMAX), ratio <= 1.0 ? ratio : 1.0));
354 }
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373 public static Desire getLaneChangeDesire(final Parameters parameters, final LanePerception perception,
374 final CarFollowingModel carFollowingModel, final Iterable<MandatoryIncentive> mandatoryIncentives,
375 final Iterable<VoluntaryIncentive> voluntaryIncentives, final Map<Class<? extends Incentive>, Desire> desireMap)
376 throws ParameterException, GtuException, OperationalPlanException
377 {
378
379 double dSync = parameters.getParameter(DSYNC);
380 double dCoop = parameters.getParameter(DCOOP);
381
382
383 double dLeftMandatory = 0.0;
384 double dRightMandatory = 0.0;
385 Desire mandatoryDesire = new Desire(dLeftMandatory, dRightMandatory);
386 for (MandatoryIncentive incentive : mandatoryIncentives)
387 {
388 Desire d = incentive.determineDesire(parameters, perception, carFollowingModel, mandatoryDesire);
389 desireMap.put(incentive.getClass(), d);
390 dLeftMandatory = Math.abs(d.left()) > Math.abs(dLeftMandatory) ? d.left() : dLeftMandatory;
391 dRightMandatory = Math.abs(d.right()) > Math.abs(dRightMandatory) ? d.right() : dRightMandatory;
392 mandatoryDesire = new Desire(dLeftMandatory, dRightMandatory);
393 }
394
395
396 double dLeftVoluntary = 0;
397 double dRightVoluntary = 0;
398 Desire voluntaryDesire = new Desire(dLeftVoluntary, dRightVoluntary);
399 for (VoluntaryIncentive incentive : voluntaryIncentives)
400 {
401 Desire d = incentive.determineDesire(parameters, perception, carFollowingModel, mandatoryDesire, voluntaryDesire);
402 desireMap.put(incentive.getClass(), d);
403 dLeftVoluntary += d.left();
404 dRightVoluntary += d.right();
405 voluntaryDesire = new Desire(dLeftVoluntary, dRightVoluntary);
406 }
407
408
409 double thetaLeft = 0;
410 double dLeftMandatoryAbs = Math.abs(dLeftMandatory);
411 double dRightMandatoryAbs = Math.abs(dRightMandatory);
412 if (dLeftMandatoryAbs <= dSync || dLeftMandatory * dLeftVoluntary >= 0)
413 {
414
415 thetaLeft = 1;
416 }
417 else if (dSync < dLeftMandatoryAbs && dLeftMandatoryAbs < dCoop && dLeftMandatory * dLeftVoluntary < 0)
418 {
419
420 thetaLeft = (dCoop - dLeftMandatoryAbs) / (dCoop - dSync);
421 }
422 double thetaRight = 0;
423 if (dRightMandatoryAbs <= dSync || dRightMandatory * dRightVoluntary >= 0)
424 {
425
426 thetaRight = 1;
427 }
428 else if (dSync < dRightMandatoryAbs && dRightMandatoryAbs < dCoop && dRightMandatory * dRightVoluntary < 0)
429 {
430
431 thetaRight = (dCoop - dRightMandatoryAbs) / (dCoop - dSync);
432 }
433 return new Desire(dLeftMandatory + thetaLeft * dLeftVoluntary, dRightMandatory + thetaRight * dRightVoluntary);
434
435 }
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453 static boolean acceptLaneChange(final LanePerception perception, final Parameters params, final SpeedLimitInfo sli,
454 final CarFollowingModel cfm, final double desire, final Speed ownSpeed, final Acceleration ownAcceleration,
455 final LateralDirectionality lat, final GapAcceptance gapAcceptance, final LaneChange laneChange)
456 throws ParameterException, OperationalPlanException
457 {
458
459 LaneBasedGtu gtu = Try.assign(() -> perception.getGtu(), "Cannot obtain GTU.");
460 if (!gtu.laneChangeAllowed())
461 {
462 return false;
463 }
464
465
466 InfrastructurePerception infra = perception.getPerceptionCategory(InfrastructurePerception.class);
467 if (infra.getLegalLaneChangePossibility(RelativeLane.CURRENT, lat).si <= 0.0)
468 {
469 return false;
470 }
471
472
473 NeighborsPerception neighbors = perception.getPerceptionCategoryOrNull(NeighborsPerception.class);
474 PerceptionCollectable<HeadwayGtu, LaneBasedGtu> leaders = neighbors.getLeaders(RelativeLane.CURRENT);
475 if (!leaders.isEmpty())
476 {
477 boolean ok = laneChange.checkRoom(gtu, leaders.first());
478 if (!ok)
479 {
480 return false;
481 }
482 }
483 RelativeLane lane = new RelativeLane(lat, 1);
484 leaders = neighbors.getLeaders(lane);
485 if (!leaders.isEmpty())
486 {
487 boolean ok = laneChange.checkRoom(gtu, leaders.first());
488 if (!ok)
489 {
490 return false;
491 }
492 }
493
494
495 IntersectionPerception intersection = perception.getPerceptionCategoryOrNull(IntersectionPerception.class);
496 if (intersection != null)
497 {
498
499
500
501
502
503
504
505
506
507
508
509
510 EgoPerception<?, ?> ego = perception.getPerceptionCategoryOrNull(EgoPerception.class);
511 PerceptionCollectable<HeadwayConflict, Conflict> conflicts = intersection.getConflicts(lane);
512 try
513 {
514 Acceleration a = ConflictUtil.approachConflicts(params, conflicts, leaders, cfm, ego.getLength(),
515 ego.getWidth(), ownSpeed, ownAcceleration, sli, new ConflictPlans(), perception.getGtu(), lane);
516 if (a.lt(params.getParameter(ParameterTypes.BCRIT).neg()))
517 {
518 return false;
519 }
520
521
522 for (HeadwayConflict conflict : conflicts)
523 {
524 if (conflict.isMerge() && conflict.getDistance().si < 10.0)
525 {
526 PerceptionCollectable<HeadwayGtu, LaneBasedGtu> down = conflict.getDownstreamConflictingGTUs();
527 if (!down.isEmpty() && down.first().isParallel())
528 {
529 return false;
530 }
531 PerceptionCollectable<HeadwayGtu, LaneBasedGtu> up = conflict.getUpstreamConflictingGTUs();
532 if (!up.isEmpty() && up.first().isParallel())
533 {
534 return false;
535 }
536 }
537 }
538 }
539 catch (GtuException exception)
540 {
541 throw new OperationalPlanException(exception);
542 }
543 conflicts = intersection.getConflicts(RelativeLane.CURRENT);
544 for (HeadwayConflict conflict : conflicts)
545 {
546 if (conflict.getLane().getLink().equals(conflict.getConflictingLink()))
547 {
548 if (conflict.isMerge() && conflict.getDistance().le0()
549 && conflict.getDistance().neg().gt(conflict.getLength()))
550 {
551 return false;
552 }
553 else if (conflict.isSplit() && conflict.getDistance().le0()
554 && conflict.getDistance().neg().lt(gtu.getLength()))
555 {
556 return false;
557 }
558 }
559 }
560
561
562 Iterable<HeadwayTrafficLight> trafficLights = intersection.getTrafficLights(lane);
563 for (HeadwayTrafficLight trafficLight : trafficLights)
564 {
565 if (trafficLight.getTrafficLightColor().isRedOrYellow())
566 {
567 boolean ok = laneChange.checkRoom(gtu, trafficLight);
568 if (!ok)
569 {
570 return false;
571 }
572 }
573 }
574 }
575
576
577 return gapAcceptance.acceptGap(perception, params, sli, cfm, desire, ownSpeed, ownAcceleration, lat);
578
579 }
580
581
582
583
584
585
586
587
588
589
590
591
592 private static Acceleration quickIntersectionScan(final Parameters params, final SpeedLimitInfo sli,
593 final CarFollowingModel cfm, final Speed ownSpeed, final LateralDirectionality lat,
594 final IntersectionPerception intersection) throws ParameterException
595 {
596 Acceleration a = Acceleration.POSITIVE_INFINITY;
597 if (intersection != null)
598 {
599 RelativeLane lane = lat.isRight() ? RelativeLane.RIGHT : RelativeLane.LEFT;
600 Iterable<HeadwayConflict> iterable = intersection.getConflicts(lane);
601 if (iterable != null)
602 {
603 Iterator<HeadwayConflict> conflicts = iterable.iterator();
604 if (conflicts.hasNext())
605 {
606 a = Acceleration.min(a, CarFollowingUtil.followSingleLeader(cfm, params, ownSpeed, sli,
607 conflicts.next().getDistance(), Speed.ZERO));
608 }
609 Iterator<HeadwayTrafficLight> trafficLights = intersection.getTrafficLights(lane).iterator();
610 if (trafficLights.hasNext())
611 {
612 HeadwayTrafficLight trafficLight = trafficLights.next();
613 if (trafficLight.getTrafficLightColor().isRedOrYellow())
614 {
615 a = Acceleration.min(a, CarFollowingUtil.followSingleLeader(cfm, params, ownSpeed, sli,
616 trafficLight.getDistance(), Speed.ZERO));
617 }
618 }
619 }
620 }
621 return a;
622 }
623
624
625
626
627
628
629
630 static void setDesiredHeadway(final Parameters params, final double desire) throws ParameterException
631 {
632 double limitedDesire = desire < 0 ? 0 : desire > 1 ? 1 : desire;
633 double tDes = limitedDesire * params.getParameter(TMIN).si + (1 - limitedDesire) * params.getParameter(TMAX).si;
634 double t = params.getParameter(T).si;
635 params.setParameterResettable(T, Duration.instantiateSI(tDes < t ? tDes : t));
636 }
637
638
639
640
641
642
643 static void resetDesiredHeadway(final Parameters params) throws ParameterException
644 {
645 params.resetParameter(T);
646 }
647
648
649
650
651
652
653
654
655
656
657
658
659
660 public static Acceleration singleAcceleration(final Length distance, final Speed followerSpeed, final Speed leaderSpeed,
661 final double desire, final Parameters params, final SpeedLimitInfo sli, final CarFollowingModel cfm)
662 throws ParameterException
663 {
664
665 setDesiredHeadway(params, desire);
666
667 Acceleration a = CarFollowingUtil.followSingleLeader(cfm, params, followerSpeed, sli, distance, leaderSpeed);
668
669 resetDesiredHeadway(params);
670 return a;
671 }
672
673 }