1 package org.opentrafficsim.road.gtu.lane.perception.categories;
2
3 import java.util.HashMap;
4 import java.util.HashSet;
5 import java.util.Map;
6 import java.util.Set;
7 import java.util.SortedSet;
8
9 import org.djunits.value.vdouble.scalar.Duration;
10 import org.djunits.value.vdouble.scalar.Length;
11 import org.djunits.value.vdouble.scalar.Time;
12 import org.opentrafficsim.base.TimeStampedObject;
13 import org.opentrafficsim.base.parameters.ParameterException;
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.perception.PerceptionException;
19 import org.opentrafficsim.core.network.LateralDirectionality;
20 import org.opentrafficsim.core.network.NetworkException;
21 import org.opentrafficsim.road.gtu.lane.LaneBasedGTU;
22 import org.opentrafficsim.road.gtu.lane.perception.LanePerception;
23 import org.opentrafficsim.road.gtu.lane.perception.PerceptionCollectable;
24 import org.opentrafficsim.road.gtu.lane.perception.RelativeLane;
25 import org.opentrafficsim.road.gtu.lane.perception.headway.HeadwayGTU;
26
27 import nl.tudelft.simulation.dsol.SimRuntimeException;
28
29
30
31
32
33
34
35
36
37
38
39 public abstract class AbstractDelayedNeighborsPerception extends AbstractDelayedPerceptionCategory
40 implements NeighborsPerception
41 {
42
43
44 private static final long serialVersionUID = 20170217L;
45
46
47 protected static final ParameterTypeDuration TR = ParameterTypes.TR;
48
49
50 protected static final ParameterTypeDuration DT = ParameterTypes.DT;
51
52
53 private Time initialTime = null;
54
55
56 private final DirectNeighborsPerception direct;
57
58
59 private Duration reactionTime = null;
60
61
62 private Duration plannerTimeStep = null;
63
64
65 private Duration remainder = null;
66
67
68 public static final String FIRSTLEADERS = "firstLeaders";
69
70
71 public static final String FIRSTFOLLOWERS = "firstFollower";
72
73
74 public static final String GTUALONGSIDE = "gtuAlongside";
75
76
77 public static final String LEADERS = "leaders";
78
79
80 public static final String FOLLOWERS = "followers";
81
82
83 public static final NeighborsInfoType<SortedSet<RelativeLane>> CROSSSECTION = new NeighborsInfoType<>("cross-section");
84
85
86 public static final NeighborsInfoType<Length> ODOMETER = new NeighborsInfoType<>("odometer");
87
88
89 private boolean gtuAlongsideLeftOverride = false;
90
91
92 private boolean gtuAlongsideRightOverride = false;
93
94
95
96
97
98 public AbstractDelayedNeighborsPerception(final LanePerception perception)
99 {
100 super(perception);
101 this.direct = new DirectNeighborsPerception(perception, HeadwayGtuType.COPY);
102 }
103
104
105 @Override
106 public final void updateAll() throws GTUException, NetworkException, ParameterException
107 {
108
109 if (this.remainder == null)
110 {
111 try
112 {
113
114 Parameters params = getPerception().getGtu().getParameters();
115 this.reactionTime = params.getParameter(TR);
116 this.plannerTimeStep = params.getParameter(DT);
117 double rem;
118 if (this.reactionTime.eq0())
119 {
120 rem = 0;
121 }
122 else if (this.reactionTime.gt(this.plannerTimeStep))
123 {
124 rem = this.reactionTime.si % this.plannerTimeStep.si;
125 }
126 else
127 {
128 rem = this.plannerTimeStep.si - this.reactionTime.si;
129 }
130 this.remainder = Duration.createSI(rem);
131 }
132 catch (ParameterException | GTUException exception)
133 {
134 throw new RuntimeException("Exception while setting up delayed neighors perception.", exception);
135 }
136 }
137
138
139 Time now = getPerception().getGtu().getSimulator().getSimulatorTime();
140 if (this.initialTime == null)
141 {
142 this.initialTime = now;
143 }
144 if (now.minus(this.initialTime).le(this.reactionTime))
145 {
146 updateAllDelayed();
147 return;
148 }
149
150 if (this.remainder.eq0())
151 {
152
153 updateAllDelayed();
154 }
155 else
156 {
157
158 Time scheduledTime = now.plus(this.remainder);
159 try
160 {
161 getPerception().getGtu().getSimulator().scheduleEventAbs(scheduledTime, this, this, "updateAllDelayed", null);
162 }
163 catch (SimRuntimeException exception)
164 {
165 throw new RuntimeException("Scheduling perception update in the past.", exception);
166 }
167 }
168
169
170
171
172
173
174
175
176 if (getPerception().getLaneStructure().getExtendedCrossSection().contains(RelativeLane.LEFT))
177 {
178 this.direct.updateFirstFollowers(LateralDirectionality.LEFT);
179 this.direct.updateFirstLeaders(LateralDirectionality.LEFT);
180 this.direct.updateGtuAlongside(LateralDirectionality.LEFT);
181 this.gtuAlongsideLeftOverride = newFirstLeaderOrFollower(getFollowers(RelativeLane.LEFT),
182 this.direct.getFirstFollowers(LateralDirectionality.LEFT))
183 || newFirstLeaderOrFollower(getLeaders(RelativeLane.LEFT),
184 this.direct.getFirstLeaders(LateralDirectionality.LEFT))
185 || this.direct.isGtuAlongside(LateralDirectionality.LEFT);
186 }
187 if (getPerception().getLaneStructure().getExtendedCrossSection().contains(RelativeLane.RIGHT))
188 {
189 this.direct.updateFirstFollowers(LateralDirectionality.RIGHT);
190 this.direct.updateFirstLeaders(LateralDirectionality.RIGHT);
191 this.direct.updateGtuAlongside(LateralDirectionality.RIGHT);
192 this.gtuAlongsideRightOverride = newFirstLeaderOrFollower(getFollowers(RelativeLane.RIGHT),
193 this.direct.getFirstFollowers(LateralDirectionality.RIGHT))
194 || newFirstLeaderOrFollower(getLeaders(RelativeLane.RIGHT),
195 this.direct.getFirstLeaders(LateralDirectionality.RIGHT))
196 || this.direct.isGtuAlongside(LateralDirectionality.RIGHT);
197 }
198
199 }
200
201
202
203
204
205
206
207 private boolean newFirstLeaderOrFollower(final Iterable<? extends HeadwayGTU> delayedSet,
208 final Set<? extends HeadwayGTU> currentSet)
209 {
210 Set<String> set = new HashSet<>();
211 for (HeadwayGTU gtu : delayedSet)
212 {
213 set.add(gtu.getId());
214 }
215 for (HeadwayGTU gtu : currentSet)
216 {
217 if (!set.contains(gtu.getId()) && gtu.getDistance().si < 50)
218 {
219 return true;
220 }
221 }
222 return false;
223 }
224
225
226
227
228
229
230 public final boolean isGtuAlongsideOverride(final LateralDirectionality lat)
231 {
232 return lat.isLeft() ? this.gtuAlongsideLeftOverride : this.gtuAlongsideRightOverride;
233 }
234
235
236
237
238
239
240
241
242 protected void updateAllDelayed() throws GTUException, NetworkException, ParameterException
243 {
244
245 try
246 {
247 getGtu().getReferencePosition();
248 }
249 catch (@SuppressWarnings("unused") GTUException exception)
250 {
251
252 return;
253 }
254
255 this.direct.updateAll();
256
257 if (getPerception().getLaneStructure().getExtendedCrossSection().contains(RelativeLane.LEFT))
258 {
259 updateFirstLeaders(LateralDirectionality.LEFT);
260 updateFirstFollowers(LateralDirectionality.LEFT);
261 updateGtuAlongside(LateralDirectionality.LEFT);
262 }
263 if (getPerception().getLaneStructure().getExtendedCrossSection().contains(RelativeLane.RIGHT))
264 {
265 updateFirstLeaders(LateralDirectionality.RIGHT);
266 updateFirstFollowers(LateralDirectionality.RIGHT);
267 updateGtuAlongside(LateralDirectionality.RIGHT);
268 }
269 for (RelativeLane lane : getPerception().getLaneStructure().getExtendedCrossSection())
270 {
271 updateLeaders(lane);
272 updateFollowers(lane);
273 }
274 setInfo(CROSSSECTION, new TimeStampedObject<>(getPerception().getLaneStructure().getExtendedCrossSection(), getTimestamp()));
275 setInfo(ODOMETER, new TimeStampedObject<>(getGtu().getOdometer(), getTimestamp()));
276 }
277
278
279 @Override
280 public final void updateFirstLeaders(final LateralDirectionality lat)
281 throws ParameterException, GTUException, NetworkException
282 {
283 setInfo(NeighborsInfoType.getSortedSetType(FIRSTLEADERS), new RelativeLane(lat, 1),
284 this.direct.getTimeStampedFirstLeaders(lat));
285 }
286
287
288 @Override
289 public final void updateFirstFollowers(final LateralDirectionality lat)
290 throws GTUException, ParameterException, NetworkException
291 {
292 setInfo(NeighborsInfoType.getSortedSetType(FIRSTFOLLOWERS), new RelativeLane(lat, 1),
293 this.direct.getTimeStampedFirstFollowers(lat));
294 }
295
296
297 @Override
298 public final void updateGtuAlongside(final LateralDirectionality lat) throws GTUException, ParameterException
299 {
300 setInfo(NeighborsInfoType.getBooleanType(GTUALONGSIDE), new RelativeLane(lat, 1),
301 this.direct.isGtuAlongsideTimeStamped(lat));
302 }
303
304
305 @Override
306 public final void updateLeaders(final RelativeLane lane) throws ParameterException, GTUException, NetworkException
307 {
308 setInfo(NeighborsInfoType.getIterableType(LEADERS), lane, this.direct.getTimeStampedLeaders(lane));
309 }
310
311
312 @Override
313 public final void updateFollowers(final RelativeLane lane) throws GTUException, NetworkException, ParameterException
314 {
315 setInfo(NeighborsInfoType.getIterableType(FOLLOWERS), lane, this.direct.getTimeStampedFollowers(lane));
316 }
317
318
319
320
321
322 public final SortedSet<RelativeLane> getDelayedCrossSection()
323 {
324 try
325 {
326 return getInfo(CROSSSECTION).getObject();
327 }
328 catch (PerceptionException exception)
329 {
330 throw new RuntimeException("Crosssection was not perceived.", exception);
331 }
332 }
333
334
335
336
337
338 public static final class NeighborsInfoType<T> extends DelayedInfoType<T>
339 {
340
341
342 private static final Map<String, NeighborsInfoType<?>> LANEINFOTYPES = new HashMap<>();
343
344
345
346
347
348 public NeighborsInfoType(final String id)
349 {
350 super(id, TR);
351 }
352
353
354
355
356
357
358 @SuppressWarnings("unchecked")
359 public static NeighborsInfoType<SortedSet<HeadwayGTU>> getSortedSetType(final String id)
360 {
361 if (!LANEINFOTYPES.containsKey(id))
362 {
363 LANEINFOTYPES.put(id, new NeighborsInfoType<SortedSet<HeadwayGTU>>(id));
364 }
365 return (NeighborsInfoType<SortedSet<HeadwayGTU>>) LANEINFOTYPES.get(id);
366 }
367
368
369
370
371
372
373 @SuppressWarnings("unchecked")
374 public static NeighborsInfoType<PerceptionCollectable<HeadwayGTU, LaneBasedGTU>> getIterableType(final String id)
375 {
376 if (!LANEINFOTYPES.containsKey(id))
377 {
378 LANEINFOTYPES.put(id, new NeighborsInfoType<SortedSet<HeadwayGTU>>(id));
379 }
380 return (NeighborsInfoType<PerceptionCollectable<HeadwayGTU, LaneBasedGTU>>) LANEINFOTYPES.get(id);
381 }
382
383
384
385
386
387
388 @SuppressWarnings("unchecked")
389 public static NeighborsInfoType<Boolean> getBooleanType(final String id)
390 {
391 if (!LANEINFOTYPES.containsKey(id))
392 {
393 LANEINFOTYPES.put(id, new NeighborsInfoType<SortedSet<HeadwayGTU>>(id));
394 }
395 return (NeighborsInfoType<Boolean>) LANEINFOTYPES.get(id);
396 }
397
398
399 @Override
400 public String toString()
401 {
402 return "NeighborsInfoType []";
403 }
404
405 }
406
407 }