package org.opentrafficsim.road.gtu.lane.tactical; import java.io.Serializable; import java.util.ArrayList; import java.util.Iterator; import java.util.LinkedHashSet; import java.util.List; import java.util.Map; import java.util.Set; import org.djunits.value.vdouble.scalar.Length; import org.opentrafficsim.base.OTSClassUtil; import org.opentrafficsim.base.parameters.ParameterTypeClass; import org.opentrafficsim.base.parameters.ParameterTypeDuration; import org.opentrafficsim.base.parameters.ParameterTypeLength; import org.opentrafficsim.base.parameters.ParameterTypes; import org.opentrafficsim.core.geometry.OTSGeometryException; import org.opentrafficsim.core.geometry.OTSLine3D; import org.opentrafficsim.core.gtu.GTUDirectionality; import org.opentrafficsim.core.gtu.GTUException; import org.opentrafficsim.core.network.LateralDirectionality; import org.opentrafficsim.core.network.Link; import org.opentrafficsim.core.network.LinkDirection; import org.opentrafficsim.core.network.NetworkException; import org.opentrafficsim.core.network.Node; import org.opentrafficsim.road.gtu.lane.LaneBasedGTU; import org.opentrafficsim.road.gtu.lane.perception.LanePerception; import org.opentrafficsim.road.gtu.lane.tactical.following.CarFollowingModel; import org.opentrafficsim.road.gtu.lane.tactical.lmrs.LMRS; import org.opentrafficsim.road.network.lane.CrossSectionElement; import org.opentrafficsim.road.network.lane.CrossSectionLink; import org.opentrafficsim.road.network.lane.DirectedLanePosition; import org.opentrafficsim.road.network.lane.Lane; import org.opentrafficsim.road.network.lane.LaneDirection; /** * A lane-based tactical planner generates an operational plan for the lane-based GTU. It can ask the strategic planner for * assistance on the route to take when the network splits. This abstract class contains a number of helper methods that make it * easy to implement a tactical planner. *

* Copyright (c) 2013-2022 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved.
* BSD-style license. See OpenTrafficSim License. *

* $LastChangedDate: 2015-07-24 02:58:59 +0200 (Fri, 24 Jul 2015) $, @version $Revision: 1147 $, by $Author: averbraeck $, * initial version Nov 25, 2015
* @author Alexander Verbraeck * @author Peter Knoppers */ public abstract class AbstractLaneBasedTacticalPlanner implements LaneBasedTacticalPlanner, Serializable { /** Tactical planner parameter. */ public static final ParameterTypeClass TACTICAL_PLANNER; static { Class type = LaneBasedTacticalPlanner.class; TACTICAL_PLANNER = new ParameterTypeClass<>("tactical planner", "Tactical planner class.", OTSClassUtil.getTypedClass(type), LMRS.class); } /** */ private static final long serialVersionUID = 20151125L; /** Look ahead parameter type. */ protected static final ParameterTypeLength LOOKAHEAD = ParameterTypes.LOOKAHEAD; /** Time step parameter type. */ protected static final ParameterTypeDuration DT = ParameterTypes.DT; /** The car-following model. */ private CarFollowingModel carFollowingModel; /** The perception. */ private final LanePerception lanePerception; /** GTU. */ private final LaneBasedGTU gtu; /** * Instantiates a tactical planner. * @param carFollowingModel CarFollowingModel; car-following model * @param gtu LaneBasedGTU; GTU * @param lanePerception LanePerception; perception */ public AbstractLaneBasedTacticalPlanner(final CarFollowingModel carFollowingModel, final LaneBasedGTU gtu, final LanePerception lanePerception) { setCarFollowingModel(carFollowingModel); this.gtu = gtu; this.lanePerception = lanePerception; } /** {@inheritDoc} */ @Override public final LaneBasedGTU getGtu() { return this.gtu; } /** * Build a list of lanes forward, with a maximum headway relative to the reference point of the GTU. * @param gtu LaneBasedGTU; the GTU for which to calculate the lane list * @param maxHeadway Length; the maximum length for which lanes should be returned * @return LanePathInfo; an instance that provides the following information for an operational plan: the lanes to follow, * and the path to follow when staying on the same lane. * @throws GTUException when the vehicle is not on one of the lanes on which it is registered * @throws NetworkException when the strategic planner is not able to return a next node in the route */ public static LanePathInfo buildLanePathInfo(final LaneBasedGTU gtu, final Length maxHeadway) throws GTUException, NetworkException { DirectedLanePosition dlp = gtu.getReferencePosition(); return buildLanePathInfo(gtu, maxHeadway, dlp.getLane(), dlp.getPosition(), dlp.getGtuDirection()); } /** * Build a list of lanes forward, with a maximum headway relative to the reference point of the GTU. * @param gtu LaneBasedGTU; the GTU for which to calculate the lane list * @param maxHeadway Length; the maximum length for which lanes should be returned * @param startLane Lane; the lane in which the path starts * @param position Length; the position on the start lane * @param startDirectionality GTUDirectionality; the driving direction on the start lane * @return LanePathInfo; an instance that provides the following information for an operational plan: the lanes to follow, * and the path to follow when staying on the same lane. * @throws GTUException when the vehicle is not on one of the lanes on which it is registered * @throws NetworkException when the strategic planner is not able to return a next node in the route * @throws GTUException when the vehicle is not on one of the lanes on which it is registered * @throws NetworkException when the strategic planner is not able to return a next node in the route */ public static LanePathInfo buildLanePathInfo(final LaneBasedGTU gtu, final Length maxHeadway, final Lane startLane, final Length position, final GTUDirectionality startDirectionality) throws GTUException, NetworkException { List laneListForward = new ArrayList<>(); Lane lane = startLane; GTUDirectionality lastGtuDir = startDirectionality; Length startPosition = position; Lane lastLane = lane; laneListForward.add(new LaneDirection(lastLane, lastGtuDir)); Length distanceToEndOfLane; OTSLine3D path; try { if (lastGtuDir.equals(GTUDirectionality.DIR_PLUS)) { distanceToEndOfLane = lane.getLength().minus(position); path = lane.getCenterLine().extract(position, lane.getLength()); } else { distanceToEndOfLane = position; path = lane.getCenterLine().extract(Length.ZERO, position).reverse(); } } catch (OTSGeometryException exception) { // System.err.println(gtu + ": " + exception.getMessage()); // System.err.println(lane + ", len=" + lane.getLength()); // System.err.println(position); // throw new GTUException(exception); // section on current lane too short, floating point operations cause only a single point at the end of the lane path = null; distanceToEndOfLane = Length.ZERO; laneListForward.clear(); startPosition = Length.ZERO; } while (distanceToEndOfLane.lt(maxHeadway)) { Map lanes = lastGtuDir.equals(GTUDirectionality.DIR_PLUS) ? lane.nextLanes(gtu.getGTUType()) : lane.prevLanes(gtu.getGTUType()); if (lanes.size() == 0) { // Dead end. Return with the list as is. return new LanePathInfo(path, laneListForward, startPosition); } else if (lanes.size() == 1) { // Ask the strategical planner what the next link should be (if known), because the strategical planner knows // best! LinkDirection ld = null; ld = gtu.getStrategicalPlanner().nextLinkDirection(lane.getParentLink(), lastGtuDir, gtu.getGTUType()); lane = lanes.keySet().iterator().next(); if (ld != null && !lane.getParentLink().equals(ld.getLink())) { // Lane not on route anymore. return with the list as is. return new LanePathInfo(path, laneListForward, startPosition); } } else { // Multiple next lanes; ask the strategical planner where to go. // Note: this is not necessarily a split; it could e.g. be a bike path on a road LinkDirection ld; try { ld = gtu.getStrategicalPlanner().nextLinkDirection(lane.getParentLink(), /* gtu.getLanes().get(lane), */ lastGtuDir, gtu.getGTUType()); } catch (NetworkException ne) { // no route found. // return the data structure up to this point... return new LanePathInfo(path, laneListForward, startPosition); } Link nextLink = ld.getLink(); Lane newLane = null; for (Lane nextLane : lanes.keySet()) { if (nextLane.getParentLink().equals(nextLink)) { newLane = nextLane; break; } } if (newLane == null) { // we cannot reach the next node on this lane -- we have to make a lane change! // return the data structure up to this point... return new LanePathInfo(path, laneListForward, startPosition); } // otherwise: continue! lane = newLane; } // determine direction for the path try { if (lastGtuDir.equals(GTUDirectionality.DIR_PLUS)) { if (lastLane.getParentLink().getEndNode().equals(lane.getParentLink().getStartNode())) { // -----> O ----->, GTU moves ----> path = concatenateNull(path, lane.getCenterLine()); // path = OTSLine3D.concatenate(Lane.MARGIN.si, path, lane.getCenterLine()); lastGtuDir = GTUDirectionality.DIR_PLUS; } else { // -----> O <-----, GTU moves ----> path = concatenateNull(path, lane.getCenterLine().reverse()); // path = OTSLine3D.concatenate(Lane.MARGIN.si, path, lane.getCenterLine().reverse()); lastGtuDir = GTUDirectionality.DIR_MINUS; } } else { if (lastLane.getParentLink().getStartNode().equals(lane.getParentLink().getStartNode())) { // <----- O ----->, GTU moves ----> path = concatenateNull(path, lane.getCenterLine()); // path = OTSLine3D.concatenate(Lane.MARGIN.si, path, lane.getCenterLine()); lastGtuDir = GTUDirectionality.DIR_PLUS; } else { // <----- O <-----, GTU moves ----> path = concatenateNull(path, lane.getCenterLine().reverse()); // path = OTSLine3D.concatenate(Lane.MARGIN.si, path, lane.getCenterLine().reverse()); lastGtuDir = GTUDirectionality.DIR_MINUS; } } lastLane = lane; } catch (OTSGeometryException exception) { throw new GTUException(exception); } laneListForward.add(new LaneDirection(lastLane, lastGtuDir)); distanceToEndOfLane = distanceToEndOfLane.plus(lastLane.getLength()); } return new LanePathInfo(path, laneListForward, startPosition); } /** * Concatenate two paths, where the first may be {@code null}. * @param path OTSLine3D; path, may be {@code null} * @param centerLine OTSLine3D; center line of lane to add * @return concatenated line * @throws OTSGeometryException when lines are degenerate or too distant */ public static OTSLine3D concatenateNull(final OTSLine3D path, final OTSLine3D centerLine) throws OTSGeometryException { if (path == null) { return centerLine; } return OTSLine3D.concatenate(Lane.MARGIN.si, path, centerLine); } /** * Calculate the next location where the network splits, with a maximum headway relative to the reference point of the GTU. * Note: a lane drop is also considered a split (!). * @param gtu LaneBasedGTU; the GTU for which to calculate the lane list * @param maxHeadway Length; the maximum length for which lanes should be returned * @return NextSplitInfo; an instance that provides the following information for an operational plan: whether the network * splits, the node where it splits, and the current lanes that lead to the right node after the split node. * @throws GTUException when the vehicle is not on one of the lanes on which it is registered * @throws NetworkException when the strategic planner is not able to return a next node in the route */ public static NextSplitInfo determineNextSplit(final LaneBasedGTU gtu, final Length maxHeadway) throws GTUException, NetworkException { Node nextSplitNode = null; Set correctCurrentLanes = new LinkedHashSet<>(); DirectedLanePosition dlp = gtu.getReferencePosition(); Lane referenceLane = dlp.getLane(); double refFrac = dlp.getPosition().si / referenceLane.getLength().si; Link lastLink = referenceLane.getParentLink(); GTUDirectionality lastGtuDir = dlp.getGtuDirection(); GTUDirectionality referenceLaneDirectionality = lastGtuDir; Length lengthForward; Length position = dlp.getPosition(); Node lastNode; if (lastGtuDir.equals(GTUDirectionality.DIR_PLUS)) { lengthForward = referenceLane.getLength().minus(position); lastNode = referenceLane.getParentLink().getEndNode(); } else { lengthForward = gtu.position(referenceLane, gtu.getReference()); lastNode = referenceLane.getParentLink().getStartNode(); } // see if we have a split within maxHeadway distance while (lengthForward.lt(maxHeadway) && nextSplitNode == null) { // calculate the number of "outgoing" links Set links = lastNode.getLinks().toSet(); // safe copy Iterator linkIterator = links.iterator(); while (linkIterator.hasNext()) { Link link = linkIterator.next(); GTUDirectionality drivingDirection = lastNode.equals(link.getStartNode()) ? GTUDirectionality.DIR_PLUS : GTUDirectionality.DIR_MINUS; if (!link.getDirectionality(gtu.getGTUType()).getDirectionalities().contains(drivingDirection)) { linkIterator.remove(); } // if (link.equals(lastLink) || !link.getLinkType().isCompatible(gtu.getGTUType()) // || (link.getDirectionality(gtu.getGTUType()).isForward() && link.getEndNode().equals(lastNode)) // || (link.getDirectionality(gtu.getGTUType()).isBackward() && link.getStartNode().equals(lastNode))) // { // linkIterator.remove(); // } } // calculate the number of incoming and outgoing lanes on the link boolean laneChange = false; if (links.size() == 1) { for (CrossSectionElement cse : ((CrossSectionLink) lastLink).getCrossSectionElementList()) { if (cse instanceof Lane && lastGtuDir.isPlus()) { Lane lane = (Lane) cse; if (lane.nextLanes(gtu.getGTUType()).size() == 0) { laneChange = true; } } if (cse instanceof Lane && lastGtuDir.isMinus()) { Lane lane = (Lane) cse; if (lane.prevLanes(gtu.getGTUType()).size() == 0) { laneChange = true; } } } } // see if we have a lane drop if (laneChange) { nextSplitNode = lastNode; // which lane(s) we are registered on and adjacent lanes link to a lane // that does not drop? for (CrossSectionElement cse : referenceLane.getParentLink().getCrossSectionElementList()) { if (cse instanceof Lane) { Lane l = (Lane) cse; // if (noLaneDrop(gtu, maxHeadway, l, position, referenceLaneDirectionality)) if (noLaneDrop(gtu, maxHeadway, l, l.getLength().times(refFrac), referenceLaneDirectionality)) { correctCurrentLanes.add(l); } } } return new NextSplitInfo(nextSplitNode, correctCurrentLanes); } // see if we have a split if (links.size() > 1) { nextSplitNode = lastNode; LinkDirection ld = gtu.getStrategicalPlanner().nextLinkDirection(nextSplitNode, lastLink, gtu.getGTUType()); // which lane(s) we are registered on and adjacent lanes link to a lane // that is on the route at the next split? for (CrossSectionElement cse : referenceLane.getParentLink().getCrossSectionElementList()) { if (cse instanceof Lane) { Lane l = (Lane) cse; // if (connectsToPath(gtu, maxHeadway, l, position, referenceLaneDirectionality, ld.getLink())) if (connectsToPath(gtu, maxHeadway, l, l.getLength().times(refFrac), referenceLaneDirectionality, ld.getLink())) { correctCurrentLanes.add(l); } } } if (correctCurrentLanes.size() > 0) { return new NextSplitInfo(nextSplitNode, correctCurrentLanes); } // split, but no lane on current link to right direction Set correctLanes = new LinkedHashSet<>(); Set wrongLanes = new LinkedHashSet<>(); for (CrossSectionElement cse : ((CrossSectionLink) lastLink).getCrossSectionElementList()) { if (cse instanceof Lane) { Lane l = (Lane) cse; if (connectsToPath(gtu, maxHeadway.plus(l.getLength()), l, Length.ZERO, lastGtuDir, ld.getLink())) { correctLanes.add(l); } else { wrongLanes.add(l); } } } for (Lane wrongLane : wrongLanes) { for (Lane adjLane : wrongLane.accessibleAdjacentLanesLegal(LateralDirectionality.LEFT, gtu.getGTUType(), referenceLaneDirectionality)) { if (correctLanes.contains(adjLane)) { return new NextSplitInfo(nextSplitNode, correctCurrentLanes, LateralDirectionality.LEFT); } } for (Lane adjLane : wrongLane.accessibleAdjacentLanesLegal(LateralDirectionality.RIGHT, gtu.getGTUType(), referenceLaneDirectionality)) { if (correctLanes.contains(adjLane)) { return new NextSplitInfo(nextSplitNode, correctCurrentLanes, LateralDirectionality.RIGHT); } } } return new NextSplitInfo(nextSplitNode, correctCurrentLanes, null); } if (links.size() == 0) { return new NextSplitInfo(null, correctCurrentLanes); } // just one link Link link = links.iterator().next(); // determine direction for the path if (lastGtuDir.equals(GTUDirectionality.DIR_PLUS)) { if (lastLink.getEndNode().equals(link.getStartNode())) { // -----> O ----->, GTU moves ----> lastGtuDir = GTUDirectionality.DIR_PLUS; lastNode = link.getEndNode(); } else { // -----> O <-----, GTU moves ----> lastGtuDir = GTUDirectionality.DIR_MINUS; lastNode = link.getEndNode(); } } else { if (lastLink.getStartNode().equals(link.getStartNode())) { // <----- O ----->, GTU moves ----> lastNode = link.getStartNode(); lastGtuDir = GTUDirectionality.DIR_PLUS; } else { // <----- O <-----, GTU moves ----> lastNode = link.getStartNode(); lastGtuDir = GTUDirectionality.DIR_MINUS; } } lastLink = links.iterator().next(); lengthForward = lengthForward.plus(lastLink.getLength()); } return new NextSplitInfo(null, correctCurrentLanes); } /** * Determine whether the lane is directly connected to our route, in other words: if we would (continue to) drive on the * given lane, can we take the right branch at the nextSplitNode without switching lanes? * @param gtu LaneBasedGTU; the GTU for which we have to determine the lane suitability * @param maxHeadway Length; the maximum length for use in the calculation * @param startLane Lane; the first lane in the list * @param startLanePosition Length; the position on the start lane * @param startDirectionality GTUDirectionality; the driving direction on the start lane * @param linkAfterSplit Link; the link after the split to which we should connect * @return boolean; true if the lane (XXXXX which lane?) is connected to our path * @throws GTUException when the vehicle is not on one of the lanes on which it is registered * @throws NetworkException when the strategic planner is not able to return a next node in the route */ protected static boolean connectsToPath(final LaneBasedGTU gtu, final Length maxHeadway, final Lane startLane, final Length startLanePosition, final GTUDirectionality startDirectionality, final Link linkAfterSplit) throws GTUException, NetworkException { List laneDirections = buildLanePathInfo(gtu, maxHeadway, startLane, startLanePosition, startDirectionality).getLaneDirectionList(); for (LaneDirection laneDirection : laneDirections) { if (laneDirection.getLane().getParentLink().equals(linkAfterSplit)) { return true; } } return false; } /** * Determine whether the lane does not drop, in other words: if we would (continue to) drive on the given lane, can we * continue to drive at the nextSplitNode without switching lanes? * @param gtu LaneBasedGTU; the GTU for which we have to determine the lane suitability * @param maxHeadway Length; the maximum length for use in the calculation * @param startLane Lane; the first lane in the list * @param startLanePosition Length; the position on the start lane * @param startDirectionality GTUDirectionality; the driving direction on the start lane * @return boolean; true if the lane (XXX which lane?) is connected to our path * @throws GTUException when the vehicle is not on one of the lanes on which it is registered * @throws NetworkException when the strategic planner is not able to return a next node in the route */ protected static boolean noLaneDrop(final LaneBasedGTU gtu, final Length maxHeadway, final Lane startLane, final Length startLanePosition, final GTUDirectionality startDirectionality) throws GTUException, NetworkException { LanePathInfo lpi = buildLanePathInfo(gtu, maxHeadway, startLane, startLanePosition, startDirectionality); if (lpi.getPath().getLength().lt(maxHeadway)) { return false; } return true; } /** * Make a list of links on which to drive next, with a maximum headway relative to the reference point of the GTU. * @param gtu LaneBasedGTU; the GTU for which to calculate the link list * @param maxHeadway Length; the maximum length for which links should be returned * @return List<LinkDirection>; a list of links on which to drive next * @throws GTUException when the vehicle is not on one of the lanes on which it is registered * @throws NetworkException when the strategic planner is not able to return a next node in the route */ protected static List buildLinkListForward(final LaneBasedGTU gtu, final Length maxHeadway) throws GTUException, NetworkException { List linkList = new ArrayList<>(); DirectedLanePosition dlp = gtu.getReferencePosition(); Lane referenceLane = dlp.getLane(); Link lastLink = referenceLane.getParentLink(); GTUDirectionality lastGtuDir = dlp.getGtuDirection(); linkList.add(new LinkDirection(lastLink, lastGtuDir)); Length lengthForward; Length position = dlp.getPosition(); Node lastNode; if (lastGtuDir.equals(GTUDirectionality.DIR_PLUS)) { lengthForward = referenceLane.getLength().minus(position); lastNode = referenceLane.getParentLink().getEndNode(); } else { lengthForward = gtu.position(referenceLane, gtu.getReference()); lastNode = referenceLane.getParentLink().getStartNode(); } // see if we have a split within maxHeadway distance while (lengthForward.lt(maxHeadway)) { // calculate the number of "outgoing" links Set links = lastNode.getLinks().toSet(); // is a safe copy Iterator linkIterator = links.iterator(); while (linkIterator.hasNext()) { Link link = linkIterator.next(); GTUDirectionality drivingDirection = lastNode.equals(link.getStartNode()) ? GTUDirectionality.DIR_PLUS : GTUDirectionality.DIR_MINUS; if (link.equals(lastLink) || !link.getLinkType().isCompatible(gtu.getGTUType(), drivingDirection)) { linkIterator.remove(); } } if (links.size() == 0) { return linkList; // the path stops here... } Link link; if (links.size() > 1) { LinkDirection ld = gtu.getStrategicalPlanner().nextLinkDirection(lastLink, lastGtuDir, gtu.getGTUType()); link = ld.getLink(); } else { link = links.iterator().next(); } // determine direction for the path if (lastGtuDir.equals(GTUDirectionality.DIR_PLUS)) { if (lastLink.getEndNode().equals(link.getStartNode())) { // -----> O ----->, GTU moves ----> lastGtuDir = GTUDirectionality.DIR_PLUS; lastNode = lastLink.getEndNode(); } else { // -----> O <-----, GTU moves ----> lastGtuDir = GTUDirectionality.DIR_MINUS; lastNode = lastLink.getEndNode(); } } else { if (lastLink.getStartNode().equals(link.getStartNode())) { // <----- O ----->, GTU moves ----> lastNode = lastLink.getStartNode(); lastGtuDir = GTUDirectionality.DIR_PLUS; } else { // <----- O <-----, GTU moves ----> lastNode = lastLink.getStartNode(); lastGtuDir = GTUDirectionality.DIR_MINUS; } } lastLink = link; linkList.add(new LinkDirection(lastLink, lastGtuDir)); lengthForward = lengthForward.plus(lastLink.getLength()); } return linkList; } /** {@inheritDoc} */ @Override public final CarFollowingModel getCarFollowingModel() { return this.carFollowingModel; } /** * Sets the car-following model. * @param carFollowingModel CarFollowingModel; Car-following model to set. */ public final void setCarFollowingModel(final CarFollowingModel carFollowingModel) { this.carFollowingModel = carFollowingModel; } /** {@inheritDoc} */ @Override public final LanePerception getPerception() { return this.lanePerception; } }