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.changing.OvertakingConditions;
48 import org.opentrafficsim.road.network.lane.object.trafficlight.SimpleTrafficLight;
49 import org.w3c.dom.NamedNodeMap;
50 import org.w3c.dom.Node;
51 import org.xml.sax.SAXException;
52
53 import nl.tudelft.simulation.dsol.animation.D2.Renderable2D;
54 import nl.tudelft.simulation.dsol.simulators.DEVSSimulatorInterface;
55
56
57
58
59
60
61
62
63
64
65 class RoadTag implements Serializable
66 {
67
68 private static final long serialVersionUID = 20150723L;
69
70
71 @SuppressWarnings("checkstyle:visibilitymodifier")
72 String id = null;
73
74
75 @SuppressWarnings("checkstyle:visibilitymodifier")
76 String name = null;
77
78
79 @SuppressWarnings("checkstyle:visibilitymodifier")
80 Length length = null;
81
82
83 @SuppressWarnings("checkstyle:visibilitymodifier")
84 String junctionId = null;
85
86
87 @SuppressWarnings("checkstyle:visibilitymodifier")
88 LinkTag linkTag = null;
89
90
91 @SuppressWarnings("checkstyle:visibilitymodifier")
92 PlanViewTag planViewTag = null;
93
94
95 @SuppressWarnings("checkstyle:visibilitymodifier")
96 ElevationProfileTag elevationProfileTag = null;
97
98
99 @SuppressWarnings("checkstyle:visibilitymodifier")
100 LateralProfileTag lateralProfileTag = null;
101
102
103 @SuppressWarnings("checkstyle:visibilitymodifier")
104 LanesTag lanesTag = null;
105
106
107 @SuppressWarnings("checkstyle:visibilitymodifier")
108 SignalsTag signalsTag = null;
109
110
111 @SuppressWarnings("checkstyle:visibilitymodifier")
112 ObjectsTag objectsTag = null;
113
114
115 @SuppressWarnings("checkstyle:visibilitymodifier")
116 TypeTag typeTag = null;
117
118
119 @SuppressWarnings("checkstyle:visibilitymodifier")
120 CrossSectionLink link = null;
121
122
123 @SuppressWarnings("checkstyle:visibilitymodifier")
124 OTSLine3D designLine = null;
125
126
127 @SuppressWarnings("checkstyle:visibilitymodifier")
128 OTSNode startNode = null;
129
130
131 @SuppressWarnings("checkstyle:visibilitymodifier")
132 OTSNode endNode = null;
133
134
135 @SuppressWarnings("checkstyle:visibilitymodifier")
136 List<CrossSectionLink> subLinks = new ArrayList<>();
137
138
139
140
141
142
143
144
145
146 @SuppressWarnings("checkstyle:needbraces")
147 static RoadTag parseRoad(final Node node, final OpenDriveNetworkLaneParser parser) throws SAXException, NetworkException
148 {
149 NamedNodeMap attributes = node.getAttributes();
150 RoadTag roadTag = new RoadTag();
151
152 Node id = attributes.getNamedItem("id");
153 if (id == null)
154 throw new SAXException("ROAD: missing attribute ID");
155 roadTag.id = id.getNodeValue().trim();
156 if (parser.roadTags.keySet().contains(roadTag.id))
157 throw new SAXException("ROAD: ID " + roadTag.id + " defined twice");
158
159 Node name = attributes.getNamedItem("name");
160 if (name == null)
161 throw new SAXException("ROAD: missing attribute ID for road with ID=" + roadTag.id);
162 roadTag.name = name.getNodeValue().trim();
163
164 Node length = attributes.getNamedItem("length");
165 if (length == null)
166 throw new SAXException("ROAD: missing attribute LENGTH");
167 roadTag.length = new Length(Double.parseDouble(length.getNodeValue().trim()), LengthUnit.METER);
168
169 Node junctionId = attributes.getNamedItem("junction");
170 if (junctionId == null)
171 throw new SAXException("ROAD: missing attribute junction for road id=" + roadTag.id);
172
173 roadTag.junctionId = junctionId.getNodeValue().trim();
174
175
176
177
178
179
180
181
182
183 parser.roadTags.put(roadTag.id, roadTag);
184
185 return roadTag;
186 }
187
188
189
190
191
192
193
194
195
196 static void buildSubLinks(RoadTag roadTag, OTSSimulatorInterface simulator,
197 OpenDriveNetworkLaneParser openDriveNetworkLaneParser)
198 throws OTSGeometryException, NetworkException, NamingException
199 {
200 OTSNetwork otsNetwork = openDriveNetworkLaneParser.network;
201 if (roadTag.lanesTag.laneSectionTags.size() == 1)
202 {
203
204 {
205 roadTag.subLinks.add(roadTag.link);
206 if (!otsNetwork.containsNode(roadTag.link.getStartNode()))
207 otsNetwork.addNode(roadTag.link.getStartNode());
208 if (!otsNetwork.containsNode(roadTag.link.getEndNode()))
209 otsNetwork.addNode(roadTag.link.getEndNode());
210 if (!otsNetwork.containsLink(roadTag.link))
211 otsNetwork.addLink(roadTag.link);
212 }
213
214 }
215 else
216 {
217
218 List<GeometryTag> tempGeometryTags = new ArrayList<GeometryTag>();
219 tempGeometryTags = roadTag.planViewTag.geometryTags;
220
221
222
223
224 int currentIndex = 0;
225 for (Integer laneSecIndex = 1; laneSecIndex < roadTag.lanesTag.laneSectionTags.size(); laneSecIndex++)
226 {
227 LaneSectionTag laneSec = roadTag.lanesTag.laneSectionTags.get(laneSecIndex);
228 Length laneSecLength = laneSec.s;
229
230 List<OTSPoint3D> points = new ArrayList<OTSPoint3D>();
231
232 GeometryTag from = tempGeometryTags.get(currentIndex);
233 GeometryTag to = tempGeometryTags.get(currentIndex);
234
235 for (int indexGeometryTag = currentIndex; indexGeometryTag < tempGeometryTags.size(); indexGeometryTag++)
236 {
237 GeometryTag currentGeometryTag = tempGeometryTags.get(indexGeometryTag);
238 if (currentGeometryTag.s.doubleValue() < laneSecLength.doubleValue())
239 {
240 OTSPoint3D point = new OTSPoint3D(currentGeometryTag.x.doubleValue(),
241 currentGeometryTag.y.doubleValue(), currentGeometryTag.z.doubleValue());
242
243 if (points.size() == 0)
244 points.add(point);
245 else
246 {
247 if (point.x != points.get(points.size() - 1).x && point.y != points.get(points.size() - 1).y)
248 points.add(point);
249 }
250
251 OTSPoint3D lastPoint = new OTSPoint3D(points.get(points.size() - 1));
252
253 if (currentGeometryTag.interLine != null)
254 {
255 for (OTSPoint3D point1 : currentGeometryTag.interLine.getPoints())
256 {
257
258
259
260
261 if (lastPoint.x != point1.x && lastPoint.y != point1.y)
262 {
263 points.add(point1);
264 lastPoint = point1;
265 }
266 }
267 }
268
269 to = tempGeometryTags.get(currentIndex);
270 currentIndex++;
271 continue;
272 }
273 else
274 {
275 OTSPoint3D point = new OTSPoint3D(currentGeometryTag.x.doubleValue(),
276 currentGeometryTag.y.doubleValue(), currentGeometryTag.z.doubleValue());
277
278 if (points.size() == 0)
279 points.add(point);
280 else
281 {
282 if (point.x != points.get(points.size() - 1).x && point.y != points.get(points.size() - 1).y)
283 points.add(point);
284 }
285
286 OTSPoint3D lastPoint = new OTSPoint3D(points.get(points.size() - 1));
287
288 if (currentGeometryTag.interLine != null)
289 {
290 for (OTSPoint3D point1 : currentGeometryTag.interLine.getPoints())
291 {
292
293
294
295
296
297
298 if (lastPoint.x != point.x && lastPoint.y != point.y)
299 {
300 points.add(point1);
301 lastPoint = point1;
302 }
303 }
304 }
305
306
307 to = tempGeometryTags.get(currentIndex);
308
309
310 OTSLine3D designLine = new OTSLine3D(points);
311 String sublinkId = roadTag.id + "." + laneSecIndex.toString();
312 CrossSectionLink sublink = new CrossSectionLink(openDriveNetworkLaneParser.network, sublinkId,
313 from.node, to.node, LinkType.ROAD, designLine, simulator, LaneKeepingPolicy.KEEP_LANE);
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 LinkType.ROAD, designLine, simulator, LaneKeepingPolicy.KEEP_LANE);
382
383 roadTag.subLinks.add(sublink);
384
385 if (!otsNetwork.containsNode(from.node))
386 otsNetwork.addNode(from.node);
387 if (!otsNetwork.containsNode(to.node))
388 otsNetwork.addNode(to.node);
389
390 if (!otsNetwork.containsLink(sublink))
391 otsNetwork.addLink(sublink);
392 else
393 System.err.println("Sublink already registered: " + sublink);
394 }
395
396 }
397
398
399
400
401
402
403
404
405
406
407 static void generateRegularRoads(RoadTag roadTag, DEVSSimulatorInterface.TimeDoubleUnit simulator,
408 OpenDriveNetworkLaneParser openDriveNetworkLaneParser)
409 throws OTSGeometryException, NetworkException, NamingException, RemoteException
410 {
411
412 for (int laneSecIndex = 0; laneSecIndex < roadTag.lanesTag.laneSectionTags.size(); laneSecIndex++)
413 {
414 LaneSectionTag currentLaneSec = roadTag.lanesTag.laneSectionTags.get(laneSecIndex);
415
416 CrossSectionLink currentLink = roadTag.subLinks.get(laneSecIndex);
417
418 Length ds = new Length(0.0, LengthUnit.METER);
419 LaneSectionTag nextLaneSec;
420 if (laneSecIndex != roadTag.lanesTag.laneSectionTags.size() - 1)
421 {
422 nextLaneSec = roadTag.lanesTag.laneSectionTags.get(laneSecIndex + 1);
423 ds = nextLaneSec.s.minus(currentLaneSec.s);
424 }
425 else
426 {
427 ds = roadTag.length.minus(currentLaneSec.s);
428 }
429
430 CrossSectionElement lastLane = null;
431
432
433 int centerLaneSize = currentLaneSec.centerLaneTags.size();
434 if (centerLaneSize != 1)
435 System.err.println("Sth is wrong in center lane");
436 Length centerOffset = new Length(0.0, LengthUnit.METER);
437
438 LaneTag centerLane = currentLaneSec.centerLaneTags.get(0);
439 Length laneWidth = new Length(0.0, LengthUnit.METER);
440 if (centerLane.widthTags.size() != 0)
441 System.err.println("error in show center stripe!");
442
443 Stripe centerStripe = new Stripe(currentLink, centerOffset, centerOffset, laneWidth);
444 try
445 {
446 Renderable2D animation = new StripeAnimation(centerStripe, simulator, StripeAnimation.TYPE.SOLID);
447 openDriveNetworkLaneParser.animationMap.put(centerStripe, animation);
448 }
449 catch (RemoteException exception)
450 {
451 exception.printStackTrace();
452 }
453
454 lastLane = centerStripe;
455
456
457 int leftLaneSize = currentLaneSec.leftLaneTags.size();
458
459
460
461 for (int leftLaneIndex = 1; leftLaneIndex <= leftLaneSize; leftLaneIndex++)
462 {
463 LaneTag leftLane = currentLaneSec.leftLaneTags.get(leftLaneIndex);
464
465 List<CrossSectionSlice> crossSectionSlices = new ArrayList<CrossSectionSlice>();
466 if (leftLane.widthTags.size() == 1)
467 {
468 leftLane.widthTags.get(0).sOffst =
469 leftLane.widthTags.get(0).a.plus(leftLane.widthTags.get(0).b.multiplyBy(ds.doubleValue()))
470 .plus(leftLane.widthTags.get(0).c.multiplyBy(Math.pow(ds.doubleValue(), 2)))
471 .plus(leftLane.widthTags.get(0).d.multiplyBy(Math.pow(ds.doubleValue(), 3)));
472
473 Length laneWidth_start = leftLane.widthTags.get(0).a;
474 Length laneWidth_end = leftLane.widthTags.get(0).sOffst;
475
476 Length leftOffset_start = lastLane.getDesignLineOffsetAtBegin()
477 .plus(lastLane.getBeginWidth().multiplyBy(0.5)).plus(laneWidth_start.multiplyBy(0.5));
478 Length leftOffset_end = lastLane.getDesignLineOffsetAtEnd().plus(lastLane.getEndWidth().multiplyBy(0.5))
479 .plus(laneWidth_end.multiplyBy(0.5));
480
481 Length length = currentLink.getLength();
482
483 CrossSectionSlice startSlice =
484 new CrossSectionSlice(new Length(0.0, LengthUnit.METER), leftOffset_start, laneWidth_start);
485 CrossSectionSlice endSlice = new CrossSectionSlice(length, leftOffset_end, laneWidth_end);
486 crossSectionSlices.add(startSlice);
487 crossSectionSlices.add(endSlice);
488
489 }
490 else
491 {
492
493
494 Length lengthofLane = leftLane.widthTags.get(leftLane.widthTags.size() - 1).sOffst;
495 for (WidthTag widthTag : leftLane.widthTags)
496 {
497 Length relativeLength = widthTag.sOffst;
498 double factor = relativeLength.divideBy(lengthofLane).doubleValue();
499
500 if (factor < 0.98)
501 {
502 Length width = widthTag.a.plus(widthTag.b.multiplyBy(relativeLength.doubleValue()))
503 .plus(widthTag.c.multiplyBy(Math.pow(relativeLength.doubleValue(), 2)))
504 .plus(widthTag.d.multiplyBy(Math.pow(relativeLength.doubleValue(), 3)));
505
506 Length offSet = lastLane.getLateralCenterPosition(factor)
507 .plus(lastLane.getWidth(factor).multiplyBy(0.5)).plus(width.multiplyBy(0.5));
508
509 relativeLength = currentLink.getLength().multiplyBy(factor);
510
511 CrossSectionSlice slice = new CrossSectionSlice(relativeLength, offSet, width);
512 crossSectionSlices.add(slice);
513 }
514 else
515 {
516 CrossSectionSlice lastSlice = crossSectionSlices.get(crossSectionSlices.size() - 1);
517 Length width = lastSlice.getWidth();
518 Length offSet = lastSlice.getDesignLineOffset();
519 relativeLength = currentLink.getLength();
520
521 CrossSectionSlice slice = new CrossSectionSlice(relativeLength, offSet, width);
522 crossSectionSlices.add(slice);
523 break;
524 }
525 }
526 }
527
528 OvertakingConditions overtakingConditions = null;
529
530 Speed speed = null;
531 if (leftLane.speedTags.size() > 0)
532 speed = leftLane.speedTags.get(0).max;
533 if (speed == null)
534 {
535
536 speed = new Speed(30.0, SpeedUnit.MILE_PER_HOUR);
537 }
538
539 Map<GTUType, Speed> speedLimit = new LinkedHashMap<>();
540 speedLimit.put(GTUType.VEHICLE, speed);
541
542 if (leftLane.type.equals("driving"))
543 {
544 LongitudinalDirectionality direction = LongitudinalDirectionality.DIR_MINUS;
545 Color color = Color.gray;
546
547
548
549
550
551
552 Lane lane = new Lane(currentLink, leftLane.id.toString(), crossSectionSlices, LaneType.FREEWAY, speedLimit,
553 overtakingConditions);
554 currentLaneSec.lanes.put(leftLane.id, lane);
555
556 lastLane = lane;
557
558 try
559 {
560 Renderable2D animation = new LaneAnimationOD(lane, simulator, color);
561 openDriveNetworkLaneParser.animationMap.put(lane, animation);
562 }
563 catch (RemoteException exception)
564 {
565 Renderable2D animation = 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 OvertakingConditions overtakingConditions = null;
732
733 Speed speed = null;
734 if (rightLane.speedTags.size() > 0)
735 speed = rightLane.speedTags.get(0).max;
736 if (speed == null)
737 {
738
739 speed = new Speed(30.0, SpeedUnit.MILE_PER_HOUR);
740 }
741
742 Map<GTUType, Speed> speedLimit = new LinkedHashMap<>();
743 speedLimit.put(GTUType.VEHICLE, speed);
744
745 if (rightLane.type.equals("driving"))
746 {
747 LongitudinalDirectionality direction = LongitudinalDirectionality.DIR_PLUS;
748
749
750 Map<GTUType, LongitudinalDirectionality> directionality = new LinkedHashMap<>();
751 directionality.put(GTUType.VEHICLE, direction);
752 Color color = Color.gray;
753
754 try
755 {
756
757
758
759 Lane lane = new Lane(currentLink, rightLane.id.toString(), crossSectionSlices, LaneType.FREEWAY,
760 speedLimit, overtakingConditions);
761
762 currentLaneSec.lanes.put(rightLane.id, lane);
763
764 lastLane = lane;
765
766 Renderable2D animation = new LaneAnimationOD(lane, simulator, color);
767 openDriveNetworkLaneParser.animationMap.put(lane, animation);
768 }
769 catch (Exception exception)
770 {
771 Renderable2D animation = new LinkAnimation(currentLink, simulator, 0.01f);
772 openDriveNetworkLaneParser.animationMap.put(currentLink, animation);
773 exception.printStackTrace();
774 }
775 }
776 else if (rightLane.type.equals("sidewalk"))
777 {
778 Color color = Color.darkGray;
779 Lane lane = new NoTrafficLane(currentLink, rightLane.id.toString(), crossSectionSlices);
780
781 currentLaneSec.lanes.put(rightLane.id, lane);
782
783 lastLane = lane;
784
785 try
786 {
787 Renderable2D animation = new LaneAnimation(lane, simulator, color, false);
788 openDriveNetworkLaneParser.animationMap.put(lane, animation);
789 }
790 catch (RemoteException exception)
791 {
792 exception.printStackTrace();
793 }
794 }
795 else if (rightLane.type.equals("border"))
796 {
797 Stripe solidLine = new Stripe(currentLink, crossSectionSlices, Permeable.BOTH);
798
799 lastLane = solidLine;
800 try
801 {
802 Renderable2D animation = new StripeAnimation(solidLine, simulator, StripeAnimation.TYPE.SOLID);
803 openDriveNetworkLaneParser.animationMap.put(solidLine, animation);
804 }
805 catch (RemoteException exception)
806 {
807 exception.printStackTrace();
808 }
809 }
810 else if (rightLane.type.equals("shoulder"))
811 {
812 Color color = Color.green;
813 Shoulder shoulder = new Shoulder(currentLink, rightLane.id.toString(), crossSectionSlices);
814 lastLane = shoulder;
815
816 try
817 {
818 Renderable2D animation = new ShoulderAnimation(shoulder, simulator, color);
819 openDriveNetworkLaneParser.animationMap.put(shoulder, animation);
820 }
821 catch (RemoteException exception)
822 {
823 exception.printStackTrace();
824 }
825 }
826 else
827 {
828 Color color = Color.green;
829
830 try
831 {
832 Lane lane = new NoTrafficLane(currentLink, rightLane.id.toString(), crossSectionSlices);
833
834 currentLaneSec.lanes.put(rightLane.id, lane);
835 lastLane = lane;
836 Renderable2D animation = new LaneAnimation(lane, simulator, color, false);
837 openDriveNetworkLaneParser.animationMap.put(lane, animation);
838 }
839 catch (Exception exception)
840 {
841 Renderable2D animation = new LinkAnimation(currentLink, simulator, 0.01f);
842 openDriveNetworkLaneParser.animationMap.put(currentLink, animation);
843 exception.printStackTrace();
844 }
845 }
846
847 }
848
849 }
850 }
851
852
853
854
855
856
857
858 public static void buildLink(RoadTag roadTag, OpenDriveNetworkLaneParser openDriveNetworkLaneParser) throws NetworkException
859 {
860 if (roadTag.junctionId == null)
861 System.out.println("sth is wrong in building links");
862
863 if (!roadTag.junctionId.equals("-1"))
864 {
865 RoadTag predecessorRoadTag = openDriveNetworkLaneParser.roadTags.get(roadTag.linkTag.predecessorId);
866 RoadTag successorRoadTag = openDriveNetworkLaneParser.roadTags.get(roadTag.linkTag.successorId);
867
868 OTSNode from = null;
869
870 if (roadTag.linkTag.predecessorContactPoint.equals(ContactPointEnum.START))
871 from = predecessorRoadTag.startNode;
872 else if (roadTag.linkTag.predecessorContactPoint.equals(ContactPointEnum.END))
873 from = predecessorRoadTag.endNode;
874 else
875 System.out.println("sth is wrong in building links");
876
877 OTSNode to = null;
878
879 if (roadTag.linkTag.successorContactPoint.equals(ContactPointEnum.START))
880 to = successorRoadTag.startNode;
881 else if (roadTag.linkTag.successorContactPoint.equals(ContactPointEnum.END))
882 to = successorRoadTag.endNode;
883 else
884 System.out.println("sth is wrong in building links");
885
886 roadTag.startNode = from;
887 roadTag.endNode = to;
888
889 CrossSectionLink newlink = new CrossSectionLink(openDriveNetworkLaneParser.network, roadTag.id, from, to,
890 LinkType.ROAD, roadTag.designLine, openDriveNetworkLaneParser.simulator, LaneKeepingPolicy.KEEP_LANE);
891 roadTag.link = newlink;
892
893 roadTag.link = newlink;
894
895 }
896 else
897 {
898 OTSNode from = roadTag.startNode;
899 OTSNode to = roadTag.endNode;
900 CrossSectionLink newlink = new CrossSectionLink(openDriveNetworkLaneParser.network, roadTag.id, from, to,
901 LinkType.ROAD, roadTag.designLine, openDriveNetworkLaneParser.simulator, LaneKeepingPolicy.KEEP_LANE);
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, new Class[] { String.class,
939 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, new Class[] { String.class,
1007 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 }