1 package org.opentrafficsim.road.gtu.strategical.route;
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.GTUDirectionality;
13 import org.opentrafficsim.core.gtu.GTUException;
14 import org.opentrafficsim.core.gtu.GTUType;
15 import org.opentrafficsim.core.network.Link;
16 import org.opentrafficsim.core.network.LinkDirection;
17 import org.opentrafficsim.core.network.NetworkException;
18 import org.opentrafficsim.core.network.Node;
19 import org.opentrafficsim.core.network.route.CompleteRoute;
20 import org.opentrafficsim.core.network.route.Route;
21 import org.opentrafficsim.road.gtu.lane.LaneBasedGTU;
22 import org.opentrafficsim.road.gtu.lane.tactical.LaneBasedTacticalPlanner;
23 import org.opentrafficsim.road.gtu.strategical.AbstractLaneBasedStrategicalPlanner;
24 import org.opentrafficsim.road.gtu.strategical.LaneBasedStrategicalPlanner;
25 import org.opentrafficsim.road.network.lane.CrossSectionElement;
26 import org.opentrafficsim.road.network.lane.CrossSectionLink;
27 import org.opentrafficsim.road.network.lane.DirectedLanePosition;
28 import org.opentrafficsim.road.network.lane.Lane;
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44 public class LaneBasedStrategicalRoutePlanner extends AbstractLaneBasedStrategicalPlanner implements
45 LaneBasedStrategicalPlanner, Serializable
46 {
47
48 private static final long serialVersionUID = 20150724L;
49
50
51 private Route route;
52
53
54 private final Node origin;
55
56
57 private final Node destination;
58
59
60 private final LaneBasedTacticalPlanner fixedTacticalPlanner;
61
62
63 private final RouteGeneratorOD routeGenerator;
64
65
66
67
68
69
70
71
72 public LaneBasedStrategicalRoutePlanner(final LaneBasedTacticalPlanner fixedTacticalPlanner, final LaneBasedGTU gtu)
73 throws GTUException
74 {
75 this(fixedTacticalPlanner, null, gtu, null, null, RouteGeneratorOD.NULL);
76 }
77
78
79
80
81
82
83
84
85
86
87 public LaneBasedStrategicalRoutePlanner(final LaneBasedTacticalPlanner fixedTacticalPlanner, final Route route,
88 final LaneBasedGTU gtu, final Node origin, final Node destination) throws GTUException
89 {
90 this(fixedTacticalPlanner, route, gtu, origin, destination, RouteGeneratorOD.NULL);
91 }
92
93
94
95
96
97
98
99
100
101
102 public LaneBasedStrategicalRoutePlanner(final LaneBasedTacticalPlanner fixedTacticalPlanner, final LaneBasedGTU gtu,
103 final Node origin, final Node destination, final RouteGeneratorOD routeGenerator) throws GTUException
104 {
105 this(fixedTacticalPlanner, null, gtu, origin, destination, routeGenerator);
106 }
107
108
109
110
111
112
113
114
115
116
117
118
119 public LaneBasedStrategicalRoutePlanner(final LaneBasedTacticalPlanner fixedTacticalPlanner, final Route route,
120 final LaneBasedGTU gtu, final Node origin, final Node destination, final RouteGeneratorOD routeGenerator)
121 throws GTUException
122 {
123 super(gtu);
124 this.route = route;
125 this.origin = origin;
126 this.destination = destination;
127 this.fixedTacticalPlanner = fixedTacticalPlanner;
128 Throw.when(fixedTacticalPlanner == null, GTUException.class,
129 "Fixed Tactical Planner for a Strategical planner is null");
130 this.routeGenerator = routeGenerator;
131 }
132
133
134 @Override
135 public final LaneBasedTacticalPlanner getTacticalPlanner()
136 {
137 return this.fixedTacticalPlanner;
138 }
139
140
141 @Override
142 public LaneBasedTacticalPlanner getTacticalPlanner(final Time time)
143 {
144 return this.fixedTacticalPlanner;
145 }
146
147
148 @Override
149 public final Node nextNode(final Link link, final GTUDirectionality direction, final GTUType gtuType)
150 throws NetworkException
151 {
152 assureRoute(gtuType);
153 LinkDirection linkDirection = nextLinkDirection(link, direction, gtuType);
154 return linkDirection.getNodeTo();
155 }
156
157
158 @Override
159 public final LinkDirection nextLinkDirection(final Link link, final GTUDirectionality direction, final GTUType gtuType)
160 throws NetworkException
161 {
162 assureRoute(gtuType);
163 Node nextNode = direction.equals(GTUDirectionality.DIR_PLUS) ? link.getEndNode() : link.getStartNode();
164 if ((null != this.route) && (!this.route.contains(nextNode)))
165 {
166 link.getSimulator().getLogger().always().warn("nextNode {} is not in route {}", nextNode, this.route);
167 Node prevNode = direction.equals(GTUDirectionality.DIR_PLUS) ? link.getStartNode() : link.getEndNode();
168 link.getSimulator().getLogger().always().warn(" other node of link is {}", prevNode);
169 int index = 0;
170 for (Node node : this.route.getNodes())
171 {
172 link.getSimulator().getLogger().always().warn("{} {}{}", index, node.equals(prevNode) ? "--->" : " ", node);
173 index++;
174 }
175 }
176 return nextLinkDirection(nextNode, link, gtuType);
177 }
178
179
180 @Override
181 public final Node nextNode(final Node node, final Link previousLink, final GTUType gtuType) throws NetworkException
182 {
183 assureRoute(gtuType);
184 LinkDirection linkDirection = nextLinkDirection(node, previousLink, gtuType);
185 return linkDirection.getNodeTo();
186 }
187
188
189 @Override
190 public final LinkDirection nextLinkDirection(final Node node, final Link previousLink, final GTUType gtuType)
191 throws NetworkException
192 {
193 assureRoute(gtuType);
194
195
196 if (node.getLinks().size() == 1 && previousLink != null)
197 {
198
199 throw new NetworkException("LaneBasedStrategicalRoutePlanner is asked for a next link, but node " + node
200 + " has no successors");
201 }
202 if (node.getLinks().size() == 1 && previousLink == null)
203 {
204
205 Link link = node.getLinks().iterator().next();
206 return link.getStartNode().equals(node) ? new LinkDirection(link, GTUDirectionality.DIR_PLUS)
207 : new LinkDirection(link, GTUDirectionality.DIR_MINUS);
208 }
209 if (node.getLinks().size() == 2)
210 {
211 for (Link link : node.getLinks())
212 {
213 if (!link.equals(previousLink))
214 {
215 return link.getStartNode().equals(node) ? new LinkDirection(link, GTUDirectionality.DIR_PLUS)
216 : new LinkDirection(link, GTUDirectionality.DIR_MINUS);
217 }
218 }
219 }
220
221
222 Set<Link> links = node.getLinks().toSet();
223 for (Iterator<Link> linkIterator = links.iterator(); linkIterator.hasNext();)
224 {
225 Link link = linkIterator.next();
226 if (link.equals(previousLink))
227 {
228
229 linkIterator.remove();
230 }
231 else
232 {
233
234 if ((link.getStartNode().equals(node) && !link.getDirectionality(gtuType).isForwardOrBoth()) || (link
235 .getEndNode().equals(node) && !link.getDirectionality(gtuType).isBackwardOrBoth()))
236 {
237 linkIterator.remove();
238 }
239 else
240 {
241
242 boolean out = false;
243 CrossSectionLink./../../org/opentrafficsim/road/network/lane/CrossSectionLink.html#CrossSectionLink">CrossSectionLink csLink = (CrossSectionLink) link;
244
245 for (CrossSectionElement cse : csLink.getCrossSectionElementList())
246 {
247 if (cse instanceof Lane)
248 {
249 Laneef="../../../../../../org/opentrafficsim/road/network/lane/Lane.html#Lane">Lane lane = (Lane) cse;
250 if ((link.getStartNode().equals(node) && lane.getLaneType().isCompatible(gtuType,
251 GTUDirectionality.DIR_PLUS)) || (link.getEndNode().equals(node) && lane.getLaneType()
252 .isCompatible(gtuType, GTUDirectionality.DIR_MINUS)))
253 {
254 out = true;
255 }
256 }
257 }
258 if (!out)
259 {
260 linkIterator.remove();
261 }
262 }
263 }
264 }
265
266 if (links.size() == 1)
267 {
268 Link link = links.iterator().next();
269 return link.getStartNode().equals(node) ? new LinkDirection(link, GTUDirectionality.DIR_PLUS)
270 : new LinkDirection(link, GTUDirectionality.DIR_MINUS);
271 }
272
273
274 if (getRoute() == null)
275 {
276 throw new NetworkException("LaneBasedStrategicalRoutePlanner does not have a route");
277 }
278 int i = this.route.getNodes().indexOf(node);
279 if (i == -1)
280 {
281 throw new NetworkException("LaneBasedStrategicalRoutePlanner is asked for a next link coming from "
282 + previousLink + ", but node " + node + " not in route " + this.route);
283 }
284 if (i == this.route.getNodes().size() - 1)
285 {
286 throw new NetworkException("LaneBasedStrategicalRoutePlanner is asked for a next link coming from "
287 + previousLink + ", but the GTU reached the last node for route " + this.route);
288 }
289 Node nextNode = this.route.getNode(i + 1);
290 LinkDirection result = null;
291 for (Link link : links)
292 {
293
294
295 LinkDirection ld = null;
296 if (link.getStartNode().equals(nextNode) && link.getEndNode().equals(node))
297 {
298 ld = new LinkDirection(link, GTUDirectionality.DIR_MINUS);
299 }
300 if (link.getEndNode().equals(nextNode) && link.getStartNode().equals(node))
301 {
302 ld = new LinkDirection(link, GTUDirectionality.DIR_PLUS);
303 }
304 if (null != result && null != ld)
305 {
306 throw new NetworkException("Cannot choose among multiple links from " + node + " to " + nextNode);
307 }
308 else if (null == result)
309 {
310 result = ld;
311 }
312 }
313 if (null == result)
314 {
315 throw new NetworkException("LaneBasedStrategicalRoutePlanner is asked for a next link coming from "
316 + previousLink.getId() + ", but no link could be found connecting node " + node + " and node " + nextNode
317 + " for route " + this.route);
318 }
319 return result;
320 }
321
322
323 @Override
324 public final Route getRoute()
325 {
326 assureRoute(getGtu().getGTUType());
327 if (this.route == null && this.destination != null)
328 {
329 try
330 {
331 DirectedLanePosition pos = getGtu().getReferencePosition();
332 CrossSectionLink link = pos.getLane().getParentLink();
333 Node from = pos.getGtuDirection().isPlus() ? link.getStartNode() : link.getEndNode();
334 if (this.routeGenerator != null)
335 {
336 this.route = this.routeGenerator.getRoute(from, this.destination, getGtu().getGTUType());
337 }
338 if (this.route == null)
339 {
340 this.route = link.getNetwork().getShortestRouteBetween(getGtu().getGTUType(), from, this.destination);
341 }
342 }
343 catch (GTUException | NetworkException exception)
344 {
345 throw new RuntimeException("Route could not be determined.", exception);
346 }
347 }
348 return this.route;
349 }
350
351
352
353
354
355 private void assureRoute(final GTUType gtuType)
356 {
357 if (this.route == null && this.destination != null && !this.routeGenerator.equals(RouteGeneratorOD.NULL))
358 {
359 DirectedLanePosition ref =
360 Try.assign(() -> getGtu().getReferencePosition(), "Could not retrieve GTU reference position.");
361 List<Node> nodes = new ArrayList<>();
362 if (this.origin != null)
363 {
364 nodes.addAll(this.routeGenerator.getRoute(this.origin, ref.getLinkDirection().getNodeFrom(), gtuType)
365 .getNodes());
366 }
367 else
368 {
369 nodes.add(ref.getLinkDirection().getNodeFrom());
370 }
371 Route newRoute = this.routeGenerator.getRoute(ref.getLinkDirection().getNodeTo(), this.destination, gtuType);
372 if (null == newRoute)
373 {
374 System.err.println("this.routeGenerator.getRoute() returned null");
375 throw new RuntimeException("getRoute failed");
376 }
377 List<Node> newNodes = newRoute.getNodes();
378 if (newNodes == null)
379 {
380 System.err.println("Route.getNodes() returned null");
381 newRoute.getNodes();
382 }
383 nodes.addAll(newNodes);
384 this.route = Try.assign(() -> new CompleteRoute("Route for " + gtuType + " from " + this.origin + "to "
385 + this.destination + " via " + ref.getLinkDirection(), gtuType, nodes), "No route possible over nodes %s",
386 nodes);
387
388 }
389 }
390
391
392 @Override
393 public final Node getOrigin()
394 {
395 return this.origin;
396 }
397
398
399 @Override
400 public final Node getDestination()
401 {
402 return this.destination;
403 }
404
405
406 @Override
407 public final String toString()
408 {
409 return "LaneBasedStrategicalRoutePlanner [route=" + this.route + ", fixedTacticalPlanner="
410 + this.fixedTacticalPlanner + "]";
411 }
412
413 }