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