View Javadoc
1   package org.opentrafficsim.road.gtu.strategical;
2   
3   import java.io.Serializable;
4   import java.util.ArrayList;
5   import java.util.Iterator;
6   import java.util.List;
7   import java.util.Set;
8   
9   import org.djunits.value.vdouble.scalar.Time;
10  import org.djutils.exceptions.Throw;
11  import org.djutils.exceptions.Try;
12  import org.opentrafficsim.core.gtu.GtuException;
13  import org.opentrafficsim.core.gtu.GtuType;
14  import org.opentrafficsim.core.network.Link;
15  import org.opentrafficsim.core.network.NetworkException;
16  import org.opentrafficsim.core.network.Node;
17  import org.opentrafficsim.core.network.route.Route;
18  import org.opentrafficsim.road.gtu.lane.LaneBasedGtu;
19  import org.opentrafficsim.road.gtu.lane.tactical.LaneBasedTacticalPlanner;
20  import org.opentrafficsim.road.network.lane.CrossSectionLink;
21  import org.opentrafficsim.road.network.lane.Lane;
22  import org.opentrafficsim.road.network.lane.LanePosition;
23  
24  /**
25   * This is the standard strategical route planner with a fixed tactical planner. If no route is supplied, but there is a
26   * destination, a route will be drawn from a {@code RouteGenerator}, or using length-based shortest path if that is also not
27   * specified. If the route is ever {@code null}, a route will be constructed from the origin to the current link, and from the
28   * current link to the destination.
29   * <p>
30   * Copyright (c) 2013-2024 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved. <br>
31   * BSD-style license. See <a href="https://opentrafficsim.org/docs/license.html">OpenTrafficSim License</a>.
32   * </p>
33   * @author <a href="https://github.com/averbraeck">Alexander Verbraeck</a>
34   * @author <a href="https://github.com/peter-knoppers">Peter Knoppers</a>
35   */
36  public class LaneBasedStrategicalRoutePlanner implements LaneBasedStrategicalPlanner, Serializable
37  {
38      /** */
39      private static final long serialVersionUID = 20151126L;
40  
41      /** GTU. */
42      private final LaneBasedGtu gtu;
43  
44      /** The route to drive. */
45      private Route route;
46  
47      /** Origin node. */
48      private final Node origin;
49  
50      /** Destination node. */
51      private Node destination;
52  
53      /** The fixed tactical planner to use for the GTU. */
54      private final LaneBasedTacticalPlanner fixedTacticalPlanner;
55  
56      /** Route supplier. */
57      private final RouteGenerator routeGenerator;
58  
59      /**
60       * Constructor for a strategical planner without route. This can only be used if the network does not have splits, or split
61       * fractions are used.
62       * @param fixedTacticalPlanner the tactical planner to use for the GTU
63       * @param gtu GTU
64       * @throws GtuException if fixed tactical planner == null
65       */
66      public LaneBasedStrategicalRoutePlanner(final LaneBasedTacticalPlanner fixedTacticalPlanner, final LaneBasedGtu gtu)
67              throws GtuException
68      {
69          this(fixedTacticalPlanner, null, gtu, null, null, RouteGenerator.NULL);
70      }
71  
72      /**
73       * Constructor for a strategical planner with route. If the route is {@code null}, a shortest path to the destination is
74       * derived.
75       * @param fixedTacticalPlanner the tactical planner to use for the GTU
76       * @param route the route to drive
77       * @param gtu GTU
78       * @param origin origin node
79       * @param destination destination node
80       * @param routeGenerator route generator
81       * @throws GtuException if fixed tactical planner == null
82       */
83      public LaneBasedStrategicalRoutePlanner(final LaneBasedTacticalPlanner fixedTacticalPlanner, final Route route,
84              final LaneBasedGtu gtu, final Node origin, final Node destination, final RouteGenerator routeGenerator)
85              throws GtuException
86      {
87          this.gtu = gtu;
88          this.route = route;
89          this.origin = origin;
90          this.destination = destination;
91          this.fixedTacticalPlanner = fixedTacticalPlanner;
92          Throw.when(fixedTacticalPlanner == null, GtuException.class,
93                  "Fixed Tactical Planner for a Strategical planner is null");
94          this.routeGenerator = routeGenerator;
95      }
96  
97      @Override
98      public final LaneBasedGtu getGtu()
99      {
100         return this.gtu;
101     }
102 
103     @Override
104     public final LaneBasedTacticalPlanner getTacticalPlanner()
105     {
106         return this.fixedTacticalPlanner;
107     }
108 
109     @Override
110     public LaneBasedTacticalPlanner getTacticalPlanner(final Time time)
111     {
112         return this.fixedTacticalPlanner; // fixed anyway
113     }
114 
115     @Override
116     public final Link nextLink(final Link previousLink, final GtuType gtuType) throws NetworkException
117     {
118         assureRoute(gtuType);
119 
120         Node node = previousLink.getEndNode();
121         // if there is no split, don't ask the route
122         if (node.getLinks().size() == 1 && previousLink != null)
123         {
124             // end node
125             throw new NetworkException(
126                     "LaneBasedStrategicalRoutePlanner is asked for a next link, but node " + node + " has no successors");
127         }
128         if (node.getLinks().size() == 1 && previousLink == null)
129         {
130             // start node
131             return node.getLinks().iterator().next();
132         }
133         if (node.getLinks().size() == 2)
134         {
135             for (Link link : node.getLinks())
136             {
137                 if (!link.equals(previousLink))
138                 {
139                     return link;
140                 }
141             }
142         }
143 
144         // if we only have one way to go, don't bother about the route yet
145         Set<Link> links = node.getLinks().toSet();
146         for (Iterator<Link> linkIterator = links.iterator(); linkIterator.hasNext();)
147         {
148             Link link = linkIterator.next();
149             if (link.equals(previousLink))
150             {
151                 // No u-turn...
152                 linkIterator.remove();
153             }
154             else
155             {
156                 // does the directionality of the link forbid us to go in?
157                 if (link.getEndNode().equals(node))
158                 {
159                     linkIterator.remove();
160                 }
161                 else
162                 {
163                     // are there no lanes from the node into this link in the outgoing direction?
164                     boolean out = false;
165                     CrossSectionLink csLink = (CrossSectionLink) link;
166                     for (Lane lane : csLink.getLanes())
167                     {
168                         if ((link.getStartNode().equals(node) && lane.getType().isCompatible(gtuType)))
169                         {
170                             out = true;
171                             break;
172                         }
173                     }
174                     if (!out)
175                     {
176                         linkIterator.remove();
177                     }
178                 }
179             }
180         }
181 
182         if (links.size() == 1)
183         {
184             return links.iterator().next();
185         }
186 
187         // more than 2 links... We have to check the route!
188         if (getRoute() == null)
189         {
190             throw new NetworkException("LaneBasedStrategicalRoutePlanner does not have a route");
191         }
192         int i = this.route.getNodes().indexOf(node);
193         if (i == -1)
194         {
195             throw new NetworkException("LaneBasedStrategicalRoutePlanner is asked for a next link coming from " + previousLink
196                     + ", but node " + node + " not in route " + this.route);
197         }
198         if (i == this.route.getNodes().size() - 1)
199         {
200             throw new NetworkException("LaneBasedStrategicalRoutePlanner is asked for a next link coming from " + previousLink
201                     + ", but the GTU reached the last node for route " + this.route);
202         }
203         Node nextNode = this.route.getNode(i + 1);
204         Link result = null;
205         for (Link link : links)
206         {
207             // TODO this takes the first in the set of links that connects the correct nodes; does not handle parallel links
208             // consistently
209             Link l = null;
210             if (link.getStartNode().equals(nextNode) && link.getEndNode().equals(node))
211             {
212                 l = link; // FIXME: Probably this test can be removed since we only go "forward"
213             }
214             if (link.getEndNode().equals(nextNode) && link.getStartNode().equals(node))
215             {
216                 l = link;
217             }
218             if (null != result && null != l)
219             {
220                 throw new NetworkException("Cannot choose among multiple links from " + node + " to " + nextNode);
221             }
222             else if (null == result)
223             {
224                 result = l;
225             }
226         }
227         if (null == result)
228         {
229             throw new NetworkException("LaneBasedStrategicalRoutePlanner is asked for a next link coming from "
230                     + previousLink.getId() + ", but no link could be found connecting node " + node + " and node " + nextNode
231                     + " for route " + this.route);
232         }
233         return result;
234     }
235 
236     @Override
237     public final Route getRoute()
238     {
239         assureRoute(getGtu().getType());
240         // if assure route left the route null although there is a destination, we have no route generator, use shortest-path
241         if (this.route == null && this.destination != null)
242         {
243             try
244             {
245                 LanePosition pos = getGtu().getReferencePosition();
246                 CrossSectionLink link = pos.lane().getLink();
247                 Node from = link.getStartNode();
248                 this.route = link.getNetwork().getShortestRouteBetween(getGtu().getType(), from, this.destination);
249             }
250             catch (GtuException | NetworkException exception)
251             {
252                 throw new RuntimeException("Route could not be determined.", exception);
253             }
254         }
255         return this.route;
256     }
257 
258     /**
259      * Assures a route is available if a route is already present, or a destination and route supplier are provided.
260      * @param gtuType the type of the GTU for which a route must be assured
261      */
262     private void assureRoute(final GtuType gtuType)
263     {
264         if (this.route == null && this.destination != null && !this.routeGenerator.equals(RouteGenerator.NULL))
265         {
266             LanePosition ref = Try.assign(() -> getGtu().getReferencePosition(), "Could not retrieve GTU reference position.");
267             List<Node> nodes = new ArrayList<>();
268             if (this.origin != null)
269             {
270                 nodes.addAll(
271                         this.routeGenerator.getRoute(this.origin, ref.lane().getLink().getStartNode(), gtuType).getNodes());
272             }
273             else
274             {
275                 nodes.add(ref.lane().getLink().getStartNode());
276             }
277             Route newRoute = this.routeGenerator.getRoute(ref.lane().getLink().getEndNode(), this.destination, gtuType);
278             nodes.addAll(newRoute.getNodes());
279             this.route = Try.assign(() -> new Route(
280                     "Route for " + gtuType + " from " + this.origin + "to " + this.destination + " via " + ref.lane().getLink(),
281                     gtuType, nodes), "No route possible over nodes %s", nodes);
282         }
283     }
284 
285     @Override
286     public final Node getOrigin()
287     {
288         return this.origin;
289     }
290 
291     @Override
292     public final Node getDestination()
293     {
294         return this.destination;
295     }
296 
297     @Override
298     public final String toString()
299     {
300         return "LaneBasedStrategicalRoutePlanner [route=" + this.route + ", fixedTacticalPlanner=" + this.fixedTacticalPlanner
301                 + "]";
302     }
303 
304 }