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