1 package org.opentrafficsim.road.network.factory;
2
3 import java.awt.geom.Rectangle2D;
4 import java.io.IOException;
5 import java.net.SocketException;
6 import java.net.URL;
7 import java.rmi.RemoteException;
8 import java.util.ArrayList;
9 import java.util.Collections;
10 import java.util.HashSet;
11 import java.util.List;
12 import java.util.Random;
13 import java.util.Set;
14
15 import javax.naming.NamingException;
16 import javax.swing.JPanel;
17 import javax.swing.SwingUtilities;
18 import javax.xml.parsers.ParserConfigurationException;
19
20 import nl.javel.gisbeans.io.esri.CoordinateTransform;
21 import nl.tudelft.simulation.dsol.SimRuntimeException;
22 import nl.tudelft.simulation.dsol.animation.D2.GisRenderable2D;
23 import nl.tudelft.simulation.dsol.simulators.SimulatorInterface;
24 import nl.tudelft.simulation.jstats.distributions.DistConstant;
25 import nl.tudelft.simulation.jstats.distributions.DistExponential;
26 import nl.tudelft.simulation.jstats.distributions.DistTriangular;
27 import nl.tudelft.simulation.jstats.distributions.DistUniform;
28 import nl.tudelft.simulation.jstats.streams.MersenneTwister;
29 import nl.tudelft.simulation.jstats.streams.StreamInterface;
30 import nl.tudelft.simulation.language.io.URLResource;
31
32 import org.djunits.unit.AccelerationUnit;
33 import org.djunits.unit.LengthUnit;
34 import org.djunits.unit.SpeedUnit;
35 import org.djunits.unit.TimeUnit;
36 import org.djunits.value.vdouble.scalar.Acceleration;
37 import org.djunits.value.vdouble.scalar.DoubleScalar;
38 import org.djunits.value.vdouble.scalar.Length;
39 import org.djunits.value.vdouble.scalar.Speed;
40 import org.djunits.value.vdouble.scalar.Time;
41 import org.opentrafficsim.core.dsol.OTSDEVSSimulatorInterface;
42 import org.opentrafficsim.core.dsol.OTSModelInterface;
43 import org.opentrafficsim.core.dsol.OTSSimTimeDouble;
44 import org.opentrafficsim.core.geometry.OTSGeometryException;
45 import org.opentrafficsim.core.gtu.GTUDirectionality;
46 import org.opentrafficsim.core.gtu.GTUException;
47 import org.opentrafficsim.core.gtu.GTUType;
48 import org.opentrafficsim.core.gtu.animation.AccelerationGTUColorer;
49 import org.opentrafficsim.core.gtu.animation.GTUColorer;
50 import org.opentrafficsim.core.gtu.animation.IDGTUColorer;
51 import org.opentrafficsim.core.gtu.animation.SwitchableGTUColorer;
52 import org.opentrafficsim.core.gtu.animation.VelocityGTUColorer;
53 import org.opentrafficsim.core.network.Link;
54 import org.opentrafficsim.core.network.LongitudinalDirectionality;
55 import org.opentrafficsim.core.network.NetworkException;
56 import org.opentrafficsim.core.network.Node;
57 import org.opentrafficsim.core.network.OTSNetwork;
58 import org.opentrafficsim.core.network.route.CompleteRoute;
59 import org.opentrafficsim.core.units.distributions.ContinuousDistDoubleScalar;
60 import org.opentrafficsim.road.gtu.lane.LaneBasedGTU;
61 import org.opentrafficsim.road.gtu.lane.LaneBasedIndividualGTU;
62 import org.opentrafficsim.road.gtu.lane.driver.LaneBasedBehavioralCharacteristics;
63 import org.opentrafficsim.road.gtu.lane.perception.LanePerceptionFull;
64 import org.opentrafficsim.road.gtu.lane.tactical.LaneBasedCFLCTacticalPlanner;
65 import org.opentrafficsim.road.gtu.lane.tactical.following.IDMPlusOld;
66 import org.opentrafficsim.road.gtu.lane.tactical.lanechangemobil.Altruistic;
67 import org.opentrafficsim.road.gtu.strategical.LaneBasedStrategicalPlanner;
68 import org.opentrafficsim.road.gtu.strategical.route.LaneBasedStrategicalRoutePlanner;
69 import org.opentrafficsim.road.network.factory.opendrive.GeneratorAnimation;
70 import org.opentrafficsim.road.network.factory.opendrive.OpenDriveNetworkLaneParser;
71 import org.opentrafficsim.road.network.factory.opendrive.communicationRTI.ReceiverThread;
72 import org.opentrafficsim.road.network.lane.CrossSectionElement;
73 import org.opentrafficsim.road.network.lane.CrossSectionLink;
74 import org.opentrafficsim.road.network.lane.DirectedLanePosition;
75 import org.opentrafficsim.road.network.lane.Lane;
76 import org.opentrafficsim.road.network.lane.NoTrafficLane;
77 import org.opentrafficsim.road.network.lane.Sensor;
78 import org.opentrafficsim.road.network.lane.SinkSensor;
79 import org.opentrafficsim.simulationengine.AbstractWrappableAnimation;
80 import org.opentrafficsim.simulationengine.OTSSimulationException;
81 import org.opentrafficsim.simulationengine.properties.AbstractProperty;
82 import org.xml.sax.SAXException;
83
84
85
86
87
88
89
90
91
92
93 public class TestOpenDriveParserNASA extends AbstractWrappableAnimation
94 {
95
96
97
98
99
100 public static void main(final String[] args) throws SimRuntimeException
101 {
102 SwingUtilities.invokeLater(new Runnable()
103 {
104 @Override
105 public void run()
106 {
107 try
108 {
109 TestOpenDriveParserNASA xmlModel = new TestOpenDriveParserNASA();
110
111 xmlModel.buildAnimator(new Time.Abs(0.0, TimeUnit.SECOND), new Time.Rel(0.0, TimeUnit.SECOND),
112 new Time.Rel(60.0, TimeUnit.MINUTE), new ArrayList<AbstractProperty<?>>(), null, true);
113 }
114 catch (SimRuntimeException | NamingException | OTSSimulationException exception)
115 {
116 exception.printStackTrace();
117 }
118 }
119 });
120 }
121
122
123 @Override
124 public final String shortName()
125 {
126 return "TestOpenDriveModel";
127 }
128
129
130 @Override
131 public final String description()
132 {
133 return "TestOpenDriveModel";
134 }
135
136
137 @Override
138 public final void stopTimersThreads()
139 {
140 super.stopTimersThreads();
141 }
142
143
144 @Override
145 protected final JPanel makeCharts()
146 {
147 return null;
148 }
149
150
151 @Override
152 protected final OTSModelInterface makeModel(final GTUColorer colorer)
153 {
154 return new TestOpenDriveModel();
155 }
156
157
158 @Override
159 protected final Rectangle2D.Double makeAnimationRectangle()
160 {
161 return new Rectangle2D.Double(-1000, -1000, 2000, 2000);
162 }
163
164
165
166
167
168
169
170
171
172
173
174
175
176 class TestOpenDriveModel implements OTSModelInterface
177 {
178
179 private static final long serialVersionUID = 20150811L;
180
181
182 private OTSDEVSSimulatorInterface simulator;
183
184 private List<LaneBasedIndividualGTU> rtiCars;
185
186
187 @Override
188 public final
189 void
190 constructModel(
191 final SimulatorInterface<DoubleScalar.Abs<TimeUnit>, DoubleScalar.Rel<TimeUnit>, OTSSimTimeDouble> pSimulator)
192 throws SimRuntimeException
193 {
194 this.simulator = (OTSDEVSSimulatorInterface) pSimulator;
195
196 this.rtiCars = new ArrayList<LaneBasedIndividualGTU>();
197
198 URL url = URLResource.getResource("/NASAames.xodr");
199
200 this.simulator.setPauseOnError(false);
201 OpenDriveNetworkLaneParser nlp = new OpenDriveNetworkLaneParser(this.simulator);
202 OTSNetwork network = null;
203 try
204 {
205 network = nlp.build(url);
206 }
207 catch (NetworkException | ParserConfigurationException | SAXException | IOException | NamingException
208 | GTUException | OTSGeometryException exception)
209 {
210 exception.printStackTrace();
211 }
212
213 URL gisURL = URLResource.getResource("/gis/map.xml");
214 System.err.println("GIS-map file: " + gisURL.toString());
215
216
217
218
219 double latCenter = nlp.getHeaderTag().getOriginLat().si, lonCenter = nlp.getHeaderTag().getOriginLong().si;
220
221 CoordinateTransform latLonToXY = new CoordinateTransformLonLatToXY(lonCenter, latCenter);
222 new GisRenderable2D(this.simulator, gisURL, latLonToXY);
223
224
225 GTUType carType = GTUType.makeGTUType("Car");
226
227
228
229
230 StreamInterface stream = new MersenneTwister(1);
231 Length.Rel M25 = new Length.Rel(25.0, LengthUnit.METER);
232 Length.Rel M0 = new Length.Rel(0.0, LengthUnit.METER);
233
234
235 ContinuousDistDoubleScalar.Rel<Speed, SpeedUnit> initialSpeedDist =
236 new ContinuousDistDoubleScalar.Rel<>(new DistConstant(stream, 0.0), SpeedUnit.SI);
237 ContinuousDistDoubleScalar.Rel<Time.Rel, TimeUnit> iatDist =
238 new ContinuousDistDoubleScalar.Rel<>(new DistExponential(stream, 30.0), TimeUnit.SECOND);
239 ContinuousDistDoubleScalar.Rel<Length.Rel, LengthUnit> lengthDist =
240 new ContinuousDistDoubleScalar.Rel<>(new DistUniform(stream, 4.0, 5.0), LengthUnit.METER);
241 ContinuousDistDoubleScalar.Rel<Length.Rel, LengthUnit> widthDist =
242 new ContinuousDistDoubleScalar.Rel<>(new DistConstant(stream, 2.0), LengthUnit.METER);
243 ContinuousDistDoubleScalar.Rel<Speed, SpeedUnit> maxSpeedDist =
244 new ContinuousDistDoubleScalar.Rel<>(new DistTriangular(stream, 30.0, 35.0, 40.0),
245 SpeedUnit.MILE_PER_HOUR);
246
247 ContinuousDistDoubleScalar.Rel<Length.Rel, LengthUnit> initialPosDist =
248 new ContinuousDistDoubleScalar.Rel<>(new DistUniform(stream, 0.0, 1.0), LengthUnit.METER);
249
250
251
252
253 for (Link link : network.getLinkMap().values())
254 {
255 CrossSectionLink csLink = (CrossSectionLink) link;
256
257 if (link.getStartNode().getLinks().size() == 1)
258 {
259
260 for (CrossSectionElement cse : csLink.getCrossSectionElementList())
261 {
262 if (cse instanceof Lane && !(cse instanceof NoTrafficLane))
263 {
264 Lane lane = (Lane) cse;
265 if (Integer.parseInt(lane.getId()) < 0)
266 {
267
268 Time.Abs startTime = Time.Abs.ZERO;
269 Time.Abs endTime = new Time.Abs(Double.MAX_VALUE, TimeUnit.SI);
270 Length.Rel position = lane.getLength().lt(M25) ? M0 : M25;
271 String id = lane.getParentLink().getId() + "." + lane.getId();
272 LaneBasedBehavioralCharacteristics drivingCharacteristics =
273 new LaneBasedBehavioralCharacteristics(new IDMPlusOld(), new Altruistic());
274 LaneBasedStrategicalPlanner strategicalPlanner =
275 new LaneBasedStrategicalRoutePlanner(drivingCharacteristics,
276 new LaneBasedCFLCTacticalPlanner());
277 LanePerceptionFull perception = new LanePerceptionFull();
278
279
280
281
282 try
283 {
284 new GeneratorAnimation(lane, position, this.simulator);
285 }
286 catch (RemoteException | NamingException | OTSGeometryException exception)
287 {
288 exception.printStackTrace();
289 }
290 }
291 else
292 {
293
294 Length.Rel position = lane.getLength().lt(M25) ? M0 : M25;
295 Sensor sensor = new SinkSensor(lane, position, this.simulator);
296 try
297 {
298 lane.addSensor(sensor, GTUType.ALL);
299 }
300 catch (NetworkException exception)
301 {
302 exception.printStackTrace();
303 }
304 }
305 }
306 }
307 }
308 else if (link.getEndNode().getLinks().size() == 1)
309 {
310
311 for (CrossSectionElement cse : csLink.getCrossSectionElementList())
312 {
313 if (cse instanceof Lane && !(cse instanceof NoTrafficLane))
314 {
315 Lane lane = (Lane) cse;
316 if (Integer.parseInt(lane.getId()) > 0)
317 {
318
319 Time.Abs startTime = Time.Abs.ZERO;
320 Time.Abs endTime = new Time.Abs(Double.MAX_VALUE, TimeUnit.SI);
321 Length.Rel position =
322 lane.getLength().lt(M25) ? lane.getLength() : lane.getLength().minus(M25);
323 String id = lane.getParentLink().getId() + "." + lane.getId();
324 LaneBasedBehavioralCharacteristics drivingCharacteristics =
325 new LaneBasedBehavioralCharacteristics(new IDMPlusOld(), new Altruistic());
326 LaneBasedStrategicalPlanner strategicalPlanner =
327 new LaneBasedStrategicalRoutePlanner(drivingCharacteristics,
328 new LaneBasedCFLCTacticalPlanner());
329 LanePerceptionFull perception = new LanePerceptionFull();
330
331
332
333
334 try
335 {
336 new GeneratorAnimation(lane, position, this.simulator);
337 }
338 catch (RemoteException | NamingException | OTSGeometryException exception)
339 {
340 exception.printStackTrace();
341 }
342 }
343 else
344 {
345
346 Length.Rel position =
347 lane.getLength().lt(M25) ? lane.getLength() : lane.getLength().minus(M25);
348 Sensor sensor = new SinkSensor(lane, position, this.simulator);
349 try
350 {
351 lane.addSensor(sensor, GTUType.ALL);
352 }
353 catch (NetworkException exception)
354 {
355 exception.printStackTrace();
356 }
357 }
358 }
359 }
360 }
361 }
362
363 CrossSectionLink link1 = (CrossSectionLink) network.getLink("54059");
364 CrossSectionLink link2 = (CrossSectionLink) network.getLink("117957");
365 CrossSectionLink link3 = (CrossSectionLink) network.getLink("54062");
366 CrossSectionLink link4 = (CrossSectionLink) network.getLink("54083");
367 CrossSectionLink link5 = (CrossSectionLink) network.getLink("54085");
368 CrossSectionLink link6 = (CrossSectionLink) network.getLink("54045");
369 CrossSectionLink link7 = (CrossSectionLink) network.getLink("166405.1");
370 CrossSectionLink link8 = (CrossSectionLink) network.getLink("54053");
371
372 CompleteRoute cr1 = null, cr2 = null, cr3 = null, cr4 = null, cr5 = null, cr6 = null;
373
374 List<Node> nodesVia1 = new ArrayList<Node>();
375 nodesVia1.add(link1.getStartNode());
376
377
378 try
379 {
380 cr1 =
381 network.getShortestRouteBetween(GTUType.ALL, link2.getStartNode(), link2.getStartNode(), nodesVia1);
382 Collections.reverse(nodesVia1);
383 cr2 =
384 network.getShortestRouteBetween(GTUType.ALL, link2.getStartNode(), link2.getStartNode(), nodesVia1);
385 }
386 catch (NetworkException exception)
387 {
388 exception.printStackTrace();
389 }
390
391 List<Node> nodesVia2 = new ArrayList<Node>();
392 nodesVia2.add(link5.getEndNode());
393 nodesVia2.add(link6.getEndNode());
394 try
395 {
396 cr3 =
397 network.getShortestRouteBetween(GTUType.ALL, link3.getStartNode(), link3.getStartNode(), nodesVia2);
398 Collections.reverse(nodesVia2);
399 cr4 =
400 network.getShortestRouteBetween(GTUType.ALL, link3.getStartNode(), link3.getStartNode(), nodesVia2);
401 }
402 catch (NetworkException exception)
403 {
404 exception.printStackTrace();
405 }
406
407 List<Node> nodesVia3 = new ArrayList<Node>();
408 nodesVia3.add(link5.getEndNode());
409 nodesVia3.add(link8.getEndNode());
410 try
411 {
412 cr5 =
413 network.getShortestRouteBetween(GTUType.ALL, link6.getStartNode(), link6.getStartNode(), nodesVia3);
414 Collections.reverse(nodesVia3);
415 cr6 =
416 network.getShortestRouteBetween(GTUType.ALL, link6.getStartNode(), link6.getStartNode(), nodesVia3);
417 }
418 catch (NetworkException exception)
419 {
420 exception.printStackTrace();
421 }
422
423 List<CompleteRoute> cRoutes = new ArrayList<>();
424 cRoutes.add(cr1);
425 cRoutes.add(cr2);
426 cRoutes.add(cr3);
427 cRoutes.add(cr4);
428 cRoutes.add(cr5);
429 cRoutes.add(cr6);
430 Random routeRandom = new Random();
431
432 List<CrossSectionLink> links = new ArrayList<>();
433 links.add(link1);
434 links.add(link2);
435 links.add(link3);
436 links.add(link4);
437 links.add(link5);
438 links.add(link6);
439 links.add(link7);
440 links.add(link8);
441
442 for (int i = 0; i < 52; i++)
443 {
444 CompleteRoute cr = cRoutes.get(routeRandom.nextInt(6));
445
446 CrossSectionLink link;
447 while (true)
448 {
449 link = links.get(routeRandom.nextInt(8));
450 if (cr.getNodes().contains(link.getStartNode()))
451 break;
452 }
453
454 GTUDirectionality dir = GTUDirectionality.DIR_PLUS;
455 Lane lane = null;
456
457 while (true)
458 {
459 CrossSectionElement cse =
460 link.getCrossSectionElementList().get(
461 routeRandom.nextInt(link.getCrossSectionElementList().size()));
462 if (cse instanceof Lane && !(cse instanceof NoTrafficLane))
463 {
464 lane = (Lane) cse;
465 break;
466
467 }
468 }
469
470 if (lane.getDirectionality(carType).equals(LongitudinalDirectionality.DIR_MINUS))
471 {
472 dir = GTUDirectionality.DIR_MINUS;
473 }
474
475 LaneBasedBehavioralCharacteristics drivingCharacteristics =
476 new LaneBasedBehavioralCharacteristics(new IDMPlusOld(), new Altruistic());
477 LaneBasedStrategicalPlanner sPlanner =
478 new LaneBasedStrategicalRoutePlanner(drivingCharacteristics, new LaneBasedCFLCTacticalPlanner());
479 LanePerceptionFull perception = new LanePerceptionFull();
480
481 DirectedLanePosition directedLanePosition = null;
482 try
483 {
484 directedLanePosition =
485 new DirectedLanePosition(lane, initialPosDist.draw().multiplyBy(
486 lane.getCenterLine().getLengthSI()), dir);
487 }
488 catch (GTUException exception1)
489 {
490 exception1.printStackTrace();
491 }
492 Set<DirectedLanePosition> lanepositionSet = new HashSet<DirectedLanePosition>();
493 lanepositionSet.add(directedLanePosition);
494
495 Length.Rel carLength = lengthDist.draw();
496 double genPosSI = directedLanePosition.getPosition().getSI();
497 double lengthSI = lane.getLength().getSI();
498 double frontNew = (genPosSI + carLength.getSI()) / lengthSI;
499 double rearNew = genPosSI / lengthSI;
500
501 boolean isEnoughSpace = true;
502
503 for (LaneBasedGTU gtu : lane.getGtuList())
504 {
505 double frontGTU = 0;
506 try
507 {
508 frontGTU = gtu.fractionalPosition(lane, gtu.getFront());
509 }
510 catch (GTUException exception)
511 {
512 exception.printStackTrace();
513 }
514 double rearGTU = 0;
515 try
516 {
517 rearGTU = gtu.fractionalPosition(lane, gtu.getRear());
518 }
519 catch (GTUException exception)
520 {
521 exception.printStackTrace();
522 }
523 if ((frontNew >= rearGTU && frontNew <= frontGTU) || (rearNew >= rearGTU && rearNew <= frontGTU)
524 || (frontGTU >= rearNew && frontGTU <= frontNew) || (rearGTU >= rearNew && rearGTU <= frontNew))
525 isEnoughSpace = false;
526 }
527
528 if (isEnoughSpace)
529 {
530 try
531 {
532 LaneBasedIndividualGTU car =
533 new LaneBasedIndividualGTU(String.valueOf(i), carType, lanepositionSet, new Speed(0.0,
534 SpeedUnit.METER_PER_SECOND), carLength, widthDist.draw(), maxSpeedDist.draw(),
535 this.simulator, sPlanner, perception, network);
536 this.rtiCars.add(car);
537
538 }
539 catch (NamingException | NetworkException | GTUException | OTSGeometryException exception)
540 {
541 exception.printStackTrace();
542 }
543 }
544 else
545 {
546 i = i - 1;
547 }
548
549 }
550
551 try
552 {
553 new Thread(new ReceiverThread(this.simulator, carType, this.rtiCars, network)).start();
554 }
555 catch (SocketException exception1)
556 {
557 exception1.printStackTrace();
558 }
559
560 }
561
562
563 @Override
564 public SimulatorInterface<DoubleScalar.Abs<TimeUnit>, DoubleScalar.Rel<TimeUnit>, OTSSimTimeDouble>
565 getSimulator()
566
567 {
568 return this.simulator;
569 }
570
571
572
573
574 private GTUColorer makeSwitchableGTUColorer()
575 {
576 GTUColorer[] gtuColorers =
577 new GTUColorer[]{
578 new IDGTUColorer(),
579 new VelocityGTUColorer(new Speed(100.0, SpeedUnit.KM_PER_HOUR)),
580 new AccelerationGTUColorer(new Acceleration(1.0, AccelerationUnit.METER_PER_SECOND_2),
581 new Acceleration(1.0, AccelerationUnit.METER_PER_SECOND_2))};
582 return new SwitchableGTUColorer(0, gtuColorers);
583 }
584 }
585
586 }