1 package org.opentrafficsim.road.network.factory.opendrive;
2
3 import java.awt.Color;
4 import java.io.Serializable;
5 import java.lang.reflect.Constructor;
6 import java.lang.reflect.InvocationTargetException;
7 import java.rmi.RemoteException;
8 import java.util.ArrayList;
9 import java.util.HashSet;
10 import java.util.LinkedHashMap;
11 import java.util.List;
12 import java.util.Map;
13 import java.util.Set;
14
15 import javax.naming.NamingException;
16
17 import org.djunits.unit.LengthUnit;
18 import org.djunits.unit.SpeedUnit;
19 import org.djunits.value.vdouble.scalar.Length;
20 import org.djunits.value.vdouble.scalar.Speed;
21 import org.djutils.reflection.ClassUtil;
22 import org.opentrafficsim.core.dsol.OTSSimulatorInterface;
23 import org.opentrafficsim.core.geometry.OTSGeometryException;
24 import org.opentrafficsim.core.geometry.OTSLine3D;
25 import org.opentrafficsim.core.geometry.OTSPoint3D;
26 import org.opentrafficsim.core.gtu.GTUType;
27 import org.opentrafficsim.core.network.LinkType;
28 import org.opentrafficsim.core.network.LongitudinalDirectionality;
29 import org.opentrafficsim.core.network.NetworkException;
30 import org.opentrafficsim.core.network.OTSNetwork;
31 import org.opentrafficsim.core.network.OTSNode;
32 import org.opentrafficsim.draw.network.LinkAnimation;
33 import org.opentrafficsim.draw.road.LaneAnimation;
34 import org.opentrafficsim.draw.road.ShoulderAnimation;
35 import org.opentrafficsim.draw.road.StripeAnimation;
36 import org.opentrafficsim.road.network.factory.opendrive.LinkTag.ContactPointEnum;
37 import org.opentrafficsim.road.network.lane.CrossSectionElement;
38 import org.opentrafficsim.road.network.lane.CrossSectionLink;
39 import org.opentrafficsim.road.network.lane.CrossSectionSlice;
40 import org.opentrafficsim.road.network.lane.Lane;
41 import org.opentrafficsim.road.network.lane.LaneType;
42 import org.opentrafficsim.road.network.lane.NoTrafficLane;
43 import org.opentrafficsim.road.network.lane.Shoulder;
44 import org.opentrafficsim.road.network.lane.Stripe;
45 import org.opentrafficsim.road.network.lane.Stripe.Permeable;
46 import org.opentrafficsim.road.network.lane.changing.LaneKeepingPolicy;
47 import org.opentrafficsim.road.network.lane.object.trafficlight.SimpleTrafficLight;
48 import org.w3c.dom.NamedNodeMap;
49 import org.w3c.dom.Node;
50 import org.xml.sax.SAXException;
51
52 import nl.tudelft.simulation.dsol.animation.D2.Renderable2D;
53 import nl.tudelft.simulation.dsol.simulators.DEVSSimulatorInterface;
54
55
56
57
58
59
60
61
62
63
64 class RoadTag implements Serializable
65 {
66
67 private static final long serialVersionUID = 20150723L;
68
69
70 @SuppressWarnings("checkstyle:visibilitymodifier")
71 String id = null;
72
73
74 @SuppressWarnings("checkstyle:visibilitymodifier")
75 String name = null;
76
77
78 @SuppressWarnings("checkstyle:visibilitymodifier")
79 Length length = null;
80
81
82 @SuppressWarnings("checkstyle:visibilitymodifier")
83 String junctionId = null;
84
85
86 @SuppressWarnings("checkstyle:visibilitymodifier")
87 LinkTag linkTag = null;
88
89
90 @SuppressWarnings("checkstyle:visibilitymodifier")
91 PlanViewTag planViewTag = null;
92
93
94 @SuppressWarnings("checkstyle:visibilitymodifier")
95 ElevationProfileTag elevationProfileTag = null;
96
97
98 @SuppressWarnings("checkstyle:visibilitymodifier")
99 LateralProfileTag lateralProfileTag = null;
100
101
102 @SuppressWarnings("checkstyle:visibilitymodifier")
103 LanesTag lanesTag = null;
104
105
106 @SuppressWarnings("checkstyle:visibilitymodifier")
107 SignalsTag signalsTag = null;
108
109
110 @SuppressWarnings("checkstyle:visibilitymodifier")
111 ObjectsTag objectsTag = null;
112
113
114 @SuppressWarnings("checkstyle:visibilitymodifier")
115 TypeTag typeTag = null;
116
117
118 @SuppressWarnings("checkstyle:visibilitymodifier")
119 CrossSectionLink link = null;
120
121
122 @SuppressWarnings("checkstyle:visibilitymodifier")
123 OTSLine3D designLine = null;
124
125
126 @SuppressWarnings("checkstyle:visibilitymodifier")
127 OTSNode startNode = null;
128
129
130 @SuppressWarnings("checkstyle:visibilitymodifier")
131 OTSNode endNode = null;
132
133
134 @SuppressWarnings("checkstyle:visibilitymodifier")
135 List<CrossSectionLink> subLinks = new ArrayList<>();
136
137
138
139
140
141
142
143
144
145 @SuppressWarnings("checkstyle:needbraces")
146 static RoadTag parseRoad(final Node node, final OpenDriveNetworkLaneParser parser) throws SAXException, NetworkException
147 {
148 NamedNodeMap attributes = node.getAttributes();
149 RoadTag roadTag = new RoadTag();
150
151 Node id = attributes.getNamedItem("id");
152 if (id == null)
153 throw new SAXException("ROAD: missing attribute ID");
154 roadTag.id = id.getNodeValue().trim();
155 if (parser.roadTags.keySet().contains(roadTag.id))
156 throw new SAXException("ROAD: ID " + roadTag.id + " defined twice");
157
158 Node name = attributes.getNamedItem("name");
159 if (name == null)
160 throw new SAXException("ROAD: missing attribute ID for road with ID=" + roadTag.id);
161 roadTag.name = name.getNodeValue().trim();
162
163 Node length = attributes.getNamedItem("length");
164 if (length == null)
165 throw new SAXException("ROAD: missing attribute LENGTH");
166 roadTag.length = new Length(Double.parseDouble(length.getNodeValue().trim()), LengthUnit.METER);
167
168 Node junctionId = attributes.getNamedItem("junction");
169 if (junctionId == null)
170 throw new SAXException("ROAD: missing attribute junction for road id=" + roadTag.id);
171
172 roadTag.junctionId = junctionId.getNodeValue().trim();
173
174
175
176
177
178
179
180
181
182 parser.roadTags.put(roadTag.id, roadTag);
183
184 return roadTag;
185 }
186
187
188
189
190
191
192
193
194
195 static void buildSubLinks(RoadTag roadTag, OTSSimulatorInterface simulator,
196 OpenDriveNetworkLaneParser openDriveNetworkLaneParser)
197 throws OTSGeometryException, NetworkException, NamingException
198 {
199 OTSNetwork otsNetwork = openDriveNetworkLaneParser.network;
200 if (roadTag.lanesTag.laneSectionTags.size() == 1)
201 {
202
203 {
204 roadTag.subLinks.add(roadTag.link);
205 if (!otsNetwork.containsNode(roadTag.link.getStartNode()))
206 otsNetwork.addNode(roadTag.link.getStartNode());
207 if (!otsNetwork.containsNode(roadTag.link.getEndNode()))
208 otsNetwork.addNode(roadTag.link.getEndNode());
209 if (!otsNetwork.containsLink(roadTag.link))
210 otsNetwork.addLink(roadTag.link);
211 }
212
213 }
214 else
215 {
216
217 List<GeometryTag> tempGeometryTags = new ArrayList<GeometryTag>();
218 tempGeometryTags = roadTag.planViewTag.geometryTags;
219
220
221
222
223 int currentIndex = 0;
224 for (Integer laneSecIndex = 1; laneSecIndex < roadTag.lanesTag.laneSectionTags.size(); laneSecIndex++)
225 {
226 LaneSectionTag laneSec = roadTag.lanesTag.laneSectionTags.get(laneSecIndex);
227 Length laneSecLength = laneSec.s;
228
229 List<OTSPoint3D> points = new ArrayList<OTSPoint3D>();
230
231 GeometryTag from = tempGeometryTags.get(currentIndex);
232 GeometryTag to = tempGeometryTags.get(currentIndex);
233
234 for (int indexGeometryTag = currentIndex; indexGeometryTag < tempGeometryTags.size(); indexGeometryTag++)
235 {
236 GeometryTag currentGeometryTag = tempGeometryTags.get(indexGeometryTag);
237 if (currentGeometryTag.s.doubleValue() < laneSecLength.doubleValue())
238 {
239 OTSPoint3D point = new OTSPoint3D(currentGeometryTag.x.doubleValue(),
240 currentGeometryTag.y.doubleValue(), currentGeometryTag.z.doubleValue());
241
242 if (points.size() == 0)
243 points.add(point);
244 else
245 {
246 if (point.x != points.get(points.size() - 1).x && point.y != points.get(points.size() - 1).y)
247 points.add(point);
248 }
249
250 OTSPoint3D lastPoint = new OTSPoint3D(points.get(points.size() - 1));
251
252 if (currentGeometryTag.interLine != null)
253 {
254 for (OTSPoint3D point1 : currentGeometryTag.interLine.getPoints())
255 {
256
257
258
259
260 if (lastPoint.x != point1.x && lastPoint.y != point1.y)
261 {
262 points.add(point1);
263 lastPoint = point1;
264 }
265 }
266 }
267
268 to = tempGeometryTags.get(currentIndex);
269 currentIndex++;
270 continue;
271 }
272 else
273 {
274 OTSPoint3D point = new OTSPoint3D(currentGeometryTag.x.doubleValue(),
275 currentGeometryTag.y.doubleValue(), currentGeometryTag.z.doubleValue());
276
277 if (points.size() == 0)
278 points.add(point);
279 else
280 {
281 if (point.x != points.get(points.size() - 1).x && point.y != points.get(points.size() - 1).y)
282 points.add(point);
283 }
284
285 OTSPoint3D lastPoint = new OTSPoint3D(points.get(points.size() - 1));
286
287 if (currentGeometryTag.interLine != null)
288 {
289 for (OTSPoint3D point1 : currentGeometryTag.interLine.getPoints())
290 {
291
292
293
294
295
296
297 if (lastPoint.x != point.x && lastPoint.y != point.y)
298 {
299 points.add(point1);
300 lastPoint = point1;
301 }
302 }
303 }
304
305
306 to = tempGeometryTags.get(currentIndex);
307
308
309 OTSLine3D designLine = new OTSLine3D(points);
310 String sublinkId = roadTag.id + "." + laneSecIndex.toString();
311 CrossSectionLink sublink = new CrossSectionLink(openDriveNetworkLaneParser.network, sublinkId,
312 from.node, to.node, openDriveNetworkLaneParser.network.getLinkType(LinkType.DEFAULTS.ROAD),
313 designLine, simulator, LaneKeepingPolicy.KEEPLANE);
314
315 roadTag.subLinks.add(sublink);
316
317 if (!otsNetwork.containsNode(from.node))
318 otsNetwork.addNode(from.node);
319 if (!otsNetwork.containsNode(to.node))
320 otsNetwork.addNode(to.node);
321
322 if (!otsNetwork.containsLink(sublink))
323 otsNetwork.addLink(sublink);
324 else
325 System.err.println("Sublink already registered: " + sublink);
326
327 break;
328 }
329 }
330 }
331
332
333 List<OTSPoint3D> points = new ArrayList<OTSPoint3D>();
334
335 GeometryTag from = tempGeometryTags.get(currentIndex);
336 GeometryTag to = tempGeometryTags.get(tempGeometryTags.size() - 1);
337
338 for (int indexGeometryTag = currentIndex; indexGeometryTag < tempGeometryTags.size(); indexGeometryTag++)
339 {
340 GeometryTag currentGeometryTag = tempGeometryTags.get(indexGeometryTag);
341
342 OTSPoint3D point = new OTSPoint3D(currentGeometryTag.x.doubleValue(), currentGeometryTag.y.doubleValue(),
343 currentGeometryTag.z.doubleValue());
344
345
346 if (points.size() == 0)
347 points.add(point);
348 else
349 {
350 if (point.x != points.get(points.size() - 1).x && point.y != points.get(points.size() - 1).y)
351 points.add(point);
352 }
353
354 OTSPoint3D lastPoint = new OTSPoint3D(points.get(points.size() - 1));
355
356 if (currentGeometryTag.interLine != null)
357 {
358 for (OTSPoint3D point1 : currentGeometryTag.interLine.getPoints())
359 {
360
361
362
363
364
365
366
367 if (lastPoint.x != point.x && lastPoint.y != point.y)
368 {
369 points.add(point1);
370 lastPoint = point1;
371 }
372 }
373 }
374 }
375
376
377
378 OTSLine3D designLine = new OTSLine3D(points);
379 String sublinkId = roadTag.id + "." + Integer.toString(roadTag.lanesTag.laneSectionTags.size());
380 CrossSectionLink sublink = new CrossSectionLink(openDriveNetworkLaneParser.network, sublinkId, from.node, to.node,
381 openDriveNetworkLaneParser.network.getLinkType(LinkType.DEFAULTS.ROAD), designLine, simulator,
382 LaneKeepingPolicy.KEEPLANE);
383
384 roadTag.subLinks.add(sublink);
385
386 if (!otsNetwork.containsNode(from.node))
387 otsNetwork.addNode(from.node);
388 if (!otsNetwork.containsNode(to.node))
389 otsNetwork.addNode(to.node);
390
391 if (!otsNetwork.containsLink(sublink))
392 otsNetwork.addLink(sublink);
393 else
394 System.err.println("Sublink already registered: " + sublink);
395 }
396
397 }
398
399
400
401
402
403
404
405
406
407
408 static void generateRegularRoads(RoadTag roadTag, DEVSSimulatorInterface.TimeDoubleUnit simulator,
409 OpenDriveNetworkLaneParser openDriveNetworkLaneParser)
410 throws OTSGeometryException, NetworkException, NamingException, RemoteException
411 {
412
413 for (int laneSecIndex = 0; laneSecIndex < roadTag.lanesTag.laneSectionTags.size(); laneSecIndex++)
414 {
415 LaneSectionTag currentLaneSec = roadTag.lanesTag.laneSectionTags.get(laneSecIndex);
416
417 CrossSectionLink currentLink = roadTag.subLinks.get(laneSecIndex);
418
419 Length ds = new Length(0.0, LengthUnit.METER);
420 LaneSectionTag nextLaneSec;
421 if (laneSecIndex != roadTag.lanesTag.laneSectionTags.size() - 1)
422 {
423 nextLaneSec = roadTag.lanesTag.laneSectionTags.get(laneSecIndex + 1);
424 ds = nextLaneSec.s.minus(currentLaneSec.s);
425 }
426 else
427 {
428 ds = roadTag.length.minus(currentLaneSec.s);
429 }
430
431 CrossSectionElement lastLane = null;
432
433
434 int centerLaneSize = currentLaneSec.centerLaneTags.size();
435 if (centerLaneSize != 1)
436 System.err.println("Sth is wrong in center lane");
437 Length centerOffset = new Length(0.0, LengthUnit.METER);
438
439 LaneTag centerLane = currentLaneSec.centerLaneTags.get(0);
440 Length laneWidth = new Length(0.0, LengthUnit.METER);
441 if (centerLane.widthTags.size() != 0)
442 System.err.println("error in show center stripe!");
443
444 Stripe centerStripe = new Stripe(currentLink, centerOffset, centerOffset, laneWidth);
445 try
446 {
447 Renderable2D<Stripe> animation = new StripeAnimation(centerStripe, simulator, StripeAnimation.TYPE.SOLID);
448 openDriveNetworkLaneParser.animationMap.put(centerStripe, animation);
449 }
450 catch (RemoteException exception)
451 {
452 exception.printStackTrace();
453 }
454
455 lastLane = centerStripe;
456
457
458 int leftLaneSize = currentLaneSec.leftLaneTags.size();
459
460
461
462 for (int leftLaneIndex = 1; leftLaneIndex <= leftLaneSize; leftLaneIndex++)
463 {
464 LaneTag leftLane = currentLaneSec.leftLaneTags.get(leftLaneIndex);
465
466 List<CrossSectionSlice> crossSectionSlices = new ArrayList<CrossSectionSlice>();
467 if (leftLane.widthTags.size() == 1)
468 {
469 leftLane.widthTags.get(0).sOffst =
470 leftLane.widthTags.get(0).a.plus(leftLane.widthTags.get(0).b.multiplyBy(ds.doubleValue()))
471 .plus(leftLane.widthTags.get(0).c.multiplyBy(Math.pow(ds.doubleValue(), 2)))
472 .plus(leftLane.widthTags.get(0).d.multiplyBy(Math.pow(ds.doubleValue(), 3)));
473
474 Length laneWidth_start = leftLane.widthTags.get(0).a;
475 Length laneWidth_end = leftLane.widthTags.get(0).sOffst;
476
477 Length leftOffset_start = lastLane.getDesignLineOffsetAtBegin()
478 .plus(lastLane.getBeginWidth().multiplyBy(0.5)).plus(laneWidth_start.multiplyBy(0.5));
479 Length leftOffset_end = lastLane.getDesignLineOffsetAtEnd().plus(lastLane.getEndWidth().multiplyBy(0.5))
480 .plus(laneWidth_end.multiplyBy(0.5));
481
482 Length length = currentLink.getLength();
483
484 CrossSectionSlice startSlice =
485 new CrossSectionSlice(new Length(0.0, LengthUnit.METER), leftOffset_start, laneWidth_start);
486 CrossSectionSlice endSlice = new CrossSectionSlice(length, leftOffset_end, laneWidth_end);
487 crossSectionSlices.add(startSlice);
488 crossSectionSlices.add(endSlice);
489
490 }
491 else
492 {
493
494
495 Length lengthofLane = leftLane.widthTags.get(leftLane.widthTags.size() - 1).sOffst;
496 for (WidthTag widthTag : leftLane.widthTags)
497 {
498 Length relativeLength = widthTag.sOffst;
499 double factor = relativeLength.divideBy(lengthofLane).doubleValue();
500
501 if (factor < 0.98)
502 {
503 Length width = widthTag.a.plus(widthTag.b.multiplyBy(relativeLength.doubleValue()))
504 .plus(widthTag.c.multiplyBy(Math.pow(relativeLength.doubleValue(), 2)))
505 .plus(widthTag.d.multiplyBy(Math.pow(relativeLength.doubleValue(), 3)));
506
507 Length offSet = lastLane.getLateralCenterPosition(factor)
508 .plus(lastLane.getWidth(factor).multiplyBy(0.5)).plus(width.multiplyBy(0.5));
509
510 relativeLength = currentLink.getLength().multiplyBy(factor);
511
512 CrossSectionSlice slice = new CrossSectionSlice(relativeLength, offSet, width);
513 crossSectionSlices.add(slice);
514 }
515 else
516 {
517 CrossSectionSlice lastSlice = crossSectionSlices.get(crossSectionSlices.size() - 1);
518 Length width = lastSlice.getWidth();
519 Length offSet = lastSlice.getDesignLineOffset();
520 relativeLength = currentLink.getLength();
521
522 CrossSectionSlice slice = new CrossSectionSlice(relativeLength, offSet, width);
523 crossSectionSlices.add(slice);
524 break;
525 }
526 }
527 }
528
529 Speed speed = null;
530 if (leftLane.speedTags.size() > 0)
531 speed = leftLane.speedTags.get(0).max;
532 if (speed == null)
533 {
534
535 speed = new Speed(30.0, SpeedUnit.MILE_PER_HOUR);
536 }
537
538 Map<GTUType, Speed> speedLimit = new LinkedHashMap<>();
539 speedLimit.put(openDriveNetworkLaneParser.network.getGtuType(GTUType.DEFAULTS.VEHICLE), speed);
540
541 if (leftLane.type.equals("driving"))
542 {
543 LongitudinalDirectionality direction = LongitudinalDirectionality.DIR_MINUS;
544 Color color = Color.gray;
545
546
547
548
549
550
551 Lane lane = new Lane(currentLink, leftLane.id.toString(), crossSectionSlices,
552 openDriveNetworkLaneParser.network.getLaneType(LaneType.DEFAULTS.FREEWAY), speedLimit);
553 currentLaneSec.lanes.put(leftLane.id, lane);
554
555 lastLane = lane;
556
557 try
558 {
559 Renderable2D animation = new LaneAnimationOD(lane, simulator, color);
560 openDriveNetworkLaneParser.animationMap.put(lane, animation);
561 }
562 catch (RemoteException exception)
563 {
564 Renderable2D animation =
565 new org.opentrafficsim.draw.network.LinkAnimation(currentLink, simulator, 0.01f);
566 openDriveNetworkLaneParser.animationMap.put(currentLink, animation);
567 exception.printStackTrace();
568 }
569 }
570 else if (leftLane.type.equals("sidewalk"))
571 {
572 Color color = Color.darkGray;
573
574
575
576
577 Lane lane = new NoTrafficLane(currentLink, leftLane.id.toString(), crossSectionSlices);
578
579 currentLaneSec.lanes.put(leftLane.id, lane);
580
581 lastLane = lane;
582
583 try
584 {
585 Renderable2D animation = new LaneAnimation(lane, simulator, color, false);
586 openDriveNetworkLaneParser.animationMap.put(lane, animation);
587 }
588 catch (RemoteException exception)
589 {
590 exception.printStackTrace();
591 }
592 }
593 else if (leftLane.type.equals("border"))
594 {
595 Stripe solidLine = new Stripe(currentLink, crossSectionSlices, Permeable.BOTH);
596
597 lastLane = solidLine;
598
599 try
600 {
601 Renderable2D animation = new StripeAnimation(solidLine, simulator, StripeAnimation.TYPE.SOLID);
602 openDriveNetworkLaneParser.animationMap.put(solidLine, animation);
603 }
604 catch (RemoteException exception)
605 {
606 exception.printStackTrace();
607 }
608 }
609 else if (leftLane.type.equals("shoulder"))
610 {
611 Color color = Color.green;
612 Shoulder shoulder = new Shoulder(currentLink, leftLane.id.toString(), crossSectionSlices);
613 lastLane = shoulder;
614
615 try
616 {
617 Renderable2D animation = new ShoulderAnimation(shoulder, simulator, color);
618 openDriveNetworkLaneParser.animationMap.put(shoulder, animation);
619 }
620 catch (RemoteException exception)
621 {
622 exception.printStackTrace();
623 }
624 }
625 else
626
627 {
628
629
630
631
632
633
634 Color color = Color.green;
635
636 try
637 {
638 Lane lane = new NoTrafficLane(currentLink, leftLane.id.toString(), crossSectionSlices);
639
640 currentLaneSec.lanes.put(leftLane.id, lane);
641
642 lastLane = lane;
643 Renderable2D animation = new LaneAnimation(lane, simulator, color, false);
644 openDriveNetworkLaneParser.animationMap.put(lane, animation);
645 }
646 catch (Exception exception)
647 {
648 Renderable2D animation = new LinkAnimation(currentLink, simulator, 0.01f);
649 openDriveNetworkLaneParser.animationMap.put(currentLink, animation);
650 exception.printStackTrace();
651 }
652 }
653
654 }
655
656 lastLane = centerStripe;
657
658
659 int rightLaneSize = currentLaneSec.rightLaneTags.size();
660
661
662
663 for (int rightLaneIndex = 1; rightLaneIndex <= rightLaneSize; rightLaneIndex++)
664 {
665 LaneTag rightLane = currentLaneSec.rightLaneTags.get(-rightLaneIndex);
666
667 List<CrossSectionSlice> crossSectionSlices = new ArrayList<CrossSectionSlice>();
668 if (rightLane.widthTags.size() == 1)
669 {
670 rightLane.widthTags.get(0).sOffst =
671 rightLane.widthTags.get(0).a.plus(rightLane.widthTags.get(0).b.multiplyBy(ds.doubleValue()))
672 .plus(rightLane.widthTags.get(0).c.multiplyBy(Math.pow(ds.doubleValue(), 2)))
673 .plus(rightLane.widthTags.get(0).d.multiplyBy(Math.pow(ds.doubleValue(), 3)));
674
675 Length laneWidth_start = rightLane.widthTags.get(0).a;
676 Length laneWidth_end = rightLane.widthTags.get(0).sOffst;
677
678 Length leftOffset_start = lastLane.getDesignLineOffsetAtBegin()
679 .minus(lastLane.getBeginWidth().multiplyBy(0.5)).minus(laneWidth_start.multiplyBy(0.5));
680 Length leftOffset_end = lastLane.getDesignLineOffsetAtEnd().minus(lastLane.getEndWidth().multiplyBy(0.5))
681 .minus(laneWidth_end.multiplyBy(0.5));
682
683 Length length = currentLink.getLength();
684
685 CrossSectionSlice startSlice =
686 new CrossSectionSlice(new Length(0.0, LengthUnit.METER), leftOffset_start, laneWidth_start);
687 CrossSectionSlice endSlice = new CrossSectionSlice(length, leftOffset_end, laneWidth_end);
688 crossSectionSlices.add(startSlice);
689 crossSectionSlices.add(endSlice);
690
691 }
692 else
693 {
694
695
696 Length lengthofLane = rightLane.widthTags.get(rightLane.widthTags.size() - 1).sOffst;
697 for (WidthTag widthTag : rightLane.widthTags)
698 {
699 Length relativeLength = widthTag.sOffst;
700 double factor = relativeLength.divideBy(lengthofLane).doubleValue();
701
702 if (factor < 0.98)
703 {
704 Length width = widthTag.a.plus(widthTag.b.multiplyBy(relativeLength.doubleValue()))
705 .plus(widthTag.c.multiplyBy(Math.pow(relativeLength.doubleValue(), 2)))
706 .plus(widthTag.d.multiplyBy(Math.pow(relativeLength.doubleValue(), 3)));
707
708 Length offSet = lastLane.getLateralCenterPosition(factor)
709 .minus(lastLane.getWidth(factor).multiplyBy(0.5)).minus(width.multiplyBy(0.5));
710
711 relativeLength = currentLink.getLength().multiplyBy(factor);
712
713 CrossSectionSlice slice = new CrossSectionSlice(relativeLength, offSet, width);
714 crossSectionSlices.add(slice);
715 }
716 else
717 {
718 CrossSectionSlice lastSlice = crossSectionSlices.get(crossSectionSlices.size() - 1);
719 Length width = lastSlice.getWidth();
720 Length offSet = lastSlice.getDesignLineOffset();
721 relativeLength = currentLink.getLength();
722
723 CrossSectionSlice slice = new CrossSectionSlice(relativeLength, offSet, width);
724 crossSectionSlices.add(slice);
725 break;
726 }
727
728 }
729 }
730
731 Speed speed = null;
732 if (rightLane.speedTags.size() > 0)
733 speed = rightLane.speedTags.get(0).max;
734 if (speed == null)
735 {
736
737 speed = new Speed(30.0, SpeedUnit.MILE_PER_HOUR);
738 }
739
740 Map<GTUType, Speed> speedLimit = new LinkedHashMap<>();
741 speedLimit.put(openDriveNetworkLaneParser.network.getGtuType(GTUType.DEFAULTS.VEHICLE), speed);
742
743 if (rightLane.type.equals("driving"))
744 {
745 LongitudinalDirectionality direction = LongitudinalDirectionality.DIR_PLUS;
746
747
748 Map<GTUType, LongitudinalDirectionality> directionality = new LinkedHashMap<>();
749 directionality.put(openDriveNetworkLaneParser.network.getGtuType(GTUType.DEFAULTS.VEHICLE), direction);
750 Color color = Color.gray;
751
752 try
753 {
754
755
756
757 Lane lane = new Lane(currentLink, rightLane.id.toString(), crossSectionSlices,
758 openDriveNetworkLaneParser.network.getLaneType(LaneType.DEFAULTS.FREEWAY), speedLimit);
759
760 currentLaneSec.lanes.put(rightLane.id, lane);
761
762 lastLane = lane;
763
764 Renderable2D animation = new LaneAnimationOD(lane, simulator, color);
765 openDriveNetworkLaneParser.animationMap.put(lane, animation);
766 }
767 catch (Exception exception)
768 {
769 Renderable2D animation = new LinkAnimation(currentLink, simulator, 0.01f);
770 openDriveNetworkLaneParser.animationMap.put(currentLink, animation);
771 exception.printStackTrace();
772 }
773 }
774 else if (rightLane.type.equals("sidewalk"))
775 {
776 Color color = Color.darkGray;
777 Lane lane = new NoTrafficLane(currentLink, rightLane.id.toString(), crossSectionSlices);
778
779 currentLaneSec.lanes.put(rightLane.id, lane);
780
781 lastLane = lane;
782
783 try
784 {
785 Renderable2D animation = new LaneAnimation(lane, simulator, color, false);
786 openDriveNetworkLaneParser.animationMap.put(lane, animation);
787 }
788 catch (RemoteException exception)
789 {
790 exception.printStackTrace();
791 }
792 }
793 else if (rightLane.type.equals("border"))
794 {
795 Stripe solidLine = new Stripe(currentLink, crossSectionSlices, Permeable.BOTH);
796
797 lastLane = solidLine;
798 try
799 {
800 Renderable2D animation = new StripeAnimation(solidLine, simulator, StripeAnimation.TYPE.SOLID);
801 openDriveNetworkLaneParser.animationMap.put(solidLine, animation);
802 }
803 catch (RemoteException exception)
804 {
805 exception.printStackTrace();
806 }
807 }
808 else if (rightLane.type.equals("shoulder"))
809 {
810 Color color = Color.green;
811 Shoulder shoulder = new Shoulder(currentLink, rightLane.id.toString(), crossSectionSlices);
812 lastLane = shoulder;
813
814 try
815 {
816 Renderable2D animation = new ShoulderAnimation(shoulder, simulator, color);
817 openDriveNetworkLaneParser.animationMap.put(shoulder, animation);
818 }
819 catch (RemoteException exception)
820 {
821 exception.printStackTrace();
822 }
823 }
824 else
825 {
826 Color color = Color.green;
827
828 try
829 {
830 Lane lane = new NoTrafficLane(currentLink, rightLane.id.toString(), crossSectionSlices);
831
832 currentLaneSec.lanes.put(rightLane.id, lane);
833 lastLane = lane;
834 Renderable2D animation = new LaneAnimation(lane, simulator, color, false);
835 openDriveNetworkLaneParser.animationMap.put(lane, animation);
836 }
837 catch (Exception exception)
838 {
839 Renderable2D animation = new LinkAnimation(currentLink, simulator, 0.01f);
840 openDriveNetworkLaneParser.animationMap.put(currentLink, animation);
841 exception.printStackTrace();
842 }
843 }
844
845 }
846
847 }
848 }
849
850
851
852
853
854
855
856 public static void buildLink(RoadTag roadTag, OpenDriveNetworkLaneParser openDriveNetworkLaneParser) throws NetworkException
857 {
858 if (roadTag.junctionId == null)
859 System.out.println("sth is wrong in building links");
860
861 if (!roadTag.junctionId.equals("-1"))
862 {
863 RoadTag predecessorRoadTag = openDriveNetworkLaneParser.roadTags.get(roadTag.linkTag.predecessorId);
864 RoadTag successorRoadTag = openDriveNetworkLaneParser.roadTags.get(roadTag.linkTag.successorId);
865
866 OTSNode from = null;
867
868 if (roadTag.linkTag.predecessorContactPoint.equals(ContactPointEnum.START))
869 from = predecessorRoadTag.startNode;
870 else if (roadTag.linkTag.predecessorContactPoint.equals(ContactPointEnum.END))
871 from = predecessorRoadTag.endNode;
872 else
873 System.out.println("sth is wrong in building links");
874
875 OTSNode to = null;
876
877 if (roadTag.linkTag.successorContactPoint.equals(ContactPointEnum.START))
878 to = successorRoadTag.startNode;
879 else if (roadTag.linkTag.successorContactPoint.equals(ContactPointEnum.END))
880 to = successorRoadTag.endNode;
881 else
882 System.out.println("sth is wrong in building links");
883
884 roadTag.startNode = from;
885 roadTag.endNode = to;
886
887 CrossSectionLink newlink = new CrossSectionLink(openDriveNetworkLaneParser.network, roadTag.id, from, to,
888 openDriveNetworkLaneParser.network.getLinkType(LinkType.DEFAULTS.ROAD), roadTag.designLine,
889 openDriveNetworkLaneParser.simulator, LaneKeepingPolicy.KEEPLANE);
890 roadTag.link = newlink;
891
892 roadTag.link = newlink;
893
894 }
895 else
896 {
897 OTSNode from = roadTag.startNode;
898 OTSNode to = roadTag.endNode;
899 CrossSectionLink newlink = new CrossSectionLink(openDriveNetworkLaneParser.network, roadTag.id, from, to,
900 openDriveNetworkLaneParser.network.getLinkType(LinkType.DEFAULTS.ROAD), roadTag.designLine,
901 openDriveNetworkLaneParser.simulator, LaneKeepingPolicy.KEEPLANE);
902
903 roadTag.link = newlink;
904 }
905 }
906
907
908
909
910
911
912
913 public static void generateTrafficLightsbySignal(RoadTag roadTag, DEVSSimulatorInterface.TimeDoubleUnit simulator,
914 OpenDriveNetworkLaneParser openDriveNetworkLaneParser) throws NetworkException
915 {
916 for (SignalTag signalTag : roadTag.signalsTag.signalTags)
917 {
918
919
920
921
922 LaneSectionTag laneSec = roadTag.lanesTag.findDrivingLaneSec(signalTag.s);
923 Lane lane = laneSec.findLanes(signalTag.orientation).get(0);
924
925 if (signalTag.type.equals("1000001") && signalTag.dynamic.equals("yes"))
926 {
927 try
928 {
929
930 Length sOffset = null;
931
932 if (!openDriveNetworkLaneParser.trafficLightsByLanes.containsKey(roadTag.id))
933 sOffset = signalTag.s.minus(laneSec.s);
934 else
935 sOffset = signalTag.s.minus(laneSec.s).plus(new Length(0.5, LengthUnit.METER));
936
937 Class<?> clazz = Class.forName(SimpleTrafficLight.class.getName());
938 Constructor<?> trafficLightConstructor = ClassUtil.resolveConstructor(clazz,
939 new Class[] {String.class, Lane.class, Length.class, DEVSSimulatorInterface.TimeDoubleUnit.class});
940
941 SimpleTrafficLight trafficLight = (SimpleTrafficLight) trafficLightConstructor
942 .newInstance(new Object[] {signalTag.id, lane, sOffset, simulator});
943
944 if (!openDriveNetworkLaneParser.trafficLightsBySignals.containsKey(signalTag.id))
945 {
946 Set<SimpleTrafficLight> lights = new HashSet<SimpleTrafficLight>();
947 openDriveNetworkLaneParser.trafficLightsBySignals.put(signalTag.id, lights);
948 }
949
950 if (!openDriveNetworkLaneParser.trafficLightsByLanes.containsKey(roadTag.id))
951 {
952 Set<SimpleTrafficLight> lights = new HashSet<SimpleTrafficLight>();
953 openDriveNetworkLaneParser.trafficLightsByLanes.put(roadTag.id, lights);
954 }
955
956 openDriveNetworkLaneParser.trafficLightsBySignals.get(signalTag.id).add(trafficLight);
957 openDriveNetworkLaneParser.trafficLightsByLanes.get(roadTag.id).add(trafficLight);
958
959 }
960 catch (ClassNotFoundException | NoSuchMethodException | InstantiationException | IllegalAccessException
961 | IllegalArgumentException | InvocationTargetException exception)
962 {
963 throw new NetworkException(
964 "Traffic Light: CLASS NAME " + SimpleTrafficLight.class.getName() + " for " + signalTag.id
965 + " on lane " + lane.toString() + " -- class not found or constructor not right",
966 exception);
967 }
968 }
969 else if (signalTag.type.equals("206") && signalTag.dynamic.equals("no"))
970 {
971
972 }
973 else
974 System.err.println("Unknown signals");
975 }
976 }
977
978
979
980
981
982
983
984 public static void generateTrafficLightsbySignalReference(RoadTag roadTag, DEVSSimulatorInterface.TimeDoubleUnit simulator,
985 OpenDriveNetworkLaneParser openDriveNetworkLaneParser) throws NetworkException
986 {
987 for (SignalReferenceTag signalReferenceTag : roadTag.signalsTag.signalReferenceTag)
988 {
989 LaneSectionTag laneSec = roadTag.lanesTag.findDrivingLaneSec(signalReferenceTag.s);
990 Lane lane = laneSec.findLanes(signalReferenceTag.orientation).get(0);
991
992 SignalTag signalTag = openDriveNetworkLaneParser.signalTags.get(signalReferenceTag.id);
993
994 if (signalTag.type.equals("1000001") && signalTag.dynamic.equals("yes"))
995 {
996 try
997 {
998 Length sOffset = null;
999
1000 if (!openDriveNetworkLaneParser.trafficLightsByLanes.containsKey(roadTag.id))
1001 sOffset = signalReferenceTag.s.minus(laneSec.s);
1002 else
1003 sOffset = signalReferenceTag.s.minus(laneSec.s).plus(new Length(0.5, LengthUnit.METER));
1004
1005 Class<?> clazz = Class.forName(SimpleTrafficLight.class.getName());
1006 Constructor<?> trafficLightConstructor = ClassUtil.resolveConstructor(clazz,
1007 new Class[] {String.class, Lane.class, Length.class, DEVSSimulatorInterface.TimeDoubleUnit.class});
1008
1009 SimpleTrafficLight trafficLight = (SimpleTrafficLight) trafficLightConstructor
1010 .newInstance(new Object[] {signalTag.id + ".ref", lane, sOffset, simulator});
1011
1012 if (!openDriveNetworkLaneParser.trafficLightsByLanes.containsKey(roadTag.id))
1013 {
1014 Set<SimpleTrafficLight> lights = new HashSet<SimpleTrafficLight>();
1015 openDriveNetworkLaneParser.trafficLightsByLanes.put(roadTag.id, lights);
1016 }
1017
1018
1019 openDriveNetworkLaneParser.trafficLightsBySignals.get(signalTag.id).add(trafficLight);
1020 openDriveNetworkLaneParser.trafficLightsByLanes.get(roadTag.id).add(trafficLight);
1021
1022 }
1023 catch (ClassNotFoundException | NoSuchMethodException | InstantiationException | IllegalAccessException
1024 | IllegalArgumentException | InvocationTargetException exception)
1025 {
1026 throw new NetworkException(
1027 "Traffic Light: CLASS NAME " + SimpleTrafficLight.class.getName() + " for " + signalTag.id
1028 + " on lane " + lane.toString() + " -- class not found or constructor not right",
1029 exception);
1030 }
1031 }
1032 else if (signalTag.type.equals("206") && signalTag.dynamic.equals("no"))
1033 {
1034
1035 }
1036 else
1037 System.err.println("Unknown signal references");
1038 }
1039 }
1040
1041
1042 @Override
1043 public final String toString()
1044 {
1045 return "RoadTag [id=" + this.id + ", name=" + this.name + ", length=" + this.length + ", junctionId=" + this.junctionId
1046 + ", linkTag=" + this.linkTag + ", planViewTag=" + this.planViewTag + ", elevationProfileTag="
1047 + this.elevationProfileTag + ", lateralProfileTag=" + this.lateralProfileTag + ", lanesTag=" + this.lanesTag
1048 + ", signalsTag=" + this.signalsTag + ", objectsTag=" + this.objectsTag + ", typeTag=" + this.typeTag
1049 + ", link=" + this.link + ", designLine=" + this.designLine + ", startNode=" + this.startNode + ", endNode="
1050 + this.endNode + ", subLinks=" + this.subLinks + "]";
1051 }
1052
1053 }