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.djutils.exceptions.Throw;
12 import org.djutils.exceptions.Try;
13 import org.opentrafficsim.base.TimeStampedObject;
14 import org.opentrafficsim.base.parameters.ParameterException;
15 import org.opentrafficsim.base.parameters.ParameterTypeLength;
16 import org.opentrafficsim.base.parameters.ParameterTypes;
17 import org.opentrafficsim.core.geometry.OTSGeometryException;
18 import org.opentrafficsim.core.geometry.OTSLine3D;
19 import org.opentrafficsim.core.gtu.GTUException;
20 import org.opentrafficsim.core.gtu.RelativePosition;
21 import org.opentrafficsim.core.network.LongitudinalDirectionality;
22 import org.opentrafficsim.core.network.NetworkException;
23 import org.opentrafficsim.core.network.route.Route;
24 import org.opentrafficsim.road.gtu.lane.LaneBasedGTU;
25 import org.opentrafficsim.road.gtu.lane.perception.LaneBasedObjectIterable;
26 import org.opentrafficsim.road.gtu.lane.perception.LanePerception;
27 import org.opentrafficsim.road.gtu.lane.perception.LaneStructure.Entry;
28 import org.opentrafficsim.road.gtu.lane.perception.LaneStructureRecord;
29 import org.opentrafficsim.road.gtu.lane.perception.PerceptionCollectable;
30 import org.opentrafficsim.road.gtu.lane.perception.RelativeLane;
31 import org.opentrafficsim.road.gtu.lane.perception.categories.neighbors.HeadwayGtuType;
32 import org.opentrafficsim.road.gtu.lane.perception.headway.HeadwayConflict;
33 import org.opentrafficsim.road.gtu.lane.perception.headway.HeadwayGTU;
34 import org.opentrafficsim.road.gtu.lane.perception.headway.HeadwayStopLine;
35 import org.opentrafficsim.road.gtu.lane.perception.headway.HeadwayTrafficLight;
36 import org.opentrafficsim.road.network.lane.CrossSectionLink;
37 import org.opentrafficsim.road.network.lane.Lane;
38 import org.opentrafficsim.road.network.lane.conflict.Conflict;
39 import org.opentrafficsim.road.network.lane.conflict.Conflict.ConflictEnd;
40 import org.opentrafficsim.road.network.lane.conflict.ConflictPriority;
41 import org.opentrafficsim.road.network.lane.conflict.ConflictRule;
42 import org.opentrafficsim.road.network.lane.conflict.ConflictType;
43 import org.opentrafficsim.road.network.lane.object.LaneBasedObject;
44 import org.opentrafficsim.road.network.lane.object.trafficlight.TrafficLight;
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,
108 TrafficLight> it = new LaneBasedObjectIterable<HeadwayTrafficLight, TrafficLight>(getGtu(),
109 TrafficLight.class, record, Length.max(Length.ZERO, pos),
110 getGtu().getParameters().getParameter(LOOKAHEAD), getGtu().getFront(), 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,
175 Conflict> it = new LaneBasedObjectIterable<HeadwayConflict, Conflict>(getGtu(), Conflict.class, record,
176 Length.max(MARGIN.neg(), pos), getGtu().getParameters().getParameter(LOOKAHEAD),
177 getGtu().getFront(), route)
178 {
179
180 @SuppressWarnings("synthetic-access")
181 @Override
182 public HeadwayConflict perceive(final LaneBasedGTU perceivingGtu, final Conflict conflict,
183 final Length distance)
184 {
185 Conflict otherConflict = conflict.getOtherConflict();
186 ConflictType conflictType = conflict.getConflictType();
187 ConflictPriority conflictPriority = conflict.conflictPriority();
188 Class<? extends ConflictRule> conflictRuleType = conflict.getConflictRule().getClass();
189 String id = conflict.getId();
190 Length length = conflict.getLength();
191 Length conflictingLength = otherConflict.getLength();
192 CrossSectionLink conflictingLink = otherConflict.getLane().getParentLink();
193
194
195 Length lookAhead = Try.assign(() -> getGtu().getParameters().getParameter(LOOKAHEAD),
196 "Parameter not present.");
197 Length conflictingVisibility = lookAhead;
198 Speed conflictingSpeedLimit;
199 try
200 {
201 conflictingSpeedLimit = otherConflict.getLane().getHighestSpeedLimit();
202 }
203 catch (NetworkException exception)
204 {
205 throw new RuntimeException("GTU type not available on conflicting lane.", exception);
206 }
207
208 LongitudinalDirectionality otherDir = otherConflict.getDirection();
209 Throw.when(otherDir.isBoth(), UnsupportedOperationException.class,
210 "Conflicts on lanes with direction BOTH are not supported.");
211
212
213
214 HeadwayConflict headwayConflict;
215 try
216 {
217 PerceptionCollectable<HeadwayGTU, LaneBasedGTU> upstreamConflictingGTUs =
218 otherConflict.getUpstreamGtus(getGtu(),
219 DirectIntersectionPerception.this.headwayGtuType, conflictingVisibility);
220 PerceptionCollectable<HeadwayGTU, LaneBasedGTU> downstreamConflictingGTUs =
221 otherConflict.getDownstreamGtus(getGtu(),
222 DirectIntersectionPerception.this.headwayGtuType, conflictingVisibility);
223
224 HeadwayStopLine stopLine = new HeadwayStopLine("stopLineId", Length.ZERO);
225 HeadwayStopLine conflictingStopLine = new HeadwayStopLine("conflictingStopLineId", Length.ZERO);
226
227 Lane thisLane = conflict.getLane();
228 Lane otherLane = otherConflict.getLane();
229 Length pos1a = conflict.getLongitudinalPosition();
230 Length pos2a = otherConflict.getLongitudinalPosition();
231 Length pos1b = Length.min(pos1a.plus(conflict.getLength()), thisLane.getLength());
232 Length pos2b = Length.min(pos2a.plus(otherConflict.getLength()), otherLane.getLength());
233 OTSLine3D line1 = thisLane.getCenterLine();
234 OTSLine3D line2 = otherLane.getCenterLine();
235 double dStart = line1.getLocation(pos1a).distance(line2.getLocation(pos2a));
236 double dEnd = line1.getLocation(pos1b).distance(line2.getLocation(pos2b));
237 Length startWidth = Length.createSI(
238 dStart + .5 * thisLane.getWidth(pos1a).si + .5 * otherLane.getWidth(pos2a).si);
239 Length endWidth = Length
240 .createSI(dEnd + .5 * thisLane.getWidth(pos1b).si + .5 * otherLane.getWidth(pos2b).si);
241
242 headwayConflict = new HeadwayConflict(conflictType, conflictPriority, conflictRuleType, id,
243 distance, length, conflictingLength, upstreamConflictingGTUs, downstreamConflictingGTUs,
244 conflictingVisibility, conflictingSpeedLimit, conflictingLink,
245 HeadwayConflict.Width.linear(startWidth, endWidth), stopLine, conflictingStopLine);
246 }
247 catch (GTUException | OTSGeometryException exception)
248 {
249 throw new RuntimeException("Could not create headway objects.", exception);
250 }
251
252
253
254
255
256 return headwayConflict;
257 }
258 };
259 this.conflicts.put(lane, new TimeStampedObject<>(it, getTimestamp()));
260 }
261
262
263 for (RelativeLane lane : new RelativeLane[] { RelativeLane.LEFT, RelativeLane.RIGHT })
264 {
265 if (getPerception().getLaneStructure().getExtendedCrossSection().contains(lane))
266 {
267 SortedSet<Entry<Conflict>> conflictEntries = getPerception().getLaneStructure().getUpstreamObjects(lane,
268 Conflict.class, getGtu(), RelativePosition.FRONT);
269 boolean alongside = false;
270 if (!conflictEntries.isEmpty())
271 {
272 Entry<Conflict> entry = conflictEntries.first();
273 alongside = entry.getDistance().si < entry.getLaneBasedObject().getLength().si + getGtu().getLength().si;
274 }
275 if (lane.isLeft())
276 {
277 this.alongsideConflictLeft = new TimeStampedObject<>(alongside, getTimestamp());
278 }
279 else
280 {
281 this.alongsideConflictRight = new TimeStampedObject<>(alongside, getTimestamp());
282 }
283 }
284 }
285 }
286
287
288 @Override
289 public final Iterable<HeadwayTrafficLight> getTrafficLights(final RelativeLane lane)
290 {
291 return getObjectOrNull(this.trafficLights.get(lane));
292 }
293
294
295 @Override
296 public final PerceptionCollectable<HeadwayConflict, Conflict> getConflicts(final RelativeLane lane)
297 {
298 return getObjectOrNull(this.conflicts.get(lane));
299 }
300
301
302 @Override
303 public final boolean isAlongsideConflictLeft()
304 {
305 return this.alongsideConflictLeft == null ? false : this.alongsideConflictLeft.getObject();
306 }
307
308
309 @Override
310 public final boolean isAlongsideConflictRight()
311 {
312 return this.alongsideConflictRight == null ? false : this.alongsideConflictRight.getObject();
313 }
314
315
316
317
318
319
320 public final TimeStampedObject<PerceptionCollectable<HeadwayTrafficLight, TrafficLight>> getTimeStampedTrafficLights(
321 final RelativeLane lane)
322 {
323 return this.trafficLights.get(lane);
324 }
325
326
327
328
329
330
331 public final TimeStampedObject<PerceptionCollectable<HeadwayConflict, Conflict>> getTimeStampedConflicts(
332 final RelativeLane lane)
333 {
334 return this.conflicts.get(lane);
335 }
336
337
338
339
340
341 public final TimeStampedObject<Boolean> isAlongsideConflictLeftTimeStamped()
342 {
343 return this.alongsideConflictLeft;
344 }
345
346
347
348
349
350 public final TimeStampedObject<Boolean> isAlongsideConflictRightTimeStamped()
351 {
352 return this.alongsideConflictRight;
353 }
354
355
356 @Override
357 public final String toString()
358 {
359 return "DirectIntersectionPerception";
360 }
361
362 }