1 package org.opentrafficsim.road.gtu.lane.perception.categories;
2
3 import java.util.HashMap;
4 import java.util.Map;
5 import java.util.SortedSet;
6 import java.util.TreeSet;
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.opentrafficsim.base.TimeStampedObject;
14 import org.opentrafficsim.base.parameters.ParameterException;
15 import org.opentrafficsim.base.parameters.ParameterTypeDouble;
16 import org.opentrafficsim.base.parameters.ParameterTypeDuration;
17 import org.opentrafficsim.base.parameters.Parameters;
18 import org.opentrafficsim.base.parameters.constraint.ConstraintInterface;
19 import org.opentrafficsim.core.gtu.GTUException;
20 import org.opentrafficsim.core.gtu.perception.EgoPerception;
21 import org.opentrafficsim.core.gtu.perception.PerceptionException;
22 import org.opentrafficsim.core.gtu.plan.operational.OperationalPlanException;
23 import org.opentrafficsim.core.network.LateralDirectionality;
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.SortedSetPerceptionIterable;
29 import org.opentrafficsim.road.gtu.lane.perception.categories.Anticipation.NeighborTriplet;
30 import org.opentrafficsim.road.gtu.lane.perception.headway.HeadwayGTU;
31
32 import nl.tudelft.simulation.jstats.distributions.DistNormal;
33 import nl.tudelft.simulation.language.Throw;
34
35
36
37
38
39
40
41
42
43
44
45
46 public class DelayedNeighborsPerception extends AbstractDelayedNeighborsPerception
47 {
48
49
50 public static final ParameterTypeDuration TA =
51 new ParameterTypeDuration("ta", "anticipation time in future", Duration.ZERO, ConstraintInterface.POSITIVEZERO);
52
53
54 public static final ParameterTypeDuration TAUE =
55 new ParameterTypeDuration("tau_e", "error correlation time", Duration.createSI(20), ConstraintInterface.POSITIVE);
56
57
58 public static final ParameterTypeDouble SERROR =
59 new ParameterTypeDouble("s_error", "distance error factor", 0.1, ConstraintInterface.POSITIVEZERO);
60
61
62 public static final ParameterTypeDouble VERROR =
63 new ParameterTypeDouble("v_error", "speed error factor", 0.1, ConstraintInterface.POSITIVEZERO);
64
65
66 public static final ParameterTypeDouble AERROR =
67 new ParameterTypeDouble("a_error", "acceleration error factor", 0.2, ConstraintInterface.POSITIVEZERO);
68
69
70 private static final double MARGIN = 1e-6;
71
72
73 private static final long serialVersionUID = 20170217L;
74
75
76 private final Anticipation anticipation;
77
78
79 private Time rearrangeTime;
80
81
82 private final Map<RelativeLane, PerceptionCollectable<HeadwayGTU, LaneBasedGTU>> followers = new HashMap<>();
83
84
85 private final Map<RelativeLane, PerceptionCollectable<HeadwayGTU, LaneBasedGTU>> leaders = new HashMap<>();
86
87
88 private final Map<LateralDirectionality, SortedSet<HeadwayGTU>> firstFollowers = new HashMap<>();
89
90
91 private final Map<LateralDirectionality, SortedSet<HeadwayGTU>> firstLeaders = new HashMap<>();
92
93
94 private final Map<LateralDirectionality, Boolean> gtuAlongside = new HashMap<>();
95
96
97 private HashMap<String, ErrorValue> errors = new HashMap<>();
98
99
100 private final DistNormal norm;
101
102
103
104
105
106 public DelayedNeighborsPerception(final LanePerception perception, final Anticipation anticipation)
107 {
108 super(perception);
109 Throw.whenNull(anticipation, "Anticipation may not be null.");
110 this.anticipation = anticipation;
111 try
112 {
113 this.norm = new DistNormal(perception.getGtu().getSimulator().getReplication().getStream("perception"));
114 }
115 catch (GTUException exception)
116 {
117 throw new RuntimeException("GTU not initialized.", exception);
118 }
119 }
120
121
122
123
124 private void rearrangeNeighbors()
125 {
126 Time time;
127 Duration ta;
128 Duration taue;
129 Length length;
130 Length traveledDistance;
131 double distanceError;
132 double speedError;
133 double accelerationError;
134 Speed egoSpeed;
135 Duration dt;
136 try
137 {
138 time = getPerception().getGtu().getSimulator().getSimulatorTime();
139 if (time.equals(this.rearrangeTime))
140 {
141 return;
142 }
143 Parameters params = getPerception().getGtu().getParameters();
144 ta = params.getParameter(TA);
145 taue = params.getParameter(TAUE);
146 distanceError = params.getParameter(SERROR);
147 speedError = params.getParameter(VERROR);
148 accelerationError = params.getParameter(AERROR);
149 length = getPerception().getGtu().getLength();
150 EgoPerception ego = getPerception().getPerceptionCategory(EgoPerception.class);
151 egoSpeed = ego.getSpeed();
152 dt = params.getParameter(DT);
153 try
154 {
155 traveledDistance = getPerception().getGtu().getOdometer().minus(getInfo(ODOMETER).getObject());
156 }
157 catch (PerceptionException exception)
158 {
159 throw new RuntimeException("Odometer not percieved.", exception);
160 }
161 if (!ta.eq0())
162 {
163 Acceleration acceleration = ego.getAcceleration();
164 traveledDistance = traveledDistance.plus(this.anticipation.egoAnticipation(egoSpeed, acceleration, ta));
165 }
166 this.rearrangeTime = time;
167 }
168 catch (GTUException exception)
169 {
170 throw new RuntimeException("GTU not initialized while rearranging neighbors.", exception);
171 }
172 catch (ParameterException exception)
173 {
174 throw new RuntimeException("Could not obtain parameter.", exception);
175 }
176 catch (OperationalPlanException exception)
177 {
178 throw new RuntimeException("No ego perception.", exception);
179 }
180 this.firstFollowers.clear();
181 this.firstLeaders.clear();
182 this.gtuAlongside.clear();
183 this.followers.clear();
184 this.leaders.clear();
185 try
186 {
187 for (RelativeLane lane : getDelayedCrossSection())
188 {
189
190
191 if (lane.getNumLanes() == 1)
192 {
193
194 boolean gtuAlongSide = getInfo(NeighborsInfoType.getBooleanType(GTUALONGSIDE), lane).getObject();
195
196
197 SortedSet<HeadwayGTU> firstFollowersSet = new TreeSet<>();
198 this.firstFollowers.put(lane.getLateralDirectionality(), firstFollowersSet);
199 TimeStampedObject<SortedSet<HeadwayGTU>> delayedFirstFollowers =
200 getInfo(NeighborsInfoType.getSortedSetType(FIRSTFOLLOWERS), lane);
201 Duration d = time.minus(delayedFirstFollowers.getTimestamp()).plus(ta);
202 for (HeadwayGTU gtu : delayedFirstFollowers.getObject())
203 {
204 NeighborTriplet info = this.anticipation.anticipate(erroneousTriplet(gtu.getDistance().neg(),
205 gtu.getSpeed(), gtu.getAcceleration(), getError(gtu.getId(), taue, dt), distanceError,
206 speedError, accelerationError, egoSpeed), d, traveledDistance);
207 if (info.getHeadway().le0())
208 {
209 firstFollowersSet.add(gtu.moved(info.getHeadway().neg(), info.getSpeed(), info.getAcceleration()));
210 }
211 else
212 {
213 gtuAlongSide = true;
214 }
215 }
216
217
218 SortedSet<HeadwayGTU> firstLeaderssSet = new TreeSet<>();
219 this.firstLeaders.put(lane.getLateralDirectionality(), firstLeaderssSet);
220 TimeStampedObject<SortedSet<HeadwayGTU>> delayedFirstLeaders =
221 getInfo(NeighborsInfoType.getSortedSetType(FIRSTLEADERS), lane);
222 d = time.minus(delayedFirstLeaders.getTimestamp()).plus(ta);
223 for (HeadwayGTU gtu : delayedFirstLeaders.getObject())
224 {
225 NeighborTriplet info = this.anticipation.anticipate(erroneousTriplet(gtu.getDistance(), gtu.getSpeed(),
226 gtu.getAcceleration(), getError(gtu.getId(), taue, dt), distanceError, speedError,
227 accelerationError, egoSpeed), d, traveledDistance);
228 if (info.getHeadway().ge0())
229 {
230 firstLeaderssSet.add(gtu.moved(info.getHeadway(), info.getSpeed(), info.getAcceleration()));
231 }
232 else
233 {
234 gtuAlongSide = true;
235 }
236 }
237
238
239 this.gtuAlongside.put(lane.getLateralDirectionality(), gtuAlongSide);
240 }
241
242
243 SortedSetPerceptionIterable<HeadwayGTU, LaneBasedGTU> followersSet = new SortedSetPerceptionIterable<>();
244 this.followers.put(lane, followersSet);
245 SortedSetPerceptionIterable<HeadwayGTU, LaneBasedGTU> leadersSet = new SortedSetPerceptionIterable<>();
246 this.leaders.put(lane, leadersSet);
247
248
249 TimeStampedObject<SortedSet<HeadwayGTU>> delayedFollowers =
250 getInfo(NeighborsInfoType.getSortedSetType(FOLLOWERS), lane);
251 Duration d = time.minus(delayedFollowers.getTimestamp()).plus(ta);
252 for (HeadwayGTU gtu : delayedFollowers.getObject())
253 {
254 NeighborTriplet info = this.anticipation.anticipate(
255 erroneousTriplet(gtu.getDistance().neg(), gtu.getSpeed(), gtu.getAcceleration(),
256 getError(gtu.getId(), taue, dt), distanceError, speedError, accelerationError, egoSpeed),
257 d, traveledDistance);
258 if (info.getHeadway().le(length) || lane.isCurrent())
259 {
260 followersSet.add(gtu.moved(info.getHeadway().neg(), info.getSpeed(), info.getAcceleration()));
261 }
262 else
263 {
264 leadersSet.add(gtu.moved(info.getHeadway().minus(length).minus(gtu.getLength()), info.getSpeed(),
265 info.getAcceleration()));
266 }
267 }
268
269
270 TimeStampedObject<SortedSet<HeadwayGTU>> delayedLeaders =
271 getInfo(NeighborsInfoType.getSortedSetType(LEADERS), lane);
272 d = time.minus(delayedLeaders.getTimestamp()).plus(ta);
273 for (HeadwayGTU gtu : delayedLeaders.getObject())
274 {
275
276 NeighborTriplet info = this.anticipation.anticipate(
277 erroneousTriplet(gtu.getDistance(), gtu.getSpeed(), gtu.getAcceleration(),
278 getError(gtu.getId(), taue, dt), distanceError, speedError, accelerationError, egoSpeed),
279 d, traveledDistance);
280 if (info.getHeadway().ge(gtu.getLength().neg()) || lane.isCurrent())
281 {
282 leadersSet.add(gtu.moved(info.getHeadway(), info.getSpeed(), info.getAcceleration()));
283 }
284 else
285 {
286 followersSet.add(gtu.moved(info.getHeadway().plus(length).plus(gtu.getLength()).neg(), info.getSpeed(),
287 info.getAcceleration()));
288 }
289 }
290
291 }
292
293 }
294 catch (@SuppressWarnings("unused") PerceptionException exception)
295 {
296
297 }
298
299 try
300 {
301
302 for (RelativeLane lane : getPerception().getLaneStructure().getExtendedCrossSection())
303 {
304 if (!this.followers.containsKey(lane))
305 {
306 this.followers.put(lane, new SortedSetPerceptionIterable<>());
307 }
308 if (!this.leaders.containsKey(lane))
309 {
310 this.leaders.put(lane, new SortedSetPerceptionIterable<>());
311 }
312 if (lane.isLeft() || lane.isRight())
313 {
314 if (!this.firstFollowers.containsKey(lane.getLateralDirectionality()))
315 {
316 this.firstFollowers.put(lane.getLateralDirectionality(), new TreeSet<>());
317 }
318 if (!this.firstLeaders.containsKey(lane.getLateralDirectionality()))
319 {
320 this.firstLeaders.put(lane.getLateralDirectionality(), new TreeSet<>());
321 }
322 if (!this.gtuAlongside.containsKey(lane.getLateralDirectionality()))
323 {
324 this.gtuAlongside.put(lane.getLateralDirectionality(), false);
325 }
326 }
327 }
328 }
329 catch (@SuppressWarnings("unused") ParameterException pe)
330 {
331
332 }
333
334 }
335
336
337
338
339
340
341
342
343 private double getError(final String gtuId, final Duration tau, final Duration dt)
344 {
345 Time now;
346 try
347 {
348 now = getTimestamp();
349 }
350 catch (GTUException exception)
351 {
352 throw new RuntimeException("Could not get time stamp.", exception);
353 }
354
355 double err;
356 ErrorValue errorValue;
357 if (!this.errors.containsKey(gtuId))
358 {
359 err = this.norm.draw();
360 errorValue = new ErrorValue();
361 this.errors.put(gtuId, errorValue);
362 }
363 else
364 {
365 errorValue = this.errors.get(gtuId);
366 if (errorValue.getTime().eq(now))
367 {
368 return errorValue.getError();
369 }
370 double dtErr = now.si - errorValue.getTime().si;
371 if (dtErr <= dt.si + MARGIN)
372 {
373 err = Math.exp(-dtErr / tau.si) * errorValue.getError() + Math.sqrt((2 * dtErr) / tau.si) * this.norm.draw();
374 }
375 else
376 {
377
378 err = this.norm.draw();
379 }
380 }
381 errorValue.set(now, err);
382 return err;
383
384 }
385
386
387
388
389
390
391
392
393
394
395
396
397
398 @SuppressWarnings("checkstyle:parameternumber")
399 private NeighborTriplet erroneousTriplet(final Length distance, final Speed speed, final Acceleration acceleration,
400 final double error, final double distanceError, final double speedError, final double accelerationError,
401 final Speed egoSpeed)
402 {
403 Length s = Length.createSI(distance.si * (1 + ((distance.ge0() ? error : -error) * distanceError)));
404 Speed v = Speed.createSI(speed.si + (error * speedError * distance.si));
405 if (v.lt0())
406 {
407 v = Speed.ZERO;
408 }
409 Acceleration a = Acceleration.createSI(acceleration.si * (1 + error * accelerationError));
410 return new NeighborTriplet(s, v, a);
411 }
412
413
414 @Override
415 public final SortedSet<HeadwayGTU> getFirstLeaders(final LateralDirectionality lat)
416 throws ParameterException, NullPointerException, IllegalArgumentException
417 {
418 rearrangeNeighbors();
419 return this.firstLeaders.get(lat);
420 }
421
422
423 @Override
424 public final SortedSet<HeadwayGTU> getFirstFollowers(final LateralDirectionality lat)
425 throws ParameterException, NullPointerException, IllegalArgumentException
426 {
427 rearrangeNeighbors();
428 return this.firstFollowers.get(lat);
429 }
430
431
432 @Override
433 public final boolean isGtuAlongside(final LateralDirectionality lat)
434 throws ParameterException, NullPointerException, IllegalArgumentException
435 {
436 if (isGtuAlongsideOverride(lat))
437 {
438 return true;
439 }
440 rearrangeNeighbors();
441 if (this.gtuAlongside.containsKey(lat))
442 {
443 return this.gtuAlongside.get(lat);
444 }
445
446
447 return true;
448 }
449
450
451 @Override
452 public final PerceptionCollectable<HeadwayGTU, LaneBasedGTU> getLeaders(final RelativeLane lane)
453 {
454 rearrangeNeighbors();
455 return this.leaders.get(lane);
456 }
457
458
459 @Override
460 public final PerceptionCollectable<HeadwayGTU, LaneBasedGTU> getFollowers(final RelativeLane lane)
461 {
462 rearrangeNeighbors();
463 return this.followers.get(lane);
464 }
465
466
467
468
469
470
471
472
473
474
475
476
477 private class ErrorValue
478 {
479
480
481 private Time time;
482
483
484 private double error;
485
486
487
488
489 ErrorValue()
490 {
491 }
492
493
494
495
496 public Time getTime()
497 {
498 return this.time;
499 }
500
501
502
503
504 public double getError()
505 {
506 return this.error;
507 }
508
509
510
511
512
513 public void set(final Time t, final double err)
514 {
515 this.time = t;
516 this.error = err;
517 }
518
519
520 @Override
521 public final String toString()
522 {
523 return "ErrorValue [time=" + this.time + ", error=" + this.error + "]";
524 }
525
526 }
527
528
529 @Override
530 public final String toString()
531 {
532 return "DelayedNeighborsPerception [anticipation=" + this.anticipation + ", rearrangeTime=" + this.rearrangeTime
533 + ", followers=" + this.followers + ", leaders=" + this.leaders + ", firstFollowers=" + this.firstFollowers
534 + ", firstLeaders=" + this.firstLeaders + ", gtuAlongside=" + this.gtuAlongside + ", errors=" + this.errors
535 + ", norm=" + this.norm + "]";
536 }
537 }