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