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