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.OtsLine2d;
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-2024 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.lane(), dlp.position());
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 OtsLine2d path;
134 distanceToEndOfLane = lane.getLength().minus(position);
135 path = lane.getCenterLine().extract(position, lane.getLength());
136
137 while (distanceToEndOfLane.lt(maxHeadway))
138 {
139 Set<Lane> lanes = lane.nextLanes(gtu.getType());
140 if (lanes.size() == 0)
141 {
142 // Dead end. Return with the list as is.
143 return new LanePathInfo(path, laneListForward, startPosition);
144 }
145 else if (lanes.size() == 1)
146 {
147 // Ask the strategical planner what the next link should be (if known), because the strategical planner knows
148 // best!
149 Link link = gtu.getStrategicalPlanner().nextLink(lane.getLink(), gtu.getType());
150 lane = lanes.iterator().next();
151 if (link != null && !lane.getLink().equals(link))
152 {
153 // Lane not on route anymore. return with the list as is.
154 return new LanePathInfo(path, laneListForward, startPosition);
155 }
156 }
157 else
158 {
159 // Multiple next lanes; ask the strategical planner where to go.
160 // Note: this is not necessarily a split; it could e.g. be a bike path on a road
161 Link link;
162 try
163 {
164 link = gtu.getStrategicalPlanner().nextLink(lane.getLink(), gtu.getType());
165 }
166 catch (NetworkException ne)
167 {
168 // no route found. return the data structure up to this point...
169 return new LanePathInfo(path, laneListForward, startPosition);
170 }
171 Link nextLink = link;
172 Lane newLane = null;
173 for (Lane nextLane : lanes)
174 {
175 if (nextLane.getLink().equals(nextLink))
176 {
177 newLane = nextLane;
178 break;
179 }
180 }
181 if (newLane == null)
182 {
183 // we cannot reach the next node on this lane -- we have to make a lane change!
184 // return the data structure up to this point...
185 return new LanePathInfo(path, laneListForward, startPosition);
186 }
187 // otherwise: continue!
188 lane = newLane;
189 }
190
191 // determine direction for the path
192 try
193 {
194 path = concatenateNull(path, lane.getCenterLine());
195 // path = OtsLine2d.concatenate(Lane.MARGIN.si, path, lane.getCenterLine());
196 }
197 catch (OtsGeometryException exception)
198 {
199 throw new GtuException(exception);
200 }
201
202 laneListForward.add(lastLane);
203 distanceToEndOfLane = distanceToEndOfLane.plus(lastLane.getLength());
204
205 }
206 return new LanePathInfo(path, laneListForward, startPosition);
207 }
208
209 /**
210 * Concatenate two paths, where the first may be {@code null}.
211 * @param path OtsLine2d; path, may be {@code null}
212 * @param centerLine OtsLine2d; center line of lane to add
213 * @return concatenated line
214 * @throws OtsGeometryException when lines are degenerate or too distant
215 */
216 public static OtsLine2d concatenateNull(final OtsLine2d path, final OtsLine2d centerLine) throws OtsGeometryException
217 {
218 if (path == null)
219 {
220 return centerLine;
221 }
222 return OtsLine2d.concatenate(Lane.MARGIN.si, path, centerLine);
223 }
224
225 /**
226 * Calculate the next location where the network splits, with a maximum headway relative to the reference point of the GTU.
227 * Note: a lane drop is also considered a split (!).
228 * @param gtu LaneBasedGtu; the GTU for which to calculate the lane list
229 * @param maxHeadway Length; the maximum length for which lanes should be returned
230 * @return NextSplitInfo; an instance that provides the following information for an operational plan: whether the network
231 * splits, the node where it splits, and the current lanes that lead to the right node after the split node.
232 * @throws GtuException when the vehicle is not on one of the lanes on which it is registered
233 * @throws NetworkException when the strategic planner is not able to return a next node in the route
234 */
235 public static NextSplitInfo determineNextSplit(final LaneBasedGtu gtu, final Length maxHeadway)
236 throws GtuException, NetworkException
237 {
238 Node nextSplitNode = null;
239 Set<Lane> correctCurrentLanes = new LinkedHashSet<>();
240 LanePosition dlp = gtu.getReferencePosition();
241 Lane referenceLane = dlp.lane();
242 double refFrac = dlp.position().si / referenceLane.getLength().si;
243 Link lastLink = referenceLane.getLink();
244 Length position = dlp.position();
245 Length lengthForward = referenceLane.getLength().minus(position);
246 Node lastNode = referenceLane.getLink().getEndNode();
247
248 // see if we have a split within maxHeadway distance
249 while (lengthForward.lt(maxHeadway) && nextSplitNode == null)
250 {
251 // calculate the number of "outgoing" links
252 Set<Link> links = lastNode.getLinks().toSet(); // safe copy
253 Iterator<Link> linkIterator = links.iterator();
254 while (linkIterator.hasNext())
255 {
256 Link link = linkIterator.next();
257 if (!link.getType().isCompatible(gtu.getType()) || link.getEndNode().equals(lastNode))
258 {
259 linkIterator.remove();
260 }
261 }
262
263 // calculate the number of incoming and outgoing lanes on the link
264 boolean laneChange = false;
265 if (links.size() == 1)
266 {
267 for (CrossSectionElement cse : ((CrossSectionLink) lastLink).getCrossSectionElementList())
268 {
269 if (cse instanceof Lane)
270 {
271 Lane lane = (Lane) cse;
272 if (lane.nextLanes(gtu.getType()).size() == 0)
273 {
274 laneChange = true;
275 }
276 }
277 }
278 }
279
280 // see if we have a lane drop
281 if (laneChange)
282 {
283 nextSplitNode = lastNode;
284 // which lane(s) we are registered on and adjacent lanes link to a lane
285 // that does not drop?
286 for (CrossSectionElement cse : referenceLane.getLink().getCrossSectionElementList())
287 {
288 if (cse instanceof Lane)
289 {
290 Lane l = (Lane) cse;
291 if (noLaneDrop(gtu, maxHeadway, l, l.getLength().times(refFrac)))
292 {
293 correctCurrentLanes.add(l);
294 }
295 }
296 }
297 return new NextSplitInfo(nextSplitNode, correctCurrentLanes);
298 }
299
300 // see if we have a split
301 if (links.size() > 1)
302 {
303 nextSplitNode = lastNode;
304 Link nextLink = gtu.getStrategicalPlanner().nextLink(lastLink, gtu.getType());
305 // which lane(s) we are registered on and adjacent lanes link to a lane
306 // that is on the route at the next split?
307 for (CrossSectionElement cse : referenceLane.getLink().getCrossSectionElementList())
308 {
309 if (cse instanceof Lane)
310 {
311 Lane l = (Lane) cse;
312 if (connectsToPath(gtu, maxHeadway, l, l.getLength().times(refFrac), nextLink))
313 {
314 correctCurrentLanes.add(l);
315 }
316 }
317 }
318 if (correctCurrentLanes.size() > 0)
319 {
320 return new NextSplitInfo(nextSplitNode, correctCurrentLanes);
321 }
322 // split, but no lane on current link to right direction
323 Set<Lane> correctLanes = new LinkedHashSet<>();
324 Set<Lane> wrongLanes = new LinkedHashSet<>();
325 for (CrossSectionElement cse : ((CrossSectionLink) lastLink).getCrossSectionElementList())
326 {
327 if (cse instanceof Lane)
328 {
329 Lane l = (Lane) cse;
330 if (connectsToPath(gtu, maxHeadway.plus(l.getLength()), l, Length.ZERO, nextLink))
331 {
332 correctLanes.add(l);
333 }
334 else
335 {
336 wrongLanes.add(l);
337 }
338 }
339 }
340 for (Lane wrongLane : wrongLanes)
341 {
342 for (Lane adjLane : wrongLane.accessibleAdjacentLanesLegal(LateralDirectionality.LEFT, gtu.getType()))
343 {
344 if (correctLanes.contains(adjLane))
345 {
346 return new NextSplitInfo(nextSplitNode, correctCurrentLanes, LateralDirectionality.LEFT);
347 }
348 }
349 for (Lane adjLane : wrongLane.accessibleAdjacentLanesLegal(LateralDirectionality.RIGHT, gtu.getType()))
350 {
351 if (correctLanes.contains(adjLane))
352 {
353 return new NextSplitInfo(nextSplitNode, correctCurrentLanes, LateralDirectionality.RIGHT);
354 }
355 }
356 }
357 return new NextSplitInfo(nextSplitNode, correctCurrentLanes, null);
358 }
359
360 if (links.size() == 0)
361 {
362 return new NextSplitInfo(null, correctCurrentLanes);
363 }
364
365 // just one link
366 Link link = links.iterator().next();
367
368 // determine direction for the path
369 lastNode = link.getEndNode();
370 lastLink = links.iterator().next();
371 lengthForward = lengthForward.plus(lastLink.getLength());
372 }
373
374 return new NextSplitInfo(null, correctCurrentLanes);
375 }
376
377 /**
378 * Determine whether the lane is directly connected to our route, in other words: if we would (continue to) drive on the
379 * given lane, can we take the right branch at the nextSplitNode without switching lanes?
380 * @param gtu LaneBasedGtu; the GTU for which we have to determine the lane suitability
381 * @param maxHeadway Length; the maximum length for use in the calculation
382 * @param startLane Lane; the first lane in the list
383 * @param startLanePosition Length; the position on the start lane
384 * @param linkAfterSplit Link; the link after the split to which we should connect
385 * @return boolean; true if the lane (XXXXX which lane?) is connected to our path
386 * @throws GtuException when the vehicle is not on one of the lanes on which it is registered
387 * @throws NetworkException when the strategic planner is not able to return a next node in the route
388 */
389 protected static boolean connectsToPath(final LaneBasedGtu gtu, final Length maxHeadway, final Lane startLane,
390 final Length startLanePosition, final Link linkAfterSplit) throws GtuException, NetworkException
391 {
392 List<Lane> lanes = buildLanePathInfo(gtu, maxHeadway, startLane, startLanePosition).laneList();
393 for (Lane lane : lanes)
394 {
395 if (lane.getLink().equals(linkAfterSplit))
396 {
397 return true;
398 }
399 }
400 return false;
401 }
402
403 /**
404 * Determine whether the lane does not drop, in other words: if we would (continue to) drive on the given lane, can we
405 * continue to drive at the nextSplitNode without switching lanes?
406 * @param gtu LaneBasedGtu; the GTU for which we have to determine the lane suitability
407 * @param maxHeadway Length; the maximum length for use in the calculation
408 * @param startLane Lane; the first lane in the list
409 * @param startLanePosition Length; the position on the start lane
410 * @return boolean; true if the lane (XXX which lane?) is connected to our path
411 * @throws GtuException when the vehicle is not on one of the lanes on which it is registered
412 * @throws NetworkException when the strategic planner is not able to return a next node in the route
413 */
414 protected static boolean noLaneDrop(final LaneBasedGtu gtu, final Length maxHeadway, final Lane startLane,
415 final Length startLanePosition) throws GtuException, NetworkException
416 {
417 LanePathInfo lpi = buildLanePathInfo(gtu, maxHeadway, startLane, startLanePosition);
418 if (lpi.path().getLength().lt(maxHeadway))
419 {
420 return false;
421 }
422 return true;
423 }
424
425 /**
426 * Make a list of links on which to drive next, with a maximum headway relative to the reference point of the GTU.
427 * @param gtu LaneBasedGtu; the GTU for which to calculate the link list
428 * @param maxHeadway Length; the maximum length for which links should be returned
429 * @return List<Link>; a list of links on which to drive next
430 * @throws GtuException when the vehicle is not on one of the lanes on which it is registered
431 * @throws NetworkException when the strategic planner is not able to return a next node in the route
432 */
433 protected static List<Link> buildLinkListForward(final LaneBasedGtu gtu, final Length maxHeadway)
434 throws GtuException, NetworkException
435 {
436 List<Link> linkList = new ArrayList<>();
437 LanePosition dlp = gtu.getReferencePosition();
438 Lane referenceLane = dlp.lane();
439 Link lastLink = referenceLane.getLink();
440 linkList.add(lastLink);
441 Length position = dlp.position();
442 Length lengthForward = referenceLane.getLength().minus(position);
443 Node lastNode = referenceLane.getLink().getEndNode();
444
445 // see if we have a split within maxHeadway distance
446 while (lengthForward.lt(maxHeadway))
447 {
448 // calculate the number of "outgoing" links
449 Set<Link> links = lastNode.getLinks().toSet(); // is a safe copy
450 Iterator<Link> linkIterator = links.iterator();
451 while (linkIterator.hasNext())
452 {
453 Link link = linkIterator.next();
454 if (link.equals(lastLink) || !link.getType().isCompatible(gtu.getType()))
455 {
456 linkIterator.remove();
457 }
458 }
459 if (links.size() == 0)
460 {
461 return linkList; // the path stops here...
462 }
463
464 Link link;
465 if (links.size() > 1)
466 {
467 link = gtu.getStrategicalPlanner().nextLink(lastLink, gtu.getType());
468 }
469 else
470 {
471 link = links.iterator().next();
472 }
473
474 // determine direction for the path
475 lastNode = lastLink.getEndNode();
476 lastLink = link;
477 linkList.add(lastLink);
478 lengthForward = lengthForward.plus(lastLink.getLength());
479 }
480 return linkList;
481 }
482
483 /** {@inheritDoc} */
484 @Override
485 public final CarFollowingModel getCarFollowingModel()
486 {
487 return this.carFollowingModel;
488 }
489
490 /**
491 * Sets the car-following model.
492 * @param carFollowingModel CarFollowingModel; Car-following model to set.
493 */
494 public final void setCarFollowingModel(final CarFollowingModel carFollowingModel)
495 {
496 this.carFollowingModel = carFollowingModel;
497 }
498
499 /** {@inheritDoc} */
500 @Override
501 public final LanePerception getPerception()
502 {
503 return this.lanePerception;
504 }
505
506 }