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 import java.util.Map;
7 import java.util.SortedSet;
8
9 import org.djunits.value.vdouble.scalar.Length;
10 import org.djunits.value.vdouble.scalar.Speed;
11 import org.opentrafficsim.base.TimeStampedObject;
12 import org.opentrafficsim.base.parameters.ParameterException;
13 import org.opentrafficsim.base.parameters.ParameterTypeLength;
14 import org.opentrafficsim.base.parameters.ParameterTypes;
15 import org.opentrafficsim.core.geometry.OTSGeometryException;
16 import org.opentrafficsim.core.geometry.OTSLine3D;
17 import org.opentrafficsim.core.gtu.GTUException;
18 import org.opentrafficsim.core.gtu.RelativePosition;
19 import org.opentrafficsim.core.gtu.Try;
20 import org.opentrafficsim.core.network.LongitudinalDirectionality;
21 import org.opentrafficsim.core.network.NetworkException;
22 import org.opentrafficsim.core.network.route.Route;
23 import org.opentrafficsim.road.gtu.lane.LaneBasedGTU;
24 import org.opentrafficsim.road.gtu.lane.perception.LaneBasedObjectIterable;
25 import org.opentrafficsim.road.gtu.lane.perception.LanePerception;
26 import org.opentrafficsim.road.gtu.lane.perception.LaneStructure.Entry;
27 import org.opentrafficsim.road.gtu.lane.perception.LaneStructureRecord;
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.headway.HeadwayConflict;
31 import org.opentrafficsim.road.gtu.lane.perception.headway.HeadwayGTU;
32 import org.opentrafficsim.road.gtu.lane.perception.headway.HeadwayStopLine;
33 import org.opentrafficsim.road.gtu.lane.perception.headway.HeadwayTrafficLight;
34 import org.opentrafficsim.road.network.lane.CrossSectionLink;
35 import org.opentrafficsim.road.network.lane.Lane;
36 import org.opentrafficsim.road.network.lane.conflict.Conflict;
37 import org.opentrafficsim.road.network.lane.conflict.Conflict.ConflictEnd;
38 import org.opentrafficsim.road.network.lane.conflict.ConflictPriority;
39 import org.opentrafficsim.road.network.lane.conflict.ConflictRule;
40 import org.opentrafficsim.road.network.lane.conflict.ConflictType;
41 import org.opentrafficsim.road.network.lane.object.LaneBasedObject;
42 import org.opentrafficsim.road.network.lane.object.trafficlight.TrafficLight;
43
44 import nl.tudelft.simulation.language.Throw;
45
46
47
48
49
50
51
52
53
54
55
56
57 public class DirectIntersectionPerception extends LaneBasedAbstractPerceptionCategory implements IntersectionPerception
58 {
59
60
61 private static final long serialVersionUID = 20160811L;
62
63
64 protected static final ParameterTypeLength LOOKAHEAD = ParameterTypes.LOOKAHEAD;
65
66
67 private static final Length MARGIN = Length.createSI(0.001);
68
69
70 private Map<RelativeLane, TimeStampedObject<PerceptionCollectable<HeadwayTrafficLight, TrafficLight>>> trafficLights =
71 new HashMap<>();
72
73
74 private Map<RelativeLane, TimeStampedObject<PerceptionCollectable<HeadwayConflict, Conflict>>> conflicts = new HashMap<>();
75
76
77 private TimeStampedObject<Boolean> alongsideConflictLeft;
78
79
80 private TimeStampedObject<Boolean> alongsideConflictRight;
81
82
83 private final HeadwayGtuType headwayGtuType;
84
85
86
87
88
89 public DirectIntersectionPerception(final LanePerception perception, final HeadwayGtuType headwayGtuType)
90 {
91 super(perception);
92 this.headwayGtuType = headwayGtuType;
93 }
94
95
96 @Override
97 public final void updateTrafficLights() throws GTUException, ParameterException
98 {
99 this.trafficLights.clear();
100 Route route = getPerception().getGtu().getStrategicalPlanner().getRoute();
101 for (RelativeLane lane : getPerception().getLaneStructure().getExtendedCrossSection())
102 {
103 LaneStructureRecord record = getPerception().getLaneStructure().getFirstRecord(lane);
104 Length pos = record.getStartDistance().neg();
105 pos = record.getDirection().isPlus() ? pos.plus(getGtu().getFront().getDx())
106 : pos.minus(getGtu().getFront().getDx());
107 PerceptionCollectable<HeadwayTrafficLight, TrafficLight> it =
108 new LaneBasedObjectIterable<HeadwayTrafficLight, TrafficLight>(getGtu(), TrafficLight.class, record,
109 Length.max(Length.ZERO, pos), getGtu().getParameters().getParameter(LOOKAHEAD), getGtu().getFront(),
110 route)
111 {
112
113 @Override
114 public HeadwayTrafficLight perceive(final LaneBasedGTU perceivingGtu, final TrafficLight trafficLight,
115 final Length distance)
116 {
117 try
118 {
119 return new HeadwayTrafficLight(trafficLight, distance);
120 }
121 catch (GTUException exception)
122 {
123 throw new RuntimeException(exception);
124 }
125 }
126 };
127 this.trafficLights.put(lane, new TimeStampedObject<>(it, getTimestamp()));
128 }
129 }
130
131
132 @Override
133 public final void updateConflicts() throws GTUException, ParameterException
134 {
135 this.conflicts.clear();
136 Route route = getPerception().getGtu().getStrategicalPlanner().getRoute();
137 for (RelativeLane lane : getPerception().getLaneStructure().getExtendedCrossSection())
138 {
139 LaneStructureRecord record = getPerception().getLaneStructure().getFirstRecord(lane);
140 Length pos = record.getStartDistance().neg().plus(getGtu().getRear().getDx());
141 while (pos.lt0() && !record.getPrev().isEmpty())
142 {
143 pos = pos.plus(record.getLength());
144 record = record.getPrev().get(0);
145 }
146
147 List<LaneBasedObject> laneObjs;
148 if (record.isDownstreamBranch())
149 {
150 if (record.getDirection().isPlus())
151 {
152 laneObjs = record.getLane().getLaneBasedObjects(Length.max(Length.ZERO, pos), record.getLane().getLength());
153 }
154 else
155 {
156 laneObjs = record.getLane().getLaneBasedObjects(Length.ZERO, pos);
157 }
158 }
159 else
160 {
161 laneObjs = new ArrayList<>();
162 }
163
164 for (LaneBasedObject object : laneObjs)
165 {
166 if (object instanceof ConflictEnd)
167 {
168 Conflict c = ((ConflictEnd) object).getConflict();
169 Length cPos = record.getDirection().isPlus() ? c.getLongitudinalPosition().minus(MARGIN)
170 : c.getLongitudinalPosition().plus(MARGIN);
171 pos = record.getDirection().isPlus() ? Length.min(pos, cPos) : Length.max(pos, cPos);
172 }
173 }
174 PerceptionCollectable<HeadwayConflict, Conflict> it = new LaneBasedObjectIterable<HeadwayConflict, Conflict>(
175 getGtu(), Conflict.class, record, Length.max(MARGIN.neg(), pos),
176 getGtu().getParameters().getParameter(LOOKAHEAD), getGtu().getFront(), route)
177 {
178
179 @SuppressWarnings("synthetic-access")
180 @Override
181 public HeadwayConflict perceive(final LaneBasedGTU perceivingGtu, final Conflict conflict,
182 final Length distance)
183 {
184 Conflict otherConflict = conflict.getOtherConflict();
185 ConflictType conflictType = conflict.getConflictType();
186 ConflictPriority conflictPriority = conflict.conflictPriority();
187 Class<? extends ConflictRule> conflictRuleType = conflict.getConflictRule().getClass();
188 String id = conflict.getId();
189 Length length = conflict.getLength();
190 Length conflictingLength = otherConflict.getLength();
191 CrossSectionLink conflictingLink = otherConflict.getLane().getParentLink();
192
193
194 Length lookAhead =
195 Try.assign(() -> getGtu().getParameters().getParameter(LOOKAHEAD), "Parameter not present.");
196 Length conflictingVisibility = lookAhead;
197 Speed conflictingSpeedLimit;
198 try
199 {
200 conflictingSpeedLimit = otherConflict.getLane().getHighestSpeedLimit();
201 }
202 catch (NetworkException exception)
203 {
204 throw new RuntimeException("GTU type not available on conflicting lane.", exception);
205 }
206
207 LongitudinalDirectionality otherDir = otherConflict.getDirection();
208 Throw.when(otherDir.isBoth(), UnsupportedOperationException.class,
209 "Conflicts on lanes with direction BOTH are not supported.");
210
211
212
213 HeadwayConflict headwayConflict;
214 try
215 {
216 PerceptionCollectable<HeadwayGTU, LaneBasedGTU> upstreamConflictingGTUs = otherConflict.getUpstreamGtus(
217 getGtu(), DirectIntersectionPerception.this.headwayGtuType, conflictingVisibility);
218 PerceptionCollectable<HeadwayGTU, LaneBasedGTU> downstreamConflictingGTUs =
219 otherConflict.getDownstreamGtus(getGtu(), DirectIntersectionPerception.this.headwayGtuType,
220 conflictingVisibility);
221
222 HeadwayStopLine stopLine = new HeadwayStopLine("stopLineId", Length.ZERO);
223 HeadwayStopLine conflictingStopLine = new HeadwayStopLine("conflictingStopLineId", Length.ZERO);
224
225 Lane thisLane = conflict.getLane();
226 Lane otherLane = otherConflict.getLane();
227 Length pos1a = conflict.getLongitudinalPosition();
228 Length pos2a = otherConflict.getLongitudinalPosition();
229 Length pos1b = Length.min(pos1a.plus(conflict.getLength()), thisLane.getLength());
230 Length pos2b = Length.min(pos2a.plus(otherConflict.getLength()), otherLane.getLength());
231 OTSLine3D line1 = thisLane.getCenterLine();
232 OTSLine3D line2 = otherLane.getCenterLine();
233 double dStart = line1.getLocation(pos1a).distance(line2.getLocation(pos2a));
234 double dEnd = line1.getLocation(pos1b).distance(line2.getLocation(pos2b));
235 Length startWidth =
236 Length.createSI(dStart + .5 * thisLane.getWidth(pos1a).si + .5 * otherLane.getWidth(pos2a).si);
237 Length endWidth =
238 Length.createSI(dEnd + .5 * thisLane.getWidth(pos1b).si + .5 * otherLane.getWidth(pos2b).si);
239
240 headwayConflict = new HeadwayConflict(conflictType, conflictPriority, conflictRuleType, id, distance,
241 length, conflictingLength, upstreamConflictingGTUs, downstreamConflictingGTUs,
242 conflictingVisibility, conflictingSpeedLimit, conflictingLink,
243 HeadwayConflict.Width.linear(startWidth, endWidth), stopLine, conflictingStopLine);
244 }
245 catch (GTUException | OTSGeometryException exception)
246 {
247 throw new RuntimeException("Could not create headway objects.", exception);
248 }
249
250
251
252
253
254 return headwayConflict;
255 }
256 };
257 this.conflicts.put(lane, new TimeStampedObject<>(it, getTimestamp()));
258 }
259
260
261 for (RelativeLane lane : new RelativeLane[] { RelativeLane.LEFT, RelativeLane.RIGHT })
262 {
263 if (getPerception().getLaneStructure().getExtendedCrossSection().contains(lane))
264 {
265 SortedSet<Entry<Conflict>> conflictEntries = getPerception().getLaneStructure().getUpstreamObjects(lane,
266 Conflict.class, getGtu(), RelativePosition.FRONT);
267 boolean alongside = false;
268 if (!conflictEntries.isEmpty())
269 {
270 Entry<Conflict> entry = conflictEntries.first();
271 alongside = entry.getDistance().si < entry.getLaneBasedObject().getLength().si + getGtu().getLength().si;
272 }
273 if (lane.isLeft())
274 {
275 this.alongsideConflictLeft = new TimeStampedObject<>(alongside, getTimestamp());
276 }
277 else
278 {
279 this.alongsideConflictRight = new TimeStampedObject<>(alongside, getTimestamp());
280 }
281 }
282 }
283 }
284
285
286 @Override
287 public final Iterable<HeadwayTrafficLight> getTrafficLights(final RelativeLane lane)
288 {
289 return getObjectOrNull(this.trafficLights.get(lane));
290 }
291
292
293 @Override
294 public final PerceptionCollectable<HeadwayConflict, Conflict> getConflicts(final RelativeLane lane)
295 {
296 return getObjectOrNull(this.conflicts.get(lane));
297 }
298
299
300 @Override
301 public final boolean isAlongsideConflictLeft()
302 {
303 return this.alongsideConflictLeft == null ? false : this.alongsideConflictLeft.getObject();
304 }
305
306
307 @Override
308 public final boolean isAlongsideConflictRight()
309 {
310 return this.alongsideConflictRight == null ? false : this.alongsideConflictRight.getObject();
311 }
312
313
314
315
316
317
318 public final TimeStampedObject<PerceptionCollectable<HeadwayTrafficLight, TrafficLight>> getTimeStampedTrafficLights(
319 final RelativeLane lane)
320 {
321 return this.trafficLights.get(lane);
322 }
323
324
325
326
327
328
329 public final TimeStampedObject<PerceptionCollectable<HeadwayConflict, Conflict>> getTimeStampedConflicts(
330 final RelativeLane lane)
331 {
332 return this.conflicts.get(lane);
333 }
334
335
336
337
338
339 public final TimeStampedObject<Boolean> isAlongsideConflictLeftTimeStamped()
340 {
341 return this.alongsideConflictLeft;
342 }
343
344
345
346
347
348 public final TimeStampedObject<Boolean> isAlongsideConflictRightTimeStamped()
349 {
350 return this.alongsideConflictRight;
351 }
352
353
354 @Override
355 public final String toString()
356 {
357 return "DirectIntersectionPerception";
358 }
359
360 }