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
26
27
28
29
30
31
32
33
34
35
36 public class LaneBasedStrategicalRoutePlanner implements LaneBasedStrategicalPlanner, Serializable
37 {
38
39 private static final long serialVersionUID = 20151126L;
40
41
42 private final LaneBasedGtu gtu;
43
44
45 private Route route;
46
47
48 private final Node origin;
49
50
51 private Node destination;
52
53
54 private final LaneBasedTacticalPlanner fixedTacticalPlanner;
55
56
57 private final RouteGenerator routeGenerator;
58
59
60
61
62
63
64
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
74
75
76
77
78
79
80
81
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;
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
122 if (node.getLinks().size() == 1 && previousLink != null)
123 {
124
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
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
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
152 linkIterator.remove();
153 }
154 else
155 {
156
157 if (link.getEndNode().equals(node))
158 {
159 linkIterator.remove();
160 }
161 else
162 {
163
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
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
208
209 Link l = null;
210 if (link.getStartNode().equals(nextNode) && link.getEndNode().equals(node))
211 {
212 l = link;
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
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
260
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 }