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