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.LanePerception;
26 import org.opentrafficsim.road.gtu.lane.perception.PerceptionCollectable;
27 import org.opentrafficsim.road.gtu.lane.perception.RelativeLane;
28 import org.opentrafficsim.road.gtu.lane.perception.categories.InfrastructurePerception;
29 import org.opentrafficsim.road.gtu.lane.perception.categories.IntersectionPerception;
30 import org.opentrafficsim.road.gtu.lane.perception.categories.neighbors.NeighborsPerception;
31 import org.opentrafficsim.road.gtu.lane.perception.headway.HeadwayConflict;
32 import org.opentrafficsim.road.gtu.lane.perception.headway.HeadwayGtu;
33 import org.opentrafficsim.road.gtu.lane.perception.headway.HeadwayTrafficLight;
34 import org.opentrafficsim.road.gtu.lane.plan.operational.LaneChange;
35 import org.opentrafficsim.road.gtu.lane.plan.operational.SimpleOperationalPlan;
36 import org.opentrafficsim.road.gtu.lane.tactical.Synchronizable;
37 import org.opentrafficsim.road.gtu.lane.tactical.following.CarFollowingModel;
38 import org.opentrafficsim.road.gtu.lane.tactical.util.CarFollowingUtil;
39 import org.opentrafficsim.road.gtu.lane.tactical.util.ConflictUtil;
40 import org.opentrafficsim.road.gtu.lane.tactical.util.ConflictUtil.ConflictPlans;
41 import org.opentrafficsim.road.network.LaneChangeInfo;
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 a = Acceleration.min(a, Synchronization.DEADEND.synchronize(perception, params, sli, carFollowingModel, 0.0,
151 laneChange.getDirection(), lmrsData, laneChange, initiatedLaneChange));
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.left() >= dFree)
165 {
166 if (acceptLaneChange(perception, params, sli, carFollowingModel, desire.left(), speed, a,
167 LateralDirectionality.LEFT, lmrsData.getGapAcceptance(), laneChange))
168 {
169
170 initiatedLaneChange = LateralDirectionality.LEFT;
171 turnIndicatorStatus = TurnIndicatorIntent.LEFT;
172 params.setParameter(DLC, desire.left());
173 setDesiredHeadway(params, desire.left());
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.right() >= dFree)
186 {
187 if (acceptLaneChange(perception, params, sli, carFollowingModel, desire.right(), speed, a,
188 LateralDirectionality.RIGHT, lmrsData.getGapAcceptance(), laneChange))
189 {
190
191 initiatedLaneChange = LateralDirectionality.RIGHT;
192 turnIndicatorStatus = TurnIndicatorIntent.RIGHT;
193 params.setParameter(DLC, desire.right());
194 setDesiredHeadway(params, desire.right());
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<LaneChangeInfo> set = infra.getLegalLaneChangeInfo(RelativeLane.CURRENT);
209 if (!set.isEmpty())
210 {
211 Length boundary = null;
212 for (LaneChangeInfo info : set)
213 {
214 int n = info.numberOfLaneChanges();
215 if (n > 1)
216 {
217 Length thisBoundary = info.remainingDistance()
218 .minus(Synchronization.requiredBufferSpace(speed, info.numberOfLaneChanges(),
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.remainingDistance().divide(info.numberOfLaneChanges());
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.left());
237 params.setParameter(DRIGHT, desire.right());
238 }
239
240
241 Acceleration aSync;
242
243
244 double dSync = params.getParameter(DSYNC);
245 lmrsData.setSynchronizationState(Synchronizable.State.NONE);
246 if (desire.leftIsLargerOrEqual() && desire.left() >= dSync)
247 {
248 Synchronizable.State state;
249 if (desire.left() >= params.getParameter(DCOOP))
250 {
251
252 turnIndicatorStatus = TurnIndicatorIntent.LEFT;
253 state = Synchronizable.State.INDICATING;
254 }
255 else
256 {
257 state = Synchronizable.State.SYNCHRONIZING;
258 }
259 aSync = lmrsData.getSynchronization().synchronize(perception, params, sli, carFollowingModel, desire.left(),
260 LateralDirectionality.LEFT, lmrsData, laneChange, initiatedLaneChange);
261 a = applyAcceleration(a, aSync, lmrsData, state);
262 }
263 else if (!desire.leftIsLargerOrEqual() && desire.right() >= dSync)
264 {
265 Synchronizable.State state;
266 if (desire.right() >= params.getParameter(DCOOP))
267 {
268
269 turnIndicatorStatus = TurnIndicatorIntent.RIGHT;
270 state = Synchronizable.State.INDICATING;
271 }
272 else
273 {
274 state = Synchronizable.State.SYNCHRONIZING;
275 }
276 aSync = lmrsData.getSynchronization().synchronize(perception, params, sli, carFollowingModel, desire.right(),
277 LateralDirectionality.RIGHT, lmrsData, laneChange, initiatedLaneChange);
278 a = applyAcceleration(a, aSync, lmrsData, state);
279 }
280
281
282 aSync = lmrsData.getCooperation().cooperate(perception, params, sli, carFollowingModel, LateralDirectionality.LEFT,
283 desire);
284 a = applyAcceleration(a, aSync, lmrsData, Synchronizable.State.COOPERATING);
285 aSync = lmrsData.getCooperation().cooperate(perception, params, sli, carFollowingModel, LateralDirectionality.RIGHT,
286 desire);
287 a = applyAcceleration(a, aSync, lmrsData, Synchronizable.State.COOPERATING);
288
289
290 exponentialHeadwayRelaxation(params);
291
292 }
293 lmrsData.finalizeStep();
294
295 SimpleOperationalPlan simplePlan = new SimpleOperationalPlan(a, params.getParameter(DT), initiatedLaneChange);
296 if (turnIndicatorStatus.isLeft())
297 {
298 simplePlan.setIndicatorIntentLeft();
299 }
300 else if (turnIndicatorStatus.isRight())
301 {
302 simplePlan.setIndicatorIntentRight();
303 }
304 return simplePlan;
305
306 }
307
308
309
310
311
312
313
314
315
316 private static Acceleration applyAcceleration(final Acceleration a, final Acceleration aNew, final LmrsData lmrsData,
317 final Synchronizable.State state)
318 {
319 if (a.si < aNew.si)
320 {
321 return a;
322 }
323 lmrsData.setSynchronizationState(state);
324 return aNew;
325 }
326
327
328
329
330
331
332
333 private static void initHeadwayRelaxation(final Parameters params, final HeadwayGtu leader) throws ParameterException
334 {
335 Double dlc = leader.getParameters().getParameterOrNull(DLC);
336 if (dlc != null)
337 {
338 setDesiredHeadway(params, dlc);
339 }
340
341 }
342
343
344
345
346
347
348 private static void exponentialHeadwayRelaxation(final Parameters params) throws ParameterException
349 {
350 double ratio = params.getParameter(DT).si / params.getParameter(TAU).si;
351 params.setParameter(T,
352 Duration.interpolate(params.getParameter(T), params.getParameter(TMAX), ratio <= 1.0 ? ratio : 1.0));
353 }
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372 public static Desire getLaneChangeDesire(final Parameters parameters, final LanePerception perception,
373 final CarFollowingModel carFollowingModel, final Iterable<MandatoryIncentive> mandatoryIncentives,
374 final Iterable<VoluntaryIncentive> voluntaryIncentives, final Map<Class<? extends Incentive>, Desire> desireMap)
375 throws ParameterException, GtuException, OperationalPlanException
376 {
377
378 double dSync = parameters.getParameter(DSYNC);
379 double dCoop = parameters.getParameter(DCOOP);
380
381
382 double dLeftMandatory = 0.0;
383 double dRightMandatory = 0.0;
384 Desire mandatoryDesire = new Desire(dLeftMandatory, dRightMandatory);
385 for (MandatoryIncentive incentive : mandatoryIncentives)
386 {
387 Desire d = incentive.determineDesire(parameters, perception, carFollowingModel, mandatoryDesire);
388 desireMap.put(incentive.getClass(), d);
389 dLeftMandatory = Math.abs(d.left()) > Math.abs(dLeftMandatory) ? d.left() : dLeftMandatory;
390 dRightMandatory = Math.abs(d.right()) > Math.abs(dRightMandatory) ? d.right() : dRightMandatory;
391 mandatoryDesire = new Desire(dLeftMandatory, dRightMandatory);
392 }
393
394
395 double dLeftVoluntary = 0;
396 double dRightVoluntary = 0;
397 Desire voluntaryDesire = new Desire(dLeftVoluntary, dRightVoluntary);
398 for (VoluntaryIncentive incentive : voluntaryIncentives)
399 {
400 Desire d = incentive.determineDesire(parameters, perception, carFollowingModel, mandatoryDesire, voluntaryDesire);
401 desireMap.put(incentive.getClass(), d);
402 dLeftVoluntary += d.left();
403 dRightVoluntary += d.right();
404 voluntaryDesire = new Desire(dLeftVoluntary, dRightVoluntary);
405 }
406
407
408 double thetaLeft = 0;
409 double dLeftMandatoryAbs = Math.abs(dLeftMandatory);
410 double dRightMandatoryAbs = Math.abs(dRightMandatory);
411 if (dLeftMandatoryAbs <= dSync || dLeftMandatory * dLeftVoluntary >= 0)
412 {
413
414 thetaLeft = 1;
415 }
416 else if (dSync < dLeftMandatoryAbs && dLeftMandatoryAbs < dCoop && dLeftMandatory * dLeftVoluntary < 0)
417 {
418
419 thetaLeft = (dCoop - dLeftMandatoryAbs) / (dCoop - dSync);
420 }
421 double thetaRight = 0;
422 if (dRightMandatoryAbs <= dSync || dRightMandatory * dRightVoluntary >= 0)
423 {
424
425 thetaRight = 1;
426 }
427 else if (dSync < dRightMandatoryAbs && dRightMandatoryAbs < dCoop && dRightMandatory * dRightVoluntary < 0)
428 {
429
430 thetaRight = (dCoop - dRightMandatoryAbs) / (dCoop - dSync);
431 }
432 return new Desire(dLeftMandatory + thetaLeft * dLeftVoluntary, dRightMandatory + thetaRight * dRightVoluntary);
433
434 }
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452 static boolean acceptLaneChange(final LanePerception perception, final Parameters params, final SpeedLimitInfo sli,
453 final CarFollowingModel cfm, final double desire, final Speed ownSpeed, final Acceleration ownAcceleration,
454 final LateralDirectionality lat, final GapAcceptance gapAcceptance, final LaneChange laneChange)
455 throws ParameterException, OperationalPlanException
456 {
457
458 LaneBasedGtu gtu = Try.assign(() -> perception.getGtu(), "Cannot obtain GTU.");
459 if (!gtu.laneChangeAllowed())
460 {
461 return false;
462 }
463
464
465 InfrastructurePerception infra = perception.getPerceptionCategory(InfrastructurePerception.class);
466 if (infra.getLegalLaneChangePossibility(RelativeLane.CURRENT, lat).si <= 0.0)
467 {
468 return false;
469 }
470
471
472 NeighborsPerception neighbors = perception.getPerceptionCategoryOrNull(NeighborsPerception.class);
473 PerceptionCollectable<HeadwayGtu, LaneBasedGtu> leaders = neighbors.getLeaders(RelativeLane.CURRENT);
474 if (!leaders.isEmpty())
475 {
476 boolean ok = laneChange.checkRoom(gtu, leaders.first());
477 if (!ok)
478 {
479 return false;
480 }
481 }
482 RelativeLane lane = new RelativeLane(lat, 1);
483 leaders = neighbors.getLeaders(lane);
484 if (!leaders.isEmpty())
485 {
486 boolean ok = laneChange.checkRoom(gtu, leaders.first());
487 if (!ok)
488 {
489 return false;
490 }
491 }
492
493
494 IntersectionPerception intersection = perception.getPerceptionCategoryOrNull(IntersectionPerception.class);
495 if (intersection != null)
496 {
497
498
499
500
501
502
503
504
505
506
507
508
509 EgoPerception<?, ?> ego = perception.getPerceptionCategoryOrNull(EgoPerception.class);
510 PerceptionCollectable<HeadwayConflict, Conflict> conflicts = intersection.getConflicts(lane);
511 try
512 {
513 Acceleration a = ConflictUtil.approachConflicts(params, conflicts, leaders, cfm, ego.getLength(),
514 ego.getWidth(), ownSpeed, ownAcceleration, sli, new ConflictPlans(), perception.getGtu(), lane);
515 if (a.lt(params.getParameter(ParameterTypes.BCRIT).neg()))
516 {
517 return false;
518 }
519
520
521 for (HeadwayConflict conflict : conflicts)
522 {
523 if (conflict.isMerge() && conflict.getDistance().si < 10.0)
524 {
525 PerceptionCollectable<HeadwayGtu, LaneBasedGtu> down = conflict.getDownstreamConflictingGTUs();
526 if (!down.isEmpty() && down.first().isParallel())
527 {
528 return false;
529 }
530 PerceptionCollectable<HeadwayGtu, LaneBasedGtu> up = conflict.getUpstreamConflictingGTUs();
531 if (!up.isEmpty() && up.first().isParallel())
532 {
533 return false;
534 }
535 }
536 }
537 }
538 catch (GtuException exception)
539 {
540 throw new OperationalPlanException(exception);
541 }
542 conflicts = intersection.getConflicts(RelativeLane.CURRENT);
543 for (HeadwayConflict conflict : conflicts)
544 {
545 if (conflict.getLane().getLink().equals(conflict.getConflictingLink()))
546 {
547 if (conflict.isMerge() && conflict.getDistance().le0()
548 && conflict.getDistance().neg().gt(conflict.getLength()))
549 {
550 return false;
551 }
552 else if (conflict.isSplit() && conflict.getDistance().le0()
553 && conflict.getDistance().neg().lt(gtu.getLength()))
554 {
555 return false;
556 }
557 }
558 }
559
560
561 Iterable<HeadwayTrafficLight> trafficLights = intersection.getTrafficLights(lane);
562 for (HeadwayTrafficLight trafficLight : trafficLights)
563 {
564 if (trafficLight.getTrafficLightColor().isRedOrYellow())
565 {
566 boolean ok = laneChange.checkRoom(gtu, trafficLight);
567 if (!ok)
568 {
569 return false;
570 }
571 }
572 }
573 }
574
575
576 return gapAcceptance.acceptGap(perception, params, sli, cfm, desire, ownSpeed, ownAcceleration, lat);
577
578 }
579
580
581
582
583
584
585
586
587
588
589
590
591 private static Acceleration quickIntersectionScan(final Parameters params, final SpeedLimitInfo sli,
592 final CarFollowingModel cfm, final Speed ownSpeed, final LateralDirectionality lat,
593 final IntersectionPerception intersection) throws ParameterException
594 {
595 Acceleration a = Acceleration.POSITIVE_INFINITY;
596 if (intersection != null)
597 {
598 RelativeLane lane = lat.isRight() ? RelativeLane.RIGHT : RelativeLane.LEFT;
599 Iterable<HeadwayConflict> iterable = intersection.getConflicts(lane);
600 if (iterable != null)
601 {
602 Iterator<HeadwayConflict> conflicts = iterable.iterator();
603 if (conflicts.hasNext())
604 {
605 a = Acceleration.min(a, CarFollowingUtil.followSingleLeader(cfm, params, ownSpeed, sli,
606 conflicts.next().getDistance(), Speed.ZERO));
607 }
608 Iterator<HeadwayTrafficLight> trafficLights = intersection.getTrafficLights(lane).iterator();
609 if (trafficLights.hasNext())
610 {
611 HeadwayTrafficLight trafficLight = trafficLights.next();
612 if (trafficLight.getTrafficLightColor().isRedOrYellow())
613 {
614 a = Acceleration.min(a, CarFollowingUtil.followSingleLeader(cfm, params, ownSpeed, sli,
615 trafficLight.getDistance(), Speed.ZERO));
616 }
617 }
618 }
619 }
620 return a;
621 }
622
623
624
625
626
627
628
629 static void setDesiredHeadway(final Parameters params, final double desire) throws ParameterException
630 {
631 double limitedDesire = desire < 0 ? 0 : desire > 1 ? 1 : desire;
632 double tDes = limitedDesire * params.getParameter(TMIN).si + (1 - limitedDesire) * params.getParameter(TMAX).si;
633 double t = params.getParameter(T).si;
634 params.setParameterResettable(T, Duration.instantiateSI(tDes < t ? tDes : t));
635 }
636
637
638
639
640
641
642 static void resetDesiredHeadway(final Parameters params) throws ParameterException
643 {
644 params.resetParameter(T);
645 }
646
647
648
649
650
651
652
653
654
655
656
657
658
659 public static Acceleration singleAcceleration(final Length distance, final Speed followerSpeed, final Speed leaderSpeed,
660 final double desire, final Parameters params, final SpeedLimitInfo sli, final CarFollowingModel cfm)
661 throws ParameterException
662 {
663
664 setDesiredHeadway(params, desire);
665
666 Acceleration a = CarFollowingUtil.followSingleLeader(cfm, params, followerSpeed, sli, distance, leaderSpeed);
667
668 resetDesiredHeadway(params);
669 return a;
670 }
671
672 }