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