1 package org.opentrafficsim.road.gtu.lane.tactical.util.lmrs;
2
3 import java.util.Iterator;
4 import java.util.LinkedHashSet;
5 import java.util.Map;
6 import java.util.SortedSet;
7
8 import org.djunits.value.vdouble.scalar.Acceleration;
9 import org.djunits.value.vdouble.scalar.Duration;
10 import org.djunits.value.vdouble.scalar.Length;
11 import org.djunits.value.vdouble.scalar.Speed;
12 import org.djunits.value.vdouble.scalar.Time;
13 import org.djutils.exceptions.Try;
14 import org.opentrafficsim.base.parameters.ParameterException;
15 import org.opentrafficsim.base.parameters.ParameterTypeAcceleration;
16 import org.opentrafficsim.base.parameters.ParameterTypeDuration;
17 import org.opentrafficsim.base.parameters.ParameterTypes;
18 import org.opentrafficsim.base.parameters.Parameters;
19 import org.opentrafficsim.core.gtu.GTUException;
20 import org.opentrafficsim.core.gtu.TurnIndicatorIntent;
21 import org.opentrafficsim.core.gtu.perception.EgoPerception;
22 import org.opentrafficsim.core.gtu.plan.operational.OperationalPlanException;
23 import org.opentrafficsim.core.network.LateralDirectionality;
24 import org.opentrafficsim.core.network.NetworkException;
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 LinkedHashSet<MandatoryIncentive> mandatoryIncentives,
106 final LinkedHashSet<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 RelativeLane secondLane = laneChange.getSecondLane(gtu);
141 initiatedLaneChange = LateralDirectionality.NONE;
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 else
151 {
152
153
154 Desire desire = getLaneChangeDesire(params, perception, carFollowingModel, mandatoryIncentives, voluntaryIncentives,
155 lmrsData.getDesireMap());
156
157
158 double dFree = params.getParameter(DFREE);
159 initiatedLaneChange = LateralDirectionality.NONE;
160 turnIndicatorStatus = TurnIndicatorIntent.NONE;
161 if (desire.leftIsLargerOrEqual() && desire.getLeft() >= dFree)
162 {
163 if (acceptLaneChange(perception, params, sli, carFollowingModel, desire.getLeft(), speed, a,
164 LateralDirectionality.LEFT, lmrsData.getGapAcceptance()))
165 {
166
167 initiatedLaneChange = LateralDirectionality.LEFT;
168 turnIndicatorStatus = TurnIndicatorIntent.LEFT;
169 params.setParameter(DLC, desire.getLeft());
170 setDesiredHeadway(params, desire.getLeft());
171 leaders = neighbors.getLeaders(RelativeLane.LEFT);
172 if (!leaders.isEmpty())
173 {
174
175
176 lmrsData.isNewLeader(leaders.first());
177 }
178 a = Acceleration.min(a, carFollowingModel.followingAcceleration(params, speed, sli,
179 neighbors.getLeaders(RelativeLane.LEFT)));
180 }
181 }
182 else if (!desire.leftIsLargerOrEqual() && desire.getRight() >= dFree)
183 {
184 if (acceptLaneChange(perception, params, sli, carFollowingModel, desire.getRight(), speed, a,
185 LateralDirectionality.RIGHT, lmrsData.getGapAcceptance()))
186 {
187
188 initiatedLaneChange = LateralDirectionality.RIGHT;
189 turnIndicatorStatus = TurnIndicatorIntent.RIGHT;
190 params.setParameter(DLC, desire.getRight());
191 setDesiredHeadway(params, desire.getRight());
192 leaders = neighbors.getLeaders(RelativeLane.RIGHT);
193 if (!leaders.isEmpty())
194 {
195
196
197 lmrsData.isNewLeader(leaders.first());
198 }
199 a = Acceleration.min(a, carFollowingModel.followingAcceleration(params, speed, sli,
200 neighbors.getLeaders(RelativeLane.RIGHT)));
201 }
202 }
203 if (!initiatedLaneChange.isNone())
204 {
205 SortedSet<InfrastructureLaneChangeInfo> set = infra.getInfrastructureLaneChangeInfo(RelativeLane.CURRENT);
206 if (!set.isEmpty())
207 {
208 Length boundary = null;
209 for (InfrastructureLaneChangeInfo info : set)
210 {
211 int n = info.getRequiredNumberOfLaneChanges();
212 if (n > 1)
213 {
214 Length thisBoundary = info.getRemainingDistance()
215 .minus(Synchronization.requiredBufferSpace(speed, info.getRequiredNumberOfLaneChanges(),
216 params.getParameter(ParameterTypes.LOOKAHEAD),
217 params.getParameter(ParameterTypes.T0), params.getParameter(ParameterTypes.LCDUR),
218 params.getParameter(DCOOP)));
219 if (thisBoundary.le0())
220 {
221 thisBoundary = info.getRemainingDistance().divideBy(info.getRequiredNumberOfLaneChanges());
222 }
223 boundary = boundary == null || thisBoundary.si < boundary.si ? thisBoundary : boundary;
224 }
225 }
226 laneChange.setBoundary(boundary);
227 }
228 params.setParameter(DLEFT, 0.0);
229 params.setParameter(DRIGHT, 0.0);
230 }
231 else
232 {
233 params.setParameter(DLEFT, desire.getLeft());
234 params.setParameter(DRIGHT, desire.getRight());
235 }
236
237
238 Acceleration aSync;
239
240
241 double dSync = params.getParameter(DSYNC);
242 if (desire.leftIsLargerOrEqual() && desire.getLeft() >= dSync)
243 {
244 Synchronizable.State state;
245 if (desire.getLeft() >= params.getParameter(DCOOP))
246 {
247
248 turnIndicatorStatus = TurnIndicatorIntent.LEFT;
249 state = Synchronizable.State.INDICATING;
250 }
251 else
252 {
253 state = Synchronizable.State.SYNCHRONIZING;
254 }
255 aSync = lmrsData.getSynchronization().synchronize(perception, params, sli, carFollowingModel, desire.getLeft(),
256 LateralDirectionality.LEFT, lmrsData);
257 a = applyAcceleration(a, aSync, lmrsData, state);
258 }
259 else if (!desire.leftIsLargerOrEqual() && desire.getRight() >= dSync)
260 {
261 Synchronizable.State state;
262 if (desire.getRight() >= params.getParameter(DCOOP))
263 {
264
265 turnIndicatorStatus = TurnIndicatorIntent.RIGHT;
266 state = Synchronizable.State.INDICATING;
267 }
268 else
269 {
270 state = Synchronizable.State.SYNCHRONIZING;
271 }
272 aSync = lmrsData.getSynchronization().synchronize(perception, params, sli, carFollowingModel, desire.getRight(),
273 LateralDirectionality.RIGHT, lmrsData);
274 a = applyAcceleration(a, aSync, lmrsData, state);
275 }
276 else
277 {
278 lmrsData.setSynchronizationState(Synchronizable.State.NONE);
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 LinkedHashSet<MandatoryIncentive> mandatoryIncentives,
374 final LinkedHashSet<VoluntaryIncentive> voluntaryIncentives,
375 final Map<Class<? extends Incentive>, Desire> desireMap)
376 throws ParameterException, GTUException, OperationalPlanException
377 {
378
379 double dSync = parameters.getParameter(DSYNC);
380 double dCoop = parameters.getParameter(DCOOP);
381
382
383 double dLeftMandatory = 0.0;
384 double dRightMandatory = 0.0;
385 Desire mandatoryDesire = new Desire(dLeftMandatory, dRightMandatory);
386 for (MandatoryIncentive incentive : mandatoryIncentives)
387 {
388 Desire d = incentive.determineDesire(parameters, perception, carFollowingModel, mandatoryDesire);
389 desireMap.put(incentive.getClass(), d);
390 dLeftMandatory = Math.abs(d.getLeft()) > Math.abs(dLeftMandatory) ? d.getLeft() : dLeftMandatory;
391 dRightMandatory = Math.abs(d.getRight()) > Math.abs(dRightMandatory) ? d.getRight() : dRightMandatory;
392 mandatoryDesire = new Desire(dLeftMandatory, dRightMandatory);
393 }
394
395
396 double dLeftVoluntary = 0;
397 double dRightVoluntary = 0;
398 Desire voluntaryDesire = new Desire(dLeftVoluntary, dRightVoluntary);
399 for (VoluntaryIncentive incentive : voluntaryIncentives)
400 {
401 Desire d = incentive.determineDesire(parameters, perception, carFollowingModel, mandatoryDesire, voluntaryDesire);
402 desireMap.put(incentive.getClass(), d);
403 dLeftVoluntary += d.getLeft();
404 dRightVoluntary += d.getRight();
405 voluntaryDesire = new Desire(dLeftVoluntary, dRightVoluntary);
406 }
407
408
409 double thetaLeft = 0;
410 double dLeftMandatoryAbs = Math.abs(dLeftMandatory);
411 double dRightMandatoryAbs = Math.abs(dRightMandatory);
412 if (dLeftMandatoryAbs <= dSync || dLeftMandatory * dLeftVoluntary >= 0)
413 {
414
415 thetaLeft = 1;
416 }
417 else if (dSync < dLeftMandatoryAbs && dLeftMandatoryAbs < dCoop && dLeftMandatory * dLeftVoluntary < 0)
418 {
419
420 thetaLeft = (dCoop - dLeftMandatoryAbs) / (dCoop - dSync);
421 }
422 double thetaRight = 0;
423 if (dRightMandatoryAbs <= dSync || dRightMandatory * dRightVoluntary >= 0)
424 {
425
426 thetaRight = 1;
427 }
428 else if (dSync < dRightMandatoryAbs && dRightMandatoryAbs < dCoop && dRightMandatory * dRightVoluntary < 0)
429 {
430
431 thetaRight = (dCoop - dRightMandatoryAbs) / (dCoop - dSync);
432 }
433 return new Desire(dLeftMandatory + thetaLeft * dLeftVoluntary, dRightMandatory + thetaRight * dRightVoluntary);
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)
455 throws ParameterException, OperationalPlanException
456 {
457
458 boolean beyond = Try.assign(() -> perception.getGtu().laneChangeAllowed(), "Cannot obtain GTU.");
459 if (!beyond)
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 IntersectionPerception intersection = perception.getPerceptionCategoryOrNull(IntersectionPerception.class);
473
474
475
476
477
478
479
480
481
482
483
484
485
486 NeighborsPerception neighbors = perception.getPerceptionCategoryOrNull(NeighborsPerception.class);
487 EgoPerception<?, ?> ego = perception.getPerceptionCategoryOrNull(EgoPerception.class);
488 RelativeLane lane = new RelativeLane(lat, 1);
489 PerceptionCollectable<HeadwayConflict, Conflict> conflicts = intersection.getConflicts(lane);
490 PerceptionCollectable<HeadwayGTU, LaneBasedGTU> leaders = neighbors.getLeaders(lane);
491 try
492 {
493 Acceleration a = ConflictUtil.approachConflicts(params, conflicts, leaders, cfm, ego.getLength(), ego.getWidth(),
494 ownSpeed, ownAcceleration, sli, new ConflictPlans(), perception.getGtu(), lane);
495 if (a.lt(params.getParameter(ParameterTypes.B).neg()))
496 {
497 return false;
498 }
499 }
500 catch (GTUException exception)
501 {
502 throw new OperationalPlanException(exception);
503 }
504
505
506 return gapAcceptance.acceptGap(perception, params, sli, cfm, desire, ownSpeed, ownAcceleration, lat);
507 }
508
509
510
511
512
513
514
515
516
517
518
519
520 private static Acceleration quickIntersectionScan(final Parameters params, final SpeedLimitInfo sli,
521 final CarFollowingModel cfm, final Speed ownSpeed, final LateralDirectionality lat,
522 final IntersectionPerception intersection) throws ParameterException
523 {
524 Acceleration a = Acceleration.POSITIVE_INFINITY;
525 if (intersection != null)
526 {
527 RelativeLane lane = lat.isRight() ? RelativeLane.RIGHT : RelativeLane.LEFT;
528 Iterable<HeadwayConflict> iterable = intersection.getConflicts(lane);
529 if (iterable != null)
530 {
531 Iterator<HeadwayConflict> conflicts = iterable.iterator();
532 if (conflicts.hasNext())
533 {
534 a = Acceleration.min(a, CarFollowingUtil.followSingleLeader(cfm, params, ownSpeed, sli,
535 conflicts.next().getDistance(), Speed.ZERO));
536 }
537 Iterator<HeadwayTrafficLight> trafficLights = intersection.getTrafficLights(lane).iterator();
538 if (trafficLights.hasNext())
539 {
540 HeadwayTrafficLight trafficLight = trafficLights.next();
541 if (trafficLight.getTrafficLightColor().isRedOrYellow())
542 {
543 a = Acceleration.min(a, CarFollowingUtil.followSingleLeader(cfm, params, ownSpeed, sli,
544 trafficLight.getDistance(), Speed.ZERO));
545 }
546 }
547 }
548 }
549 return a;
550 }
551
552
553
554
555
556
557
558 static void setDesiredHeadway(final Parameters params, final double desire) throws ParameterException
559 {
560 double limitedDesire = desire < 0 ? 0 : desire > 1 ? 1 : desire;
561 double tDes = limitedDesire * params.getParameter(TMIN).si + (1 - limitedDesire) * params.getParameter(TMAX).si;
562 double t = params.getParameter(T).si;
563 params.setParameterResettable(T, Duration.createSI(tDes < t ? tDes : t));
564 }
565
566
567
568
569
570
571 static void resetDesiredHeadway(final Parameters params) throws ParameterException
572 {
573 params.resetParameter(T);
574 }
575
576
577
578
579
580
581
582
583
584
585
586
587
588 public static Acceleration singleAcceleration(final Length distance, final Speed followerSpeed, final Speed leaderSpeed,
589 final double desire, final Parameters params, final SpeedLimitInfo sli, final CarFollowingModel cfm)
590 throws ParameterException
591 {
592
593 setDesiredHeadway(params, desire);
594
595 Acceleration a = CarFollowingUtil.followSingleLeader(cfm, params, followerSpeed, sli, distance, leaderSpeed);
596
597 resetDesiredHeadway(params);
598 return a;
599 }
600
601 }