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