1 package org.opentrafficsim.road.gtu.lane.tactical;
2
3 import java.io.Serializable;
4 import java.util.ArrayList;
5 import java.util.Iterator;
6 import java.util.LinkedHashSet;
7 import java.util.List;
8 import java.util.Set;
9
10 import org.djunits.value.vdouble.scalar.Length;
11 import org.opentrafficsim.base.OtsClassUtil;
12 import org.opentrafficsim.base.parameters.ParameterTypeClass;
13 import org.opentrafficsim.base.parameters.ParameterTypeDuration;
14 import org.opentrafficsim.base.parameters.ParameterTypeLength;
15 import org.opentrafficsim.base.parameters.ParameterTypes;
16 import org.opentrafficsim.core.geometry.OtsGeometryException;
17 import org.opentrafficsim.core.geometry.OtsLine3d;
18 import org.opentrafficsim.core.gtu.GtuException;
19 import org.opentrafficsim.core.network.LateralDirectionality;
20 import org.opentrafficsim.core.network.Link;
21 import org.opentrafficsim.core.network.NetworkException;
22 import org.opentrafficsim.core.network.Node;
23 import org.opentrafficsim.road.gtu.lane.LaneBasedGtu;
24 import org.opentrafficsim.road.gtu.lane.perception.LanePerception;
25 import org.opentrafficsim.road.gtu.lane.tactical.following.CarFollowingModel;
26 import org.opentrafficsim.road.gtu.lane.tactical.lmrs.Lmrs;
27 import org.opentrafficsim.road.network.lane.CrossSectionElement;
28 import org.opentrafficsim.road.network.lane.CrossSectionLink;
29 import org.opentrafficsim.road.network.lane.Lane;
30 import org.opentrafficsim.road.network.lane.LanePosition;
31
32 /**
33 * A lane-based tactical planner generates an operational plan for the lane-based GTU. It can ask the strategic planner for
34 * assistance on the route to take when the network splits. This abstract class contains a number of helper methods that make it
35 * easy to implement a tactical planner.
36 * <p>
37 * Copyright (c) 2013-2023 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved. <br>
38 * BSD-style license. See <a href="https://opentrafficsim.org/docs/license.html">OpenTrafficSim License</a>.
39 * </p>
40 * @author <a href="https://github.com/averbraeck">Alexander Verbraeck</a>
41 * @author <a href="https://tudelft.nl/staff/p.knoppers-1">Peter Knoppers</a>
42 */
43 public abstract class AbstractLaneBasedTacticalPlanner implements LaneBasedTacticalPlanner, Serializable
44 {
45
46 /** Tactical planner parameter. */
47 public static final ParameterTypeClass<LaneBasedTacticalPlanner> TACTICAL_PLANNER;
48
49 static
50 {
51 Class<LaneBasedTacticalPlanner> type = LaneBasedTacticalPlanner.class;
52 TACTICAL_PLANNER = new ParameterTypeClass<>("tactical planner", "Tactical planner class.",
53 OtsClassUtil.getTypedClass(type), Lmrs.class);
54 }
55
56 /** */
57 private static final long serialVersionUID = 20151125L;
58
59 /** Look ahead parameter type. */
60 protected static final ParameterTypeLength LOOKAHEAD = ParameterTypes.LOOKAHEAD;
61
62 /** Time step parameter type. */
63 protected static final ParameterTypeDuration DT = ParameterTypes.DT;
64
65 /** The car-following model. */
66 private CarFollowingModel carFollowingModel;
67
68 /** The perception. */
69 private final LanePerception lanePerception;
70
71 /** GTU. */
72 private final LaneBasedGtu gtu;
73
74 /**
75 * Instantiates a tactical planner.
76 * @param carFollowingModel CarFollowingModel; car-following model
77 * @param gtu LaneBasedGtu; GTU
78 * @param lanePerception LanePerception; perception
79 */
80 public AbstractLaneBasedTacticalPlanner(final CarFollowingModel carFollowingModel, final LaneBasedGtu gtu,
81 final LanePerception lanePerception)
82 {
83 setCarFollowingModel(carFollowingModel);
84 this.gtu = gtu;
85 this.lanePerception = lanePerception;
86 }
87
88 /** {@inheritDoc} */
89 @Override
90 public final LaneBasedGtu getGtu()
91 {
92 return this.gtu;
93 }
94
95 /**
96 * Build a list of lanes forward, with a maximum headway relative to the reference point of the GTU.
97 * @param gtu LaneBasedGtu; the GTU for which to calculate the lane list
98 * @param maxHeadway Length; the maximum length for which lanes should be returned
99 * @return LanePathInfo; an instance that provides the following information for an operational plan: the lanes to follow,
100 * and the path to follow when staying on the same lane.
101 * @throws GtuException when the vehicle is not on one of the lanes on which it is registered
102 * @throws NetworkException when the strategic planner is not able to return a next node in the route
103 */
104 public static LanePathInfo buildLanePathInfo(final LaneBasedGtu gtu, final Length maxHeadway)
105 throws GtuException, NetworkException
106 {
107 LanePosition dlp = gtu.getReferencePosition();
108 return buildLanePathInfo(gtu, maxHeadway, dlp.getLane(), dlp.getPosition());
109 }
110
111 /**
112 * Build a list of lanes forward, with a maximum headway relative to the reference point of the GTU.
113 * @param gtu LaneBasedGtu; the GTU for which to calculate the lane list
114 * @param maxHeadway Length; the maximum length for which lanes should be returned
115 * @param startLane Lane; the lane in which the path starts
116 * @param position Length; the position on the start lane
117 * @return LanePathInfo; an instance that provides the following information for an operational plan: the lanes to follow,
118 * and the path to follow when staying on the same lane.
119 * @throws GtuException when the vehicle is not on one of the lanes on which it is registered
120 * @throws NetworkException when the strategic planner is not able to return a next node in the route
121 * @throws GtuException when the vehicle is not on one of the lanes on which it is registered
122 * @throws NetworkException when the strategic planner is not able to return a next node in the route
123 */
124 public static LanePathInfo buildLanePathInfo(final LaneBasedGtu gtu, final Length maxHeadway, final Lane startLane,
125 final Length position) throws GtuException, NetworkException
126 {
127 List<Lane> laneListForward = new ArrayList<>();
128 Lane lane = startLane;
129 Length startPosition = position;
130 Lane lastLane = lane;
131 laneListForward.add(lastLane);
132 Length distanceToEndOfLane;
133 OtsLine3d path;
134 try
135 {
136 distanceToEndOfLane = lane.getLength().minus(position);
137 path = lane.getCenterLine().extract(position, lane.getLength());
138 }
139 catch (OtsGeometryException exception)
140 {
141 // System.err.println(gtu + ": " + exception.getMessage());
142 // System.err.println(lane + ", len=" + lane.getLength());
143 // System.err.println(position);
144 // throw new GTUException(exception);
145
146 // section on current lane too short, floating point operations cause only a single point at the end of the lane
147 path = null;
148 distanceToEndOfLane = Length.ZERO;
149 laneListForward.clear();
150 startPosition = Length.ZERO;
151 }
152
153 while (distanceToEndOfLane.lt(maxHeadway))
154 {
155 Set<Lane> lanes = lane.nextLanes(gtu.getType());
156 if (lanes.size() == 0)
157 {
158 // Dead end. Return with the list as is.
159 return new LanePathInfo(path, laneListForward, startPosition);
160 }
161 else if (lanes.size() == 1)
162 {
163 // Ask the strategical planner what the next link should be (if known), because the strategical planner knows
164 // best!
165 Link link = gtu.getStrategicalPlanner().nextLink(lane.getParentLink(), gtu.getType());
166 lane = lanes.iterator().next();
167 if (link != null && !lane.getParentLink().equals(link))
168 {
169 // Lane not on route anymore. return with the list as is.
170 return new LanePathInfo(path, laneListForward, startPosition);
171 }
172 }
173 else
174 {
175 // Multiple next lanes; ask the strategical planner where to go.
176 // Note: this is not necessarily a split; it could e.g. be a bike path on a road
177 Link link;
178 try
179 {
180 link = gtu.getStrategicalPlanner().nextLink(lane.getParentLink(), gtu.getType());
181 }
182 catch (NetworkException ne)
183 {
184 // no route found. return the data structure up to this point...
185 return new LanePathInfo(path, laneListForward, startPosition);
186 }
187 Link nextLink = link;
188 Lane newLane = null;
189 for (Lane nextLane : lanes)
190 {
191 if (nextLane.getParentLink().equals(nextLink))
192 {
193 newLane = nextLane;
194 break;
195 }
196 }
197 if (newLane == null)
198 {
199 // we cannot reach the next node on this lane -- we have to make a lane change!
200 // return the data structure up to this point...
201 return new LanePathInfo(path, laneListForward, startPosition);
202 }
203 // otherwise: continue!
204 lane = newLane;
205 }
206
207 // determine direction for the path
208 try
209 {
210 path = concatenateNull(path, lane.getCenterLine());
211 // path = OtsLine3d.concatenate(Lane.MARGIN.si, path, lane.getCenterLine());
212 }
213 catch (OtsGeometryException exception)
214 {
215 throw new GtuException(exception);
216 }
217
218 laneListForward.add(lastLane);
219 distanceToEndOfLane = distanceToEndOfLane.plus(lastLane.getLength());
220
221 }
222 return new LanePathInfo(path, laneListForward, startPosition);
223 }
224
225 /**
226 * Concatenate two paths, where the first may be {@code null}.
227 * @param path OtsLine3d; path, may be {@code null}
228 * @param centerLine OtsLine3d; center line of lane to add
229 * @return concatenated line
230 * @throws OtsGeometryException when lines are degenerate or too distant
231 */
232 public static OtsLine3d concatenateNull(final OtsLine3d path, final OtsLine3d centerLine) throws OtsGeometryException
233 {
234 if (path == null)
235 {
236 return centerLine;
237 }
238 return OtsLine3d.concatenate(Lane.MARGIN.si, path, centerLine);
239 }
240
241 /**
242 * Calculate the next location where the network splits, with a maximum headway relative to the reference point of the GTU.
243 * Note: a lane drop is also considered a split (!).
244 * @param gtu LaneBasedGtu; the GTU for which to calculate the lane list
245 * @param maxHeadway Length; the maximum length for which lanes should be returned
246 * @return NextSplitInfo; an instance that provides the following information for an operational plan: whether the network
247 * splits, the node where it splits, and the current lanes that lead to the right node after the split node.
248 * @throws GtuException when the vehicle is not on one of the lanes on which it is registered
249 * @throws NetworkException when the strategic planner is not able to return a next node in the route
250 */
251 public static NextSplitInfo determineNextSplit(final LaneBasedGtu gtu, final Length maxHeadway)
252 throws GtuException, NetworkException
253 {
254 Node nextSplitNode = null;
255 Set<Lane> correctCurrentLanes = new LinkedHashSet<>();
256 LanePosition dlp = gtu.getReferencePosition();
257 Lane referenceLane = dlp.getLane();
258 double refFrac = dlp.getPosition().si / referenceLane.getLength().si;
259 Link lastLink = referenceLane.getParentLink();
260 Length position = dlp.getPosition();
261 Length lengthForward = referenceLane.getLength().minus(position);
262 Node lastNode = referenceLane.getParentLink().getEndNode();
263
264 // see if we have a split within maxHeadway distance
265 while (lengthForward.lt(maxHeadway) && nextSplitNode == null)
266 {
267 // calculate the number of "outgoing" links
268 Set<Link> links = lastNode.getLinks().toSet(); // safe copy
269 Iterator<Link> linkIterator = links.iterator();
270 while (linkIterator.hasNext())
271 {
272 Link link = linkIterator.next();
273 if (!link.getType().isCompatible(gtu.getType()) || link.getEndNode().equals(lastNode))
274 {
275 linkIterator.remove();
276 }
277 }
278
279 // calculate the number of incoming and outgoing lanes on the link
280 boolean laneChange = false;
281 if (links.size() == 1)
282 {
283 for (CrossSectionElement cse : ((CrossSectionLink) lastLink).getCrossSectionElementList())
284 {
285 if (cse instanceof Lane)
286 {
287 Lane lane = (Lane) cse;
288 if (lane.nextLanes(gtu.getType()).size() == 0)
289 {
290 laneChange = true;
291 }
292 }
293 }
294 }
295
296 // see if we have a lane drop
297 if (laneChange)
298 {
299 nextSplitNode = lastNode;
300 // which lane(s) we are registered on and adjacent lanes link to a lane
301 // that does not drop?
302 for (CrossSectionElement cse : referenceLane.getParentLink().getCrossSectionElementList())
303 {
304 if (cse instanceof Lane)
305 {
306 Lane l = (Lane) cse;
307 if (noLaneDrop(gtu, maxHeadway, l, l.getLength().times(refFrac)))
308 {
309 correctCurrentLanes.add(l);
310 }
311 }
312 }
313 return new NextSplitInfo(nextSplitNode, correctCurrentLanes);
314 }
315
316 // see if we have a split
317 if (links.size() > 1)
318 {
319 nextSplitNode = lastNode;
320 Link nextLink = gtu.getStrategicalPlanner().nextLink(lastLink, gtu.getType());
321 // which lane(s) we are registered on and adjacent lanes link to a lane
322 // that is on the route at the next split?
323 for (CrossSectionElement cse : referenceLane.getParentLink().getCrossSectionElementList())
324 {
325 if (cse instanceof Lane)
326 {
327 Lane l = (Lane) cse;
328 if (connectsToPath(gtu, maxHeadway, l, l.getLength().times(refFrac), nextLink))
329 {
330 correctCurrentLanes.add(l);
331 }
332 }
333 }
334 if (correctCurrentLanes.size() > 0)
335 {
336 return new NextSplitInfo(nextSplitNode, correctCurrentLanes);
337 }
338 // split, but no lane on current link to right direction
339 Set<Lane> correctLanes = new LinkedHashSet<>();
340 Set<Lane> wrongLanes = new LinkedHashSet<>();
341 for (CrossSectionElement cse : ((CrossSectionLink) lastLink).getCrossSectionElementList())
342 {
343 if (cse instanceof Lane)
344 {
345 Lane l = (Lane) cse;
346 if (connectsToPath(gtu, maxHeadway.plus(l.getLength()), l, Length.ZERO, nextLink))
347 {
348 correctLanes.add(l);
349 }
350 else
351 {
352 wrongLanes.add(l);
353 }
354 }
355 }
356 for (Lane wrongLane : wrongLanes)
357 {
358 for (Lane adjLane : wrongLane.accessibleAdjacentLanesLegal(LateralDirectionality.LEFT, gtu.getType()))
359 {
360 if (correctLanes.contains(adjLane))
361 {
362 return new NextSplitInfo(nextSplitNode, correctCurrentLanes, LateralDirectionality.LEFT);
363 }
364 }
365 for (Lane adjLane : wrongLane.accessibleAdjacentLanesLegal(LateralDirectionality.RIGHT, gtu.getType()))
366 {
367 if (correctLanes.contains(adjLane))
368 {
369 return new NextSplitInfo(nextSplitNode, correctCurrentLanes, LateralDirectionality.RIGHT);
370 }
371 }
372 }
373 return new NextSplitInfo(nextSplitNode, correctCurrentLanes, null);
374 }
375
376 if (links.size() == 0)
377 {
378 return new NextSplitInfo(null, correctCurrentLanes);
379 }
380
381 // just one link
382 Link link = links.iterator().next();
383
384 // determine direction for the path
385 lastNode = link.getEndNode();
386 lastLink = links.iterator().next();
387 lengthForward = lengthForward.plus(lastLink.getLength());
388 }
389
390 return new NextSplitInfo(null, correctCurrentLanes);
391 }
392
393 /**
394 * Determine whether the lane is directly connected to our route, in other words: if we would (continue to) drive on the
395 * given lane, can we take the right branch at the nextSplitNode without switching lanes?
396 * @param gtu LaneBasedGtu; the GTU for which we have to determine the lane suitability
397 * @param maxHeadway Length; the maximum length for use in the calculation
398 * @param startLane Lane; the first lane in the list
399 * @param startLanePosition Length; the position on the start lane
400 * @param linkAfterSplit Link; the link after the split to which we should connect
401 * @return boolean; true if the lane (XXXXX which lane?) is connected to our path
402 * @throws GtuException when the vehicle is not on one of the lanes on which it is registered
403 * @throws NetworkException when the strategic planner is not able to return a next node in the route
404 */
405 protected static boolean connectsToPath(final LaneBasedGtu gtu, final Length maxHeadway, final Lane startLane,
406 final Length startLanePosition, final Link linkAfterSplit) throws GtuException, NetworkException
407 {
408 List<Lane> lanes = buildLanePathInfo(gtu, maxHeadway, startLane, startLanePosition).getLaneList();
409 for (Lane lane : lanes)
410 {
411 if (lane.getParentLink().equals(linkAfterSplit))
412 {
413 return true;
414 }
415 }
416 return false;
417 }
418
419 /**
420 * Determine whether the lane does not drop, in other words: if we would (continue to) drive on the given lane, can we
421 * continue to drive at the nextSplitNode without switching lanes?
422 * @param gtu LaneBasedGtu; the GTU for which we have to determine the lane suitability
423 * @param maxHeadway Length; the maximum length for use in the calculation
424 * @param startLane Lane; the first lane in the list
425 * @param startLanePosition Length; the position on the start lane
426 * @return boolean; true if the lane (XXX which lane?) is connected to our path
427 * @throws GtuException when the vehicle is not on one of the lanes on which it is registered
428 * @throws NetworkException when the strategic planner is not able to return a next node in the route
429 */
430 protected static boolean noLaneDrop(final LaneBasedGtu gtu, final Length maxHeadway, final Lane startLane,
431 final Length startLanePosition) throws GtuException, NetworkException
432 {
433 LanePathInfo lpi = buildLanePathInfo(gtu, maxHeadway, startLane, startLanePosition);
434 if (lpi.getPath().getLength().lt(maxHeadway))
435 {
436 return false;
437 }
438 return true;
439 }
440
441 /**
442 * Make a list of links on which to drive next, with a maximum headway relative to the reference point of the GTU.
443 * @param gtu LaneBasedGtu; the GTU for which to calculate the link list
444 * @param maxHeadway Length; the maximum length for which links should be returned
445 * @return List<Link>; a list of links on which to drive next
446 * @throws GtuException when the vehicle is not on one of the lanes on which it is registered
447 * @throws NetworkException when the strategic planner is not able to return a next node in the route
448 */
449 protected static List<Link> buildLinkListForward(final LaneBasedGtu gtu, final Length maxHeadway)
450 throws GtuException, NetworkException
451 {
452 List<Link> linkList = new ArrayList<>();
453 LanePosition dlp = gtu.getReferencePosition();
454 Lane referenceLane = dlp.getLane();
455 Link lastLink = referenceLane.getParentLink();
456 linkList.add(lastLink);
457 Length position = dlp.getPosition();
458 Length lengthForward = referenceLane.getLength().minus(position);
459 Node lastNode = referenceLane.getParentLink().getEndNode();
460
461 // see if we have a split within maxHeadway distance
462 while (lengthForward.lt(maxHeadway))
463 {
464 // calculate the number of "outgoing" links
465 Set<Link> links = lastNode.getLinks().toSet(); // is a safe copy
466 Iterator<Link> linkIterator = links.iterator();
467 while (linkIterator.hasNext())
468 {
469 Link link = linkIterator.next();
470 if (link.equals(lastLink) || !link.getType().isCompatible(gtu.getType()))
471 {
472 linkIterator.remove();
473 }
474 }
475 if (links.size() == 0)
476 {
477 return linkList; // the path stops here...
478 }
479
480 Link link;
481 if (links.size() > 1)
482 {
483 link = gtu.getStrategicalPlanner().nextLink(lastLink, gtu.getType());
484 }
485 else
486 {
487 link = links.iterator().next();
488 }
489
490 // determine direction for the path
491 lastNode = lastLink.getEndNode();
492 lastLink = link;
493 linkList.add(lastLink);
494 lengthForward = lengthForward.plus(lastLink.getLength());
495 }
496 return linkList;
497 }
498
499 /** {@inheritDoc} */
500 @Override
501 public final CarFollowingModel getCarFollowingModel()
502 {
503 return this.carFollowingModel;
504 }
505
506 /**
507 * Sets the car-following model.
508 * @param carFollowingModel CarFollowingModel; Car-following model to set.
509 */
510 public final void setCarFollowingModel(final CarFollowingModel carFollowingModel)
511 {
512 this.carFollowingModel = carFollowingModel;
513 }
514
515 /** {@inheritDoc} */
516 @Override
517 public final LanePerception getPerception()
518 {
519 return this.lanePerception;
520 }
521
522 }