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