1 package org.opentrafficsim.road.gtu.lane.perception;
2
3 import java.io.Serializable;
4 import java.util.HashMap;
5 import java.util.HashSet;
6 import java.util.Iterator;
7 import java.util.Map;
8 import java.util.Set;
9 import java.util.SortedSet;
10 import java.util.TreeMap;
11 import java.util.TreeSet;
12
13 import org.djunits.unit.LengthUnit;
14 import org.djunits.value.vdouble.scalar.Length;
15 import org.opentrafficsim.core.gtu.GTU;
16 import org.opentrafficsim.core.gtu.GTUException;
17 import org.opentrafficsim.core.gtu.RelativePosition;
18 import org.opentrafficsim.core.network.route.Route;
19 import org.opentrafficsim.road.network.lane.CrossSectionLink;
20 import org.opentrafficsim.road.network.lane.object.LaneBasedObject;
21
22 import nl.tudelft.simulation.language.Throw;
23
24 /**
25 * This data structure can clearly indicate the lane structure ahead of us, e.g. in the following situation:
26 *
27 * <pre>
28 * (---- a ----)(---- b ----)(---- c ----)(---- d ----)(---- e ----)(---- f ----)(---- g ----)
29 * __________ __________
30 * / _________ 1 / _________ 2
31 * / / / /
32 * __________/ / _______________________/ /
33 * 1 ____________ ____________ /_ _ _ _ _ _/____________ /_ _ _ _ _ _ _ _ _ _ _ _ /
34 * 0 |_ _X_ _ _ _ |_ _ _ _ _ _ |_ _ _ _ _ _ |_ _ _ _ _ _ |_ _ _ _ _ _ |_ _ _ _ _ _ \____________
35 * -1 |____________|_ _ _ _ _ _ |____________|____________| __________|____________|____________| 3
36 * -2 / __________/ \ \
37 * ________/ / \ \___________
38 * 5 _________/ \____________ 4
39 * </pre>
40 *
41 * When the GTU is looking ahead, it needs to know that when it continues to destination 3, it needs to shift one lane to the
42 * right at some point, but <b>not</b> two lanes to the right in link b, and not later than at the end of link f. When it needs
43 * to go to destination 1, it needs to shift to the left in link c. When it has to go to destination 2, it has to shift to the
44 * left, but not earlier than at link e. At node [de], it is possible to leave the rightmost lane of link e, and go to
45 * destination 4. The rightmost lane just splits into two lanes at the end of link d, and the GTU can either continue driving to
46 * destination 3, turn right to destination 4. This means that the right lane of link d has <b>two</b> successor lanes.
47 * <p>
48 * In the data structures, lanes are numbered laterally. Suppose that the lane where vehicle X resides would be number 0.
49 * Consistent with "left is positive" for angles, the lane right of X would have number -1, and entry 5 would have number -2.
50 * <p>
51 * In the data structure, this can be indicated as follows (N = next, P = previous, L = left, R = right, D = lane drop, . =
52 * continued but not in this structure). The merge lane in b is considered "off limits" for the GTUs on the "main" lane -1; the
53 * "main" lane 0 is considered off limits from the exit lanes on c, e, and f. Still, we need to maintain pointers to these
54 * lanes, as we are interested in the GTUs potentially driving next to us, feeding into our lane, etc.
55 *
56 * <pre>
57 * 1 0 -1 -2
58 *
59 * ROOT
60 * _____|_____ ___________ ___________
61 * |_-_|_._|_R_|----|_L_|_._|_-_| |_-_|_._|_-_| a
62 * | | |
63 * _____V_____ _____V_____ _____V_____
64 * |_-_|_N_|_R_|----|_L_|_N_|_R_|<---|_L_|_D_|_-_| b
65 * | |
66 * ___________ _____V_____ _____V_____
67 * |_-_|_N_|_R_|<---|_L_|_N_|_R_|----|_L_|_N_|_-_| c
68 * | | |
69 * _____V_____ _____V_____ _____V_____
70 * |_-_|_._|_-_| |_-_|_N_|_R_|----|_L_|_NN|_-_| d
71 * | ||_______________
72 * ___________ _____V_____ _____V_____ _____V_____
73 * |_-_|_N_|_R_|<---|_L_|_N_|_R_|----|_L_|_N_|_-_| |_-_|_N_|_-_| e
74 * | | | |
75 * _____V_____ _____V_____ _____V_____ _____V_____
76 * |_-_|_N_|_R_|<---|_L_|_D_|_R_|----|_L_|_N_|_-_| |_-_|_._|_-_| f
77 * | |
78 * _____V_____ _____V_____
79 * |_-_|_._|_-_| |_-_|_._|_-_| g
80 * </pre>
81 * <p>
82 * Copyright (c) 2013-2017 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved. <br>
83 * BSD-style license. See <a href="http://opentrafficsim.org/docs/license.html">OpenTrafficSim License</a>.
84 * </p>
85 * $LastChangedDate: 2015-07-24 02:58:59 +0200 (Fri, 24 Jul 2015) $, @version $Revision: 1147 $, by $Author: averbraeck $,
86 * initial version Feb 20, 2016 <br>
87 * @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
88 */
89 public class LaneStructure implements Serializable
90 {
91 /** */
92 private static final long serialVersionUID = 20160400L;
93
94 /** The lanes from which we observe the situation. */
95 private final LaneStructureRecord rootLSR;
96
97 /** Look ahead distance. */
98 private Length lookAhead;
99
100 /** Lane structure records of the cross section. */
101 private TreeMap<RelativeLane, LaneStructureRecord> crossSectionRecords = new TreeMap<>();
102
103 /** Lane structure records grouped per relative lane. */
104 private final Map<RelativeLane, Set<LaneStructureRecord>> relativeLaneMap = new HashMap<>();
105
106 /**
107 * @param rootLSR the root record.
108 * @param lookAhead look ahead distance
109 */
110 public LaneStructure(final LaneStructureRecord rootLSR, final Length lookAhead)
111 {
112 this.rootLSR = rootLSR;
113 this.lookAhead = lookAhead;
114 }
115
116 /**
117 * @return rootLSR
118 */
119 public final LaneStructureRecord getRootLSR()
120 {
121 return this.rootLSR;
122 }
123
124 /**
125 * Returns the cross section.
126 * @return cross section
127 */
128 public final SortedSet<RelativeLane> getCrossSection()
129 {
130 return this.crossSectionRecords.navigableKeySet();
131 }
132
133 /**
134 * @param lane lane to check
135 * @return record at given lane
136 * @throws GTUException if the lane is not in the cross section
137 */
138 public final LaneStructureRecord getLaneLSR(final RelativeLane lane) throws GTUException
139 {
140 Throw.when(!this.crossSectionRecords.containsKey(lane), GTUException.class,
141 "The requested lane %s is not in the most recent cross section.", lane);
142 return this.crossSectionRecords.get(lane);
143 }
144
145 /**
146 * Removes all mappings to relative lanes that are not in the most recent cross section.
147 * @param map map to clear mappings from
148 */
149 public final void removeInvalidMappings(final Map<RelativeLane, ?> map)
150 {
151 Iterator<RelativeLane> iterator = map.keySet().iterator();
152 while (iterator.hasNext())
153 {
154 RelativeLane lane = iterator.next();
155 if (!this.crossSectionRecords.containsKey(lane))
156 {
157 iterator.remove();
158 }
159 }
160 }
161
162 /**
163 * Adds a lane structure record in a mapping from relative lanes. The record is also added to the current cross section if
164 * the start distance is negative, and the start distance plus length is positive. If the relative lane is already in the
165 * current cross section, it is <b>not</b> overwritten.
166 * @param lsr lane structure record
167 * @param relativeLane relative lane
168 */
169 public final void addLaneStructureRecord(final LaneStructureRecord lsr, final RelativeLane relativeLane)
170 {
171 if (!this.relativeLaneMap.containsKey(relativeLane))
172 {
173 this.relativeLaneMap.put(relativeLane, new HashSet<>());
174 }
175 this.relativeLaneMap.get(relativeLane).add(lsr);
176 if (lsr.getStartDistance().le0() && lsr.getStartDistance().plus(lsr.getLane().getLength()).gt0()
177 && (!this.crossSectionRecords.containsKey(relativeLane)
178 || this.crossSectionRecords.get(relativeLane).getStartDistance().gt0() && lsr.getStartDistance().le0()))
179 {
180 this.crossSectionRecords.put(relativeLane, lsr);
181 }
182 }
183
184 /**
185 * Retrieve objects on a lane of a specific type. Returns objects over a maximum length of the look ahead distance
186 * downstream from the relative position, or as far as the lane map goes.
187 * @param lane lane
188 * @param clazz class of objects to find
189 * @param gtu gtu
190 * @param pos relative position to start search from
191 * @param <T> type of objects to find
192 * @return Sorted set of objects of requested type
193 * @throws GTUException if lane is not in current set
194 */
195 @SuppressWarnings("unchecked")
196 public final <T extends LaneBasedObject> SortedSet<Entry<T>> getDownstreamObjects(final RelativeLane lane,
197 final Class<T> clazz, final GTU gtu, final RelativePosition.TYPE pos) throws GTUException
198 {
199 LaneStructureRecord record = null;
200 if (this.crossSectionRecords.containsKey(lane))
201 {
202 record = this.getLaneLSR(lane);
203 }
204 else
205 {
206 Length minLength = new Length(Double.MAX_VALUE, LengthUnit.SI);
207 // not in current cross section, get first downstream
208 for (LaneStructureRecord rec : this.relativeLaneMap.get(lane))
209 {
210 if (rec.getStartDistance().ge0() && rec.getStartDistance().lt(minLength))
211 {
212 record = rec;
213 minLength = rec.getStartDistance();
214 }
215 }
216 }
217 Throw.when(record == null, GTUException.class, "Trying to get objects on %s, but that lane is not in the structure.",
218 lane);
219 Length ds = gtu.getRelativePositions().get(pos).getDx().minus(gtu.getReference().getDx());
220 // the list is ordered, but only for DIR_PLUS, need to do our own ordering
221 Length minimumPosition;
222 Length maximumPosition;
223 if (record.getDirection().isPlus())
224 {
225 minimumPosition = ds.minus(record.getStartDistance());
226 maximumPosition = record.getLane().getLength();
227 }
228 else
229 {
230 minimumPosition = Length.ZERO;
231 maximumPosition = record.getLane().getLength().plus(record.getStartDistance()).minus(ds);
232 }
233 SortedSet<Entry<T>> set = new TreeSet<>();
234 Length distance;
235 for (LaneBasedObject object : record.getLane().getLaneBasedObjects(minimumPosition, maximumPosition))
236 {
237 distance = record.getDistanceToPosition(object.getLongitudinalPosition()).minus(ds);
238 if (clazz.isAssignableFrom(object.getClass()) && distance.le(this.lookAhead)
239 && ((record.getDirection().isPlus() && object.getDirection().isForwardOrBoth())
240 || (record.getDirection().isMinus() && object.getDirection().isBackwardOrBoth())))
241 {
242 // unchecked, but the above isAssignableFrom assures correctness
243 set.add(new Entry<>(distance, (T) object));
244 }
245 }
246 getDownstreamObjectsRecursive(set, record, clazz, ds);
247 return set;
248 }
249
250 /**
251 * Retrieve objects on a lane of a specific type. Returns objects over a maximum length of the look ahead distance
252 * downstream from the relative position, or as far as the lane map goes. Objects on links not on the route are ignored.
253 * @param lane lane
254 * @param clazz class of objects to find
255 * @param gtu gtu
256 * @param pos relative position to start search from
257 * @param <T> type of objects to find
258 * @param route the route
259 * @return Sorted set of objects of requested type
260 * @throws GTUException if lane is not in current set
261 */
262 public final <T extends LaneBasedObject> SortedSet<Entry<T>> getDownstreamObjectsOnRoute(final RelativeLane lane,
263 final Class<T> clazz, final GTU gtu, final RelativePosition.TYPE pos, final Route route) throws GTUException
264 {
265 SortedSet<Entry<T>> set = getDownstreamObjects(lane, clazz, gtu, pos);
266 if (route != null)
267 {
268 Iterator<Entry<T>> iterator = set.iterator();
269 while (iterator.hasNext())
270 {
271 Entry<T> entry = iterator.next();
272 CrossSectionLink link = entry.getLaneBasedObject().getLane().getParentLink();
273 if (!route.contains(link.getStartNode()) || !route.contains(link.getEndNode())
274 || Math.abs(route.indexOf(link.getStartNode()) - route.indexOf(link.getEndNode())) != 1)
275 {
276 iterator.remove();
277 }
278 }
279 }
280 return set;
281 }
282
283 /**
284 * Recursive search for lane based objects downstream.
285 * @param set set to store entries into
286 * @param record current record
287 * @param clazz class of objects to find
288 * @param ds distance from reference to chosen relative position
289 * @param <T> type of objects to find
290 */
291 @SuppressWarnings("unchecked")
292 private <T extends LaneBasedObject> void getDownstreamObjectsRecursive(final SortedSet<Entry<T>> set,
293 final LaneStructureRecord record, final Class<T> clazz, final Length ds)
294 {
295 if (record.getNext().isEmpty() || record.getNext().get(0).getStartDistance().gt(this.lookAhead))
296 {
297 return;
298 }
299 for (LaneStructureRecord next : record.getNext())
300 {
301 Length distance;
302 for (LaneBasedObject object : next.getLane().getLaneBasedObjects())
303 {
304 distance = next.getDistanceToPosition(object.getLongitudinalPosition()).minus(ds);
305 if (clazz.isAssignableFrom(object.getClass()) && distance.le(this.lookAhead)
306 && ((record.getDirection().isPlus() && object.getDirection().isForwardOrBoth())
307 || (record.getDirection().isMinus() && object.getDirection().isBackwardOrBoth())))
308 {
309 // unchecked, but the above isAssignableFrom assures correctness
310 set.add(new Entry<>(distance, (T) object));
311 }
312 }
313 getDownstreamObjectsRecursive(set, next, clazz, ds);
314 }
315 }
316
317 /**
318 * Retrieve objects of a specific type. Returns objects over a maximum length of the look ahead distance downstream from the
319 * relative position, or as far as the lane map goes. Objects on links not on the route are ignored.
320 * @param clazz class of objects to find
321 * @param gtu gtu
322 * @param pos relative position to start search from
323 * @param <T> type of objects to find
324 * @param route the route
325 * @return Sorted set of objects of requested type
326 * @throws GTUException if lane is not in current set
327 */
328 public <T extends LaneBasedObject> Map<RelativeLane, SortedSet<Entry<T>>> getDownstreamObjectsOnRoute(final Class<T> clazz,
329 final GTU gtu, final RelativePosition.TYPE pos, final Route route) throws GTUException
330 {
331 Map<RelativeLane, SortedSet<Entry<T>>> out = new HashMap<>();
332 for (RelativeLane relativeLane : this.relativeLaneMap.keySet())
333 {
334 out.put(relativeLane, getDownstreamObjectsOnRoute(relativeLane, clazz, gtu, pos, route));
335 }
336 return out;
337 }
338
339 /**
340 * Retrieve objects on a lane of a specific type. Returns upstream objects from the relative position for as far as the lane
341 * map goes. Distances to upstream objects are given as positive values.
342 * @param lane lane
343 * @param clazz class of objects to find
344 * @param gtu gtu
345 * @param pos relative position to start search from
346 * @param <T> type of objects to find
347 * @return Sorted set of objects of requested type
348 * @throws GTUException if lane is not in current set
349 */
350 @SuppressWarnings("unchecked")
351 public final <T extends LaneBasedObject> SortedSet<Entry<T>> getUpstreamObjects(final RelativeLane lane,
352 final Class<T> clazz, final GTU gtu, final RelativePosition.TYPE pos) throws GTUException
353 {
354 LaneStructureRecord record = this.getLaneLSR(lane);
355 Length ds = gtu.getReference().getDx().minus(gtu.getRelativePositions().get(pos).getDx());
356 // the list is ordered, but only for DIR_PLUS, need to do our own ordering
357 Length minimumPosition;
358 Length maximumPosition;
359 if (record.getDirection().isPlus())
360 {
361 minimumPosition = Length.ZERO;
362 maximumPosition = record.getStartDistance().neg().minus(ds);
363 }
364 else
365 {
366 minimumPosition = record.getLane().getLength().plus(record.getStartDistance()).plus(ds);
367 maximumPosition = record.getLane().getLength();
368 }
369 SortedSet<Entry<T>> set = new TreeSet<>();
370 Length distance;
371 for (LaneBasedObject object : record.getLane().getLaneBasedObjects(minimumPosition, maximumPosition))
372 {
373 if (clazz.isAssignableFrom(object.getClass())
374 && ((record.getDirection().isPlus() && object.getDirection().isForwardOrBoth())
375 || (record.getDirection().isMinus() && object.getDirection().isBackwardOrBoth())))
376 {
377 distance = record.getDistanceToPosition(object.getLongitudinalPosition()).neg().minus(ds);
378 // unchecked, but the above isAssignableFrom assures correctness
379 set.add(new Entry<>(distance, (T) object));
380 }
381 }
382 getUpstreamObjectsRecursive(set, record, clazz, ds);
383 return set;
384 }
385
386 /**
387 * Recursive search for lane based objects upstream.
388 * @param set set to store entries into
389 * @param record current record
390 * @param clazz class of objects to find
391 * @param ds distance from reference to chosen relative position
392 * @param <T> type of objects to find
393 */
394 @SuppressWarnings("unchecked")
395 private <T extends LaneBasedObject> void getUpstreamObjectsRecursive(final SortedSet<Entry<T>> set,
396 final LaneStructureRecord record, final Class<T> clazz, final Length ds)
397 {
398 for (LaneStructureRecord prev : record.getPrev())
399 {
400 Length distance;
401 for (LaneBasedObject object : prev.getLane().getLaneBasedObjects())
402 {
403 if (clazz.isAssignableFrom(object.getClass())
404 && ((record.getDirection().isPlus() && object.getDirection().isForwardOrBoth())
405 || (record.getDirection().isMinus() && object.getDirection().isBackwardOrBoth())))
406 {
407 distance = prev.getDistanceToPosition(object.getLongitudinalPosition()).neg().minus(ds);
408 // unchecked, but the above isAssignableFrom assures correctness
409 set.add(new Entry<>(distance, (T) object));
410 }
411 }
412 getUpstreamObjectsRecursive(set, prev, clazz, ds);
413 }
414 }
415
416 /** {@inheritDoc} */
417 @Override
418 public final String toString()
419 {
420 return "LaneStructure [rootLSR=" + this.rootLSR + "]";
421 }
422
423 /**
424 * <p>
425 * Copyright (c) 2013-2017 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved.
426 * <br>
427 * BSD-style license. See <a href="http://opentrafficsim.org/docs/current/license.html">OpenTrafficSim License</a>.
428 * <p>
429 * @version $Revision$, $LastChangedDate$, by $Author$, initial version Sep 15, 2016 <br>
430 * @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
431 * @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
432 * @author <a href="http://www.transport.citg.tudelft.nl">Wouter Schakel</a>
433 * @param <T> class of lane based object contained
434 */
435 public static class Entry<T extends LaneBasedObject> implements Comparable<Entry<T>>
436 {
437
438 /** Distance to lane based object. */
439 private final Length distance;
440
441 /** Lane based object. */
442 private final T laneBasedObject;
443
444 /**
445 * @param distance distance to lane based object
446 * @param laneBasedObject lane based object
447 */
448 public Entry(final Length distance, final T laneBasedObject)
449 {
450 this.distance = distance;
451 this.laneBasedObject = laneBasedObject;
452 }
453
454 /**
455 * @return distance.
456 */
457 public final Length getDistance()
458 {
459 return this.distance;
460 }
461
462 /**
463 * @return laneBasedObject.
464 */
465 public final T getLaneBasedObject()
466 {
467 return this.laneBasedObject;
468 }
469
470 /** {@inheritDoc} */
471 @Override
472 public final int hashCode()
473 {
474 final int prime = 31;
475 int result = 1;
476 result = prime * result + ((this.distance == null) ? 0 : this.distance.hashCode());
477 result = prime * result + ((this.laneBasedObject == null) ? 0 : this.laneBasedObject.hashCode());
478 return result;
479 }
480
481 /** {@inheritDoc} */
482 @Override
483 public final boolean equals(final Object obj)
484 {
485 if (this == obj)
486 {
487 return true;
488 }
489 if (obj == null)
490 {
491 return false;
492 }
493 if (getClass() != obj.getClass())
494 {
495 return false;
496 }
497 Entry<?> other = (Entry<?>) obj;
498 if (this.distance == null)
499 {
500 if (other.distance != null)
501 {
502 return false;
503 }
504 }
505 else if (!this.distance.equals(other.distance))
506 {
507 return false;
508 }
509 if (this.laneBasedObject == null)
510 {
511 if (other.laneBasedObject != null)
512 {
513 return false;
514 }
515 }
516 // laneBasedObject does not implement equals...
517 else if (!this.laneBasedObject.equals(other.laneBasedObject))
518 {
519 return false;
520 }
521 return true;
522 }
523
524 /** {@inheritDoc} */
525 @Override
526 public final int compareTo(final Entry<T> arg)
527 {
528 int d = this.distance.compareTo(arg.distance);
529 if (d != 0 || this.laneBasedObject.equals(arg.laneBasedObject))
530 {
531 return d; // different distance (-1 or 1), or same distance but also equal lane based object (0)
532 }
533 return 1; // same distance, unequal lane based object (1)
534 }
535
536 /** {@inheritDoc} */
537 @Override
538 public final String toString()
539 {
540 return "LaneStructure.Entry [distance=" + this.distance + ", laneBasedObject=" + this.laneBasedObject + "]";
541 }
542
543 }
544
545 }