CompleteLaneBasedRouteNavigator.java
package org.opentrafficsim.core.network.route;
import java.util.HashMap;
import java.util.Map;
import org.djunits.unit.LengthUnit;
import org.djunits.unit.TimeUnit;
import org.opentrafficsim.core.gtu.GTUType;
import org.opentrafficsim.core.network.LateralDirectionality;
import org.opentrafficsim.core.network.Link;
import org.opentrafficsim.core.network.NetworkException;
import org.opentrafficsim.core.network.Node;
import org.opentrafficsim.core.network.lane.CrossSectionElement;
import org.opentrafficsim.core.network.lane.CrossSectionLink;
import org.opentrafficsim.core.network.lane.Lane;
/**
* A RouteNavigator helps to navigate on a route. In addition, helper methods are available to see if the GTU needs to change
* lanes to reach the next link on the route.
* <p>
* Copyright (c) 2013-2015 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved. <br>
* BSD-style license. See <a href="http://opentrafficsim.org/docs/license.html">OpenTrafficSim License</a>.
* <p>
* $LastChangedDate: 2015-07-16 10:20:53 +0200 (Thu, 16 Jul 2015) $, @version $Revision: 1124 $, by $Author: pknoppers $,
* initial version Jul 22, 2015 <br>
* @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
* @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
*/
public class CompleteLaneBasedRouteNavigator extends AbstractLaneBasedRouteNavigator
{
/** */
private static final long serialVersionUID = 20150722L;
/** The complete route. */
@SuppressWarnings("checkstyle:visibilitymodifier")
protected final CompleteRoute completeRoute;
/** last visited node on the route. */
@SuppressWarnings("checkstyle:visibilitymodifier")
protected int lastVisitedNodeIndex = -1;
/**
* Create a navigator.
* @param completeRoute the route to follow
*/
public CompleteLaneBasedRouteNavigator(final CompleteRoute completeRoute)
{
this.completeRoute = completeRoute;
}
/** {@inheritDoc} */
@Override
public final Node lastVisitedNode() throws NetworkException
{
if (this.lastVisitedNodeIndex == -1)
{
return null;
}
return this.completeRoute.getNodes().get(this.lastVisitedNodeIndex);
}
/** {@inheritDoc} */
@Override
public final Node nextNodeToVisit() throws NetworkException
{
if (this.lastVisitedNodeIndex >= this.completeRoute.size() - 1)
{
return null;
}
return this.completeRoute.getNodes().get(this.lastVisitedNodeIndex + 1);
}
/** {@inheritDoc} */
@Override
public final Node visitNextNode() throws NetworkException
{
if (this.lastVisitedNodeIndex >= this.completeRoute.size() - 1)
{
return null;
}
this.lastVisitedNodeIndex++;
return this.completeRoute.getNodes().get(this.lastVisitedNodeIndex);
}
/** {@inheritDoc} */
@Override
public final Length.Rel suitability(final Lane lane, final Length.Rel longitudinalPosition, final GTUType gtuType,
final Time.Rel timeHorizon) throws NetworkException
{
double remainingDistance = lane.getLength().getSI() - longitudinalPosition.getSI();
double spareTime = timeHorizon.getSI() - remainingDistance / lane.getSpeedLimit(gtuType).getSI();
// Find the first upcoming Node where there is a branch
Node nextNode = lane.getParentLink().getEndNode();
Node nextSplitNode = null;
Lane currentLane = lane;
CrossSectionLink linkBeforeBranch = lane.getParentLink();
while (null != nextNode)
{
if (spareTime <= 0)
{
return NOLANECHANGENEEDED; // It is not yet time to worry; this lane will do as well as any other
}
int laneCount = countCompatibleLanes(linkBeforeBranch, gtuType);
if (0 == laneCount)
{
throw new NetworkException("No compatible Lanes on Link " + linkBeforeBranch);
}
if (1 == laneCount)
{
return NOLANECHANGENEEDED; // Only one compatible lane available; we'll get there "automatically";
// i.e. without influence from the Route
}
int branching = nextNode.getLinksOut().size();
if (branching > 1)
{ // Found a split
nextSplitNode = nextNode;
break;
}
else if (0 == branching)
{
return NOLANECHANGENEEDED; // dead end; no more choices to make
}
else
{ // Look beyond this nextNode
Link nextLink = nextNode.getLinksOut().iterator().next(); // cannot be null
if (nextLink instanceof CrossSectionLink)
{
nextNode = nextLink.getEndNode();
// Oops: wrong code added the length of linkBeforeBranch in stead of length of nextLink
remainingDistance += nextLink.getLength().getSI();
linkBeforeBranch = (CrossSectionLink) nextLink;
// Figure out the new currentLane
if (currentLane.nextLanes(gtuType).size() == 0)
{
// Lane drop; our lane disappears. This is a compulsory lane change; which is not controlled
// by the Route. Perform the forced lane change.
if (currentLane.accessibleAdjacentLanes(LateralDirectionality.RIGHT, gtuType).size() > 0)
{
for (Lane adjacentLane : currentLane.accessibleAdjacentLanes(LateralDirectionality.RIGHT,
gtuType))
{
if (adjacentLane.nextLanes(gtuType).size() > 0)
{
currentLane = adjacentLane;
break;
}
// If there are several adjacent lanes that have non empty nextLanes, we simple take the
// first in the set
}
}
for (Lane adjacentLane : currentLane.accessibleAdjacentLanes(LateralDirectionality.LEFT, gtuType))
{
if (adjacentLane.nextLanes(gtuType).size() > 0)
{
currentLane = adjacentLane;
break;
}
// If there are several adjacent lanes that have non empty nextLanes, we simple take the
// first in the set
}
if (currentLane.nextLanes(gtuType).size() == 0)
{
throw new NetworkException("Lane ends and there is not a compatible adjacent lane that does "
+ "not end");
}
}
// Any compulsory lane change(s) have been performed and there is guaranteed a compatible next lane.
for (Lane nextLane : currentLane.nextLanes(gtuType))
{
if (nextLane.getLaneType().isCompatible(gtuType))
{
currentLane = currentLane.nextLanes(gtuType).iterator().next();
break;
}
}
spareTime -= currentLane.getLength().getSI() / currentLane.getSpeedLimit(gtuType).getSI();
}
else
{
// There is a non-CrossSectionLink on the path to the next branch. A non-CrossSectionLink does not
// have identifiable Lanes, therefore we can't aim for a particular Lane
return NOLANECHANGENEEDED; // Any Lane will do equally well
}
}
}
if (null == nextNode)
{
throw new NetworkException("Cannot find the next branch or sink node");
}
// We have now found the first upcoming branching Node
// Which continuing link is the one we need?
Map<Lane, Length.Rel> suitabilityOfLanesBeforeBranch = new HashMap<Lane, Length.Rel>();
Link linkAfterBranch = null;
for (Link link : nextSplitNode.getLinksOut())
{
Node nextNodeOnLink = link.getEndNode();
for (int i = this.lastVisitedNodeIndex + 1; i < this.completeRoute.size(); i++)
{
Node n = this.completeRoute.getNodes().get(i);
if (nextNodeOnLink == n)
{
if (null != linkAfterBranch)
{
throw new NetworkException("Parallel Links at " + nextSplitNode + " go to " + nextNodeOnLink);
// FIXME If all but one of these have no Lane compatible with gtuType, dying here is a bit
// premature
}
linkAfterBranch = link;
break;
}
}
}
if (null == linkAfterBranch)
{
throw new NetworkException("Cannot identify the link to follow after " + nextSplitNode + " in " + this);
}
for (CrossSectionElement cse : linkBeforeBranch.getCrossSectionElementList())
{
if (cse instanceof Lane)
{
Lane l = (Lane) cse;
if (l.getLaneType().isCompatible(gtuType))
{
for (Lane connectingLane : l.nextLanes(gtuType))
{
if (connectingLane.getParentLink() == linkAfterBranch
&& connectingLane.getLaneType().isCompatible(gtuType))
{
Length.Rel currentValue = suitabilityOfLanesBeforeBranch.get(l);
// Use recursion to find out HOW suitable this continuation lane is, but don't revert back
// to the maximum time horizon (or we could end up in infinite recursion when there are
// loops in the network).
Length.Rel value =
suitability(connectingLane, new Length.Rel(0, LengthUnit.SI), gtuType, new Time.Rel(
spareTime, TimeUnit.SI));
// Use the minimum of the value computed for the first split junction (if there is one)
// and the value computed for the second split junction.
suitabilityOfLanesBeforeBranch.put(l, null == currentValue || value.le(currentValue) ? value
: currentValue);
}
}
}
}
}
if (suitabilityOfLanesBeforeBranch.size() == 0)
{
throw new NetworkException("No lanes available on Link " + linkBeforeBranch);
}
Length.Rel currentLaneSuitability = suitabilityOfLanesBeforeBranch.get(currentLane);
if (null != currentLaneSuitability)
{
return currentLaneSuitability; // Following the current lane will keep us on the Route
}
// Performing one or more lane changes (left or right) is required.
int totalLanes = countCompatibleLanes(currentLane.getParentLink(), gtuType);
Length.Rel leftSuitability =
computeSuitabilityWithLaneChanges(currentLane, remainingDistance, suitabilityOfLanesBeforeBranch, totalLanes,
LateralDirectionality.LEFT, gtuType);
Length.Rel rightSuitability =
computeSuitabilityWithLaneChanges(currentLane, remainingDistance, suitabilityOfLanesBeforeBranch, totalLanes,
LateralDirectionality.RIGHT, gtuType);
if (leftSuitability.ge(rightSuitability))
{
return leftSuitability;
}
else if (rightSuitability.ge(leftSuitability))
{
return rightSuitability;
}
if (leftSuitability.le(GETOFFTHISLANENOW))
{
throw new NetworkException("Changing lanes in any direction does not get the GTU on a suitable lane");
}
return leftSuitability; // left equals right; this is odd but topologically possible
}
/**
* @return the (complete) route.
*/
public final CompleteRoute getRoute()
{
return this.completeRoute;
}
/** {@inheritDoc} */
@SuppressWarnings("checkstyle:designforextension")
@Override
public String toString()
{
StringBuilder result = new StringBuilder();
final String currentLocationMark = "<>";
result.append("Route: [");
String separator = "";
if (this.lastVisitedNodeIndex < 0)
{
result.append(currentLocationMark);
}
for (int index = 0; index < this.completeRoute.size(); index++)
{
Node node = this.completeRoute.getNodes().get(index);
result.append(separator + node);
if (index == this.lastVisitedNodeIndex)
{
result.append(" " + currentLocationMark); // Indicate current position in the route
}
separator = ", ";
}
result.append("]");
return result.toString();
}
}