1 package org.opentrafficsim.road.gtu.lane.perception.categories;
2
3 import java.util.ArrayList;
4 import java.util.HashMap;
5 import java.util.List;
6
7 import org.djunits.value.vdouble.scalar.Duration;
8 import org.djunits.value.vdouble.scalar.Time;
9 import org.opentrafficsim.base.Identifiable;
10 import org.opentrafficsim.base.TimeStampedObject;
11 import org.opentrafficsim.base.Type;
12 import org.opentrafficsim.base.parameters.ParameterException;
13 import org.opentrafficsim.base.parameters.ParameterTypeDuration;
14 import org.opentrafficsim.base.parameters.Parameters;
15 import org.opentrafficsim.core.gtu.GTUException;
16 import org.opentrafficsim.core.gtu.perception.PerceptionException;
17 import org.opentrafficsim.core.network.LateralDirectionality;
18 import org.opentrafficsim.road.gtu.lane.perception.LanePerception;
19 import org.opentrafficsim.road.gtu.lane.perception.RelativeLane;
20
21 import nl.tudelft.simulation.language.Throw;
22
23
24
25
26
27
28
29
30
31
32
33
34 public abstract class AbstractDelayedPerceptionCategory extends LaneBasedAbstractPerceptionCategory
35 implements LaneBasedPerceptionCategory
36 {
37
38
39 private static final Duration MARGIN = Duration.createSI(0.001);
40
41
42 private static final long serialVersionUID = 20170217L;
43
44
45
46
47 public AbstractDelayedPerceptionCategory(final LanePerception perception)
48 {
49 super(perception);
50 }
51
52
53 private final HashMap<DelayedInfoType<?>, HashMap<RelativeLane, List<TimeStampedObject<?>>>> map = new HashMap<>();
54
55
56
57
58
59
60
61 public final <T> void setInfo(final DelayedInfoType<T> delayedInfoType, final TimeStampedObject<T> info)
62 {
63 setInfo(delayedInfoType, null, info);
64 }
65
66
67
68
69
70
71
72
73 public final <T> void setInfo(final DelayedInfoType<T> delayedInfoType, final RelativeLane lane,
74 final TimeStampedObject<T> info)
75 {
76 Throw.whenNull(delayedInfoType, "Delayed info type may not be null.");
77 Throw.whenNull(info, "Info may not be null.");
78 if (!this.map.containsKey(delayedInfoType))
79 {
80 this.map.put(delayedInfoType, new HashMap<>());
81 }
82 if (!this.map.get(delayedInfoType).containsKey(lane))
83 {
84 this.map.get(delayedInfoType).put(lane, new ArrayList<TimeStampedObject<?>>());
85 }
86 List<TimeStampedObject<?>> list = this.map.get(delayedInfoType).get(lane);
87 if (!list.isEmpty())
88 {
89 Throw.when(!list.isEmpty() && info.getTimestamp().le(list.get(list.size() - 1).getTimestamp()),
90 RuntimeException.class,
91 "Setting delayed info for type %s with timestamp %s while info with timestamp %s is already present.",
92 delayedInfoType, info.getTimestamp(), list.get(list.size() - 1).getTimestamp());
93 }
94
95
96 list.add(info);
97 }
98
99
100
101
102
103
104
105
106
107 public final <T> TimeStampedObject<T> getInfo(final DelayedInfoType<T> delayedInfoType) throws PerceptionException
108 {
109 return getInfo(delayedInfoType, null);
110 }
111
112
113
114
115
116
117
118
119
120
121 @SuppressWarnings("unchecked")
122 public final <T> TimeStampedObject<T> getInfo(final DelayedInfoType<T> delayedInfoType, final RelativeLane lane)
123 throws PerceptionException
124 {
125 Throw.whenNull(delayedInfoType, "Delayed info type may not be null.");
126 Throw.when(!this.map.containsKey(delayedInfoType), PerceptionException.class,
127 "Perception does not contain any data for info type %s.", delayedInfoType);
128 Throw.when(!this.map.get(delayedInfoType).containsKey(lane), PerceptionException.class,
129 "Perception does not contain any data for info type %s for lane %s.", delayedInfoType, lane);
130 List<TimeStampedObject<?>> list = this.map.get(delayedInfoType).get(lane);
131 Throw.when(list.isEmpty(), RuntimeException.class, "Perception does not contain any data for info type %s.",
132 delayedInfoType);
133
134
135 Parameters params;
136 Time now;
137 try
138 {
139 params = getPerception().getGtu().getParameters();
140 now = getPerception().getGtu().getSimulator().getSimulatorTime();
141 }
142 catch (GTUException exception)
143 {
144 throw new RuntimeException("GTU not yet initialized.", exception);
145 }
146 Time delayedTime;
147 try
148 {
149 delayedTime = now.minus(params.getParameter(delayedInfoType.getDelayParameter())).plus(MARGIN);
150 }
151 catch (ParameterException exception)
152 {
153 throw new RuntimeException("Delay parameter not found.", exception);
154 }
155 while (list.size() > 1 && list.get(1).getTimestamp().le(delayedTime))
156 {
157 list.remove(0);
158 }
159
160 return (TimeStampedObject<T>) list.get(0);
161 }
162
163
164
165
166
167
168 public final void changeLane(final LateralDirectionality dir)
169 {
170 for (DelayedInfoType<?> delayedInfoType : this.map.keySet())
171 {
172 HashMap<RelativeLane, List<TimeStampedObject<?>>> newMap = new HashMap<>();
173 for (RelativeLane lane : this.map.get(delayedInfoType).keySet())
174 {
175 if (lane != null)
176 {
177 newMap.put(dir.isLeft() ? lane.getRight() : lane.getLeft(), this.map.get(delayedInfoType).get(lane));
178 }
179 else
180 {
181 newMap.put(lane, this.map.get(delayedInfoType).get(lane));
182 }
183 }
184 this.map.put(delayedInfoType, newMap);
185 }
186 }
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201 public static class DelayedInfoType<T> extends Type<DelayedInfoType<T>> implements Identifiable
202 {
203
204
205 private final String id;
206
207
208 private final ParameterTypeDuration delayParameter;
209
210
211
212
213
214
215 public DelayedInfoType(final String id, final ParameterTypeDuration delayParameter)
216 {
217 this.id = id;
218 this.delayParameter = delayParameter;
219 }
220
221
222
223
224
225 @Override
226 public final String getId()
227 {
228 return this.getId();
229 }
230
231
232
233
234
235 public final ParameterTypeDuration getDelayParameter()
236 {
237 return this.delayParameter;
238 }
239
240
241 @Override
242 public final int hashCode()
243 {
244 final int prime = 31;
245 int result = 1;
246 result = prime * result + ((this.id == null) ? 0 : this.id.hashCode());
247 return result;
248 }
249
250
251 @Override
252 public final boolean equals(final Object obj)
253 {
254 if (this == obj)
255 {
256 return true;
257 }
258 if (obj == null)
259 {
260 return false;
261 }
262 if (getClass() != obj.getClass())
263 {
264 return false;
265 }
266 DelayedInfoType<?> other = (DelayedInfoType<?>) obj;
267 if (this.id == null)
268 {
269 if (other.id != null)
270 {
271 return false;
272 }
273 }
274 else if (!this.id.equals(other.id))
275 {
276 return false;
277 }
278 return true;
279 }
280
281 }
282
283 }