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