View Javadoc
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   * Implementation of delayed neighbors perception which anticipates using constant speed.
42   * <p>
43   * Copyright (c) 2013-2019 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved. <br>
44   * BSD-style license. See <a href="http://opentrafficsim.org/node/13">OpenTrafficSim License</a>.
45   * <p>
46   * @version $Revision$, $LastChangedDate$, by $Author$, initial version 17 feb. 2017 <br>
47   * @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
48   * @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
49   * @author <a href="http://www.transport.citg.tudelft.nl">Wouter Schakel</a>
50   */
51  @Deprecated
52  public class DelayedNeighborsPerception extends AbstractDelayedNeighborsPerception implements EventListenerInterface
53  {
54  
55      /** Parameter for anticipating beyond current time. */
56      public static final ParameterTypeDuration TA =
57              new ParameterTypeDuration("ta", "anticipation time in future", Duration.ZERO, ConstraintInterface.POSITIVEZERO);
58  
59      /** Parameter for correlation in errors. */
60      public static final ParameterTypeDuration TAUE =
61              new ParameterTypeDuration("tau_e", "error correlation time", Duration.createSI(20), ConstraintInterface.POSITIVE);
62  
63      /** Parameter for distance error factor. */
64      public static final ParameterTypeDouble SERROR =
65              new ParameterTypeDouble("s_error", "distance error factor", 0.1, ConstraintInterface.POSITIVEZERO);
66  
67      /** Parameter for speed error factor. */
68      public static final ParameterTypeDouble VERROR =
69              new ParameterTypeDouble("v_error", "speed error factor", 0.1, ConstraintInterface.POSITIVEZERO);
70  
71      /** Parameter for acceleration error factor. */
72      public static final ParameterTypeDouble AERROR =
73              new ParameterTypeDouble("a_error", "acceleration error factor", 0.2, ConstraintInterface.POSITIVEZERO);
74  
75      /** Margin to check time step in Wiener process. */
76      private static final double MARGIN = 1e-6;
77  
78      /** */
79      private static final long serialVersionUID = 20170217L;
80  
81      /** Form of anticipation. */
82      private final Anticipation anticipation;
83  
84      /** Latest update time of neighbor rearrangement. */
85      private Time rearrangeTime;
86  
87      /** Set of followers per relative lane. */
88      private final Map<RelativeLane, PerceptionCollectable<HeadwayGTU, LaneBasedGTU>> followers = new HashMap<>();
89  
90      /** Set of leaders per relative lane. */
91      private final Map<RelativeLane, PerceptionCollectable<HeadwayGTU, LaneBasedGTU>> leaders = new HashMap<>();
92  
93      /** Set of first followers per lane upstream of merge per lateral direction, i.e. in the left or right lane. */
94      private final Map<LateralDirectionality, SortedSet<HeadwayGTU>> firstFollowers = new HashMap<>();
95  
96      /** Set of first leaders per lane downstream of split per lateral direction, i.e. in the left or right lane. */
97      private final Map<LateralDirectionality, SortedSet<HeadwayGTU>> firstLeaders = new HashMap<>();
98  
99      /** Whether a GTU is alongside per lateral direction, i.e. in the left or right lane. */
100     private final Map<LateralDirectionality, Boolean> gtuAlongside = new HashMap<>();
101 
102     /** Map of errors by a Wiener process for each GTU. */
103     private HashMap<String, ErrorValue> errors = new HashMap<>();
104 
105     /** Random numbers for perception errors. */
106     private final DistNormal norm;
107 
108     /**
109      * @param perception LanePerception; perception
110      * @param anticipation Anticipation; anticipation
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     /** {@inheritDoc} */
129     @Override
130     public void notify(final EventInterface event) throws RemoteException
131     {
132         changeLane((LateralDirectionality) ((Object[]) event.getContent())[1]);
133     }
134 
135     /**
136      * Rearrange neighbors, i.e. a follower may be anticipated to be a leader, etc.
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                 // adjacent lanes
205                 if (lane.getNumLanes() == 1)
206                 {
207                     // alongside, initial (can be overwritten as true by anticipation of first leaders/followers)
208                     boolean gtuAlongSide = getInfo(NeighborsInfoType.getBooleanType(GTUALONGSIDE), lane).getObject();
209 
210                     // followers
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                     // leaders
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                     // store alongside
253                     this.gtuAlongside.put(lane.getLateralDirectionality(), gtuAlongSide);
254                 }
255 
256                 // initiate sets
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                 // followers
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                 // leaders
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             // lane change performed, info on a lane not present
317         }
318 
319         try
320         {
321             // add empty sets on all lanes in the current cross section that are not considered yet
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      * Returns a standard Gaussian distributed random value generated with a Wiener process.
360      * @param gtuId String; gtu id of neighbor
361      * @param tau Duration; error correlation parameter
362      * @param dt Duration; model time step
363      * @return standard Gaussian distributed random value generated with a Wiener process
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                 // too long ago, exp may result in extreme values, draw new independent value
400                 err = this.norm.draw();
401             }
402         }
403         errorValue.set(now, err);
404         return err;
405 
406     }
407 
408     /**
409      * Creates the initial erroneous values for distance, speed and acceleration.
410      * @param distance Length; actual distance
411      * @param speed Speed; actual speed
412      * @param acceleration Acceleration; actual acceleration
413      * @param error double; random error
414      * @param distanceError double; error factor on distance
415      * @param speedError double; error factor on speed
416      * @param accelerationError double; error factor on acceleration
417      * @param egoSpeed Speed; own speed
418      * @return erroneous triplet
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     /** {@inheritDoc} */
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     /** {@inheritDoc} */
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     /** {@inheritDoc} */
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         // If the lane was not perceived at the reaction time in the past, but there is a lane now, be on the safe side.
468         // Note that infrastructure perception is separate, i.e. might be with a different or no reaction time.
469         return true;
470     }
471 
472     /** {@inheritDoc} */
473     @Override
474     public final PerceptionCollectable<HeadwayGTU, LaneBasedGTU> getLeaders(final RelativeLane lane)
475     {
476         rearrangeNeighbors();
477         return this.leaders.get(lane);
478     }
479 
480     /** {@inheritDoc} */
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      * <p>
490      * Copyright (c) 2013-2019 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved.
491      * <br>
492      * BSD-style license. See <a href="http://opentrafficsim.org/node/13">OpenTrafficSim License</a>.
493      * <p>
494      * @version $Revision$, $LastChangedDate$, by $Author$, initial version 4 mrt. 2017 <br>
495      * @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
496      * @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
497      * @author <a href="http://www.transport.citg.tudelft.nl">Wouter Schakel</a>
498      */
499     private class ErrorValue
500     {
501 
502         /** Time. */
503         private Time time;
504 
505         /** Error. */
506         private double error;
507 
508         /**
509          * 
510          */
511         ErrorValue()
512         {
513         }
514 
515         /**
516          * @return time.
517          */
518         public Time getTime()
519         {
520             return this.time;
521         }
522 
523         /**
524          * @return error.
525          */
526         public double getError()
527         {
528             return this.error;
529         }
530 
531         /**
532          * @param t Time; time
533          * @param err double; error
534          */
535         public void set(final Time t, final double err)
536         {
537             this.time = t;
538             this.error = err;
539         }
540 
541         /** {@inheritDoc} */
542         @Override
543         public final String toString()
544         {
545             return "ErrorValue [time=" + this.time + ", error=" + this.error + "]";
546         }
547 
548     }
549 
550     /** {@inheritDoc} */
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 }