Eclipse SUMO - Simulation of Urban MObility
NIImporter_OpenDrive.cpp
Go to the documentation of this file.
1/****************************************************************************/
2// Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.org/sumo
3// Copyright (C) 2001-2022 German Aerospace Center (DLR) and others.
4// This program and the accompanying materials are made available under the
5// terms of the Eclipse Public License 2.0 which is available at
6// https://www.eclipse.org/legal/epl-2.0/
7// This Source Code may also be made available under the following Secondary
8// Licenses when the conditions for such availability set forth in the Eclipse
9// Public License 2.0 are satisfied: GNU General Public License, version 2
10// or later which is available at
11// https://www.gnu.org/licenses/old-licenses/gpl-2.0-standalone.html
12// SPDX-License-Identifier: EPL-2.0 OR GPL-2.0-or-later
13/****************************************************************************/
21// Importer for networks stored in openDrive format
22/****************************************************************************/
23#include <config.h>
24#include <string>
25#include <cmath>
26#include <iterator>
36#include <netbuild/NBEdge.h>
37#include <netbuild/NBEdgeCont.h>
38#include <netbuild/NBNode.h>
39#include <netbuild/NBNodeCont.h>
41#include <netbuild/NBOwnTLDef.h>
49#include <utils/xml/XMLSubSys.h>
50#include <utils/geom/Boundary.h>
51#include "NILoader.h"
53
54//#define DEBUG_VARIABLE_WIDTHS
55//#define DEBUG_VARIABLE_SPEED
56//#define DEBUG_CONNECTIONS
57//#define DEBUG_SPIRAL
58//#define DEBUG_INTERNALSHAPES
59//#define DEBUG_SHAPE
60
61#define DEBUG_COND(road) ((road)->id == "32")
62#define DEBUG_COND2(edgeID) (StringUtils::startsWith((edgeID), "67"))
63#define DEBUG_COND3(roadID) (roadID == "32")
64
65// ===========================================================================
66// definitions
67// ===========================================================================
68
69// ===========================================================================
70// static variables
71// ===========================================================================
102
104};
105
106
157 // towards xodr v1.4 speed:unit
159
161};
162
163
168
169// ===========================================================================
170// method definitions
171// ===========================================================================
172// ---------------------------------------------------------------------------
173// static methods (interface in this case)
174// ---------------------------------------------------------------------------
175void
177 // check whether the option is set (properly)
178 if (!oc.isUsableFileList("opendrive-files")) {
179 return;
180 }
181 // prepare types
182 myImportAllTypes = oc.getBool("opendrive.import-all-lanes");
183 myImportWidths = !oc.getBool("opendrive.ignore-widths");
184 myMinWidth = oc.getFloat("opendrive.min-width");
185 myImportInternalShapes = oc.getBool("opendrive.internal-shapes");
186 bool customLaneShapes = oc.getBool("opendrive.lane-shapes");
187 NBTypeCont& tc = nb.getTypeCont();
188 NBNodeCont& nc = nb.getNodeCont();
189 // build the handler
190 std::map<std::string, OpenDriveEdge*> edges;
191 NIImporter_OpenDrive handler(nb.getTypeCont(), edges);
192 handler.needsCharacterData();
193 // parse file(s)
194 std::vector<std::string> files = oc.getStringVector("opendrive-files");
195 for (std::vector<std::string>::const_iterator file = files.begin(); file != files.end(); ++file) {
196 if (!FileHelpers::isReadable(*file)) {
197 WRITE_ERROR("Could not open opendrive file '" + *file + "'.");
198 return;
199 }
200 handler.setFileName(*file);
201 PROGRESS_BEGIN_MESSAGE("Parsing opendrive from '" + *file + "'");
202 XMLSubSys::runParser(handler, *file);
204 }
205 // apply signal reference information
206 for (auto& item : edges) {
207 for (OpenDriveSignal& signal : item.second->signals) {
208 if (signal.type == "") {
209 if (handler.getSignals().count(signal.id) == 0) {
210 WRITE_WARNING("Could not find signal reference '" + signal.id + "'.");
211 } else {
212 const OpenDriveSignal& ref = handler.getSignals()[signal.id];
213 signal.type = ref.type;
214 signal.name = ref.name;
215 signal.dynamic = ref.dynamic;
216 }
217 }
218 }
219 }
220
221 // split inner/outer edges
222 std::map<std::string, OpenDriveEdge*> innerEdges, outerEdges;
223 for (std::map<std::string, OpenDriveEdge*>::iterator i = edges.begin(); i != edges.end(); ++i) {
224 if ((*i).second->isInner) {
225 innerEdges[(*i).first] = (*i).second;
226 } else {
227 outerEdges[(*i).first] = (*i).second;
228 }
229 }
230
231 // convert geometries into a discretised representation
232 computeShapes(edges);
233 // check whether lane sections are valid and whether further must be introduced
234 revisitLaneSections(tc, edges);
235
236 // -------------------------
237 // node building
238 // -------------------------
239 // build nodes#1
240 // look at all links which belong to a node, collect their bounding boxes
241 // and place the node in the middle of this bounding box
242 std::map<std::string, Boundary> posMap;
243 std::map<std::string, std::string> edge2junction;
244 std::vector<NodeSet> joinedNodeIDs;
245 // compute node positions
246 for (std::map<std::string, OpenDriveEdge*>::iterator i = innerEdges.begin(); i != innerEdges.end(); ++i) {
247 OpenDriveEdge* e = (*i).second;
248 assert(e->junction != "-1" && e->junction != "");
249 edge2junction[e->id] = e->junction;
250 if (posMap.find(e->junction) == posMap.end()) {
251 posMap[e->junction] = Boundary();
252 }
253 posMap[e->junction].add(e->geom.getBoxBoundary());
254 }
255 // build nodes
256 for (std::map<std::string, Boundary>::iterator i = posMap.begin(); i != posMap.end(); ++i) {
257 //std::cout << " import node=" << (*i).first << " z=" << (*i).second.getCenter() << " boundary=" << (*i).second << "\n";
258 if (!nb.getNodeCont().insert((*i).first, (*i).second.getCenter())) {
259 throw ProcessError("Could not add node '" + (*i).first + "'.");
260 }
261 }
262 // assign built nodes
263 for (std::map<std::string, OpenDriveEdge*>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
264 OpenDriveEdge* e = (*i).second;
265 for (std::vector<OpenDriveLink>::iterator j = e->links.begin(); j != e->links.end(); ++j) {
266 OpenDriveLink& l = *j;
267 const std::string& nid = l.elementID;
269 if (nb.getNodeCont().retrieve(nid) == nullptr) {
270 // not yet seen, build (possibly a junction without connections)
271 Position pos = l.linkType == OPENDRIVE_LT_SUCCESSOR ? e->geom[-1] : e->geom[0];
272 if (!nb.getNodeCont().insert(nid, pos)) {
273 throw ProcessError("Could not build node '" + nid + "'.");
274 }
275 }
276 // set node information
277 setNodeSecure(nb.getNodeCont(), *e, l.elementID, l.linkType, joinedNodeIDs);
278 continue;
279 }
280 if (edge2junction.find(l.elementID) != edge2junction.end()) {
281 // set node information of an internal road
282 setNodeSecure(nb.getNodeCont(), *e, edge2junction[l.elementID], l.linkType, joinedNodeIDs);
283 continue;
284 }
285 }
286 }
287 // we should now have all nodes set for links which are not outer edge-to-outer edge links
288
289
290 // build nodes#2
291 // build nodes for all outer edge-to-outer edge connections
292 for (std::map<std::string, OpenDriveEdge*>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
293 OpenDriveEdge* e = (*i).second;
294 for (std::vector<OpenDriveLink>::iterator j = e->links.begin(); j != e->links.end(); ++j) {
295 OpenDriveLink& l = *j;
296 if (l.elementType != OPENDRIVE_ET_ROAD || edge2junction.find(l.elementID) != edge2junction.end()) {
297 // is a connection to an internal edge, or a node, skip
298 continue;
299 }
300 // we have a direct connection between to external edges
301 std::string id1 = e->id;
302 std::string id2 = l.elementID;
303 if (id1 < id2) {
304 std::swap(id1, id2);
305 }
306 std::string nid = id1 + "." + id2;
307 if (nb.getNodeCont().retrieve(nid) == nullptr) {
308 // not yet seen, build
309 Position pos = l.linkType == OPENDRIVE_LT_SUCCESSOR ? e->geom[-1] : e->geom[0];
310 if (!nb.getNodeCont().insert(nid, pos)) {
311 throw ProcessError("Could not build node '" + nid + "'.");
312 }
313 }
314 /* debug-stuff
315 else {
316 Position pos = l.linkType==OPENDRIVE_LT_SUCCESSOR ? e.geom[e.geom.size()-1] : e.geom[0];
317 cout << nid << " " << pos << " " << nb.getNodeCont().retrieve(nid)->getPosition() << endl;
318 }
319 */
320 setNodeSecure(nb.getNodeCont(), *e, nid, l.linkType, joinedNodeIDs);
321 }
322 }
323 // we should now have start/end nodes for all outer edge-to-outer edge connections
324
325
326 // build nodes#3
327 // assign further nodes generated from inner-edges
328 // these nodes have not been assigned earlier, because the connections are referenced in inner-edges
329 for (std::map<std::string, OpenDriveEdge*>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
330 OpenDriveEdge* e = (*i).second;
331 if (e->to != nullptr && e->from != nullptr) {
332 continue;
333 }
334 for (std::map<std::string, OpenDriveEdge*>::iterator j = innerEdges.begin(); j != innerEdges.end(); ++j) {
335 OpenDriveEdge* ie = (*j).second;
336 for (std::vector<OpenDriveLink>::iterator k = ie->links.begin(); k != ie->links.end(); ++k) {
337 OpenDriveLink& il = *k;
338 if (il.elementType != OPENDRIVE_ET_ROAD || il.elementID != e->id) {
339 // not conneted to the currently investigated outer edge
340 continue;
341 }
342 std::string nid = edge2junction[ie->id];
344 setNodeSecure(nb.getNodeCont(), *e, nid, OPENDRIVE_LT_PREDECESSOR, joinedNodeIDs);
345 } else {
346 setNodeSecure(nb.getNodeCont(), *e, nid, OPENDRIVE_LT_SUCCESSOR, joinedNodeIDs);
347 }
348 }
349 }
350
351 }
352
353 // build start/end nodes which were not defined previously
354 for (std::map<std::string, OpenDriveEdge*>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
355 OpenDriveEdge* e = (*i).second;
356 if ((e->from == nullptr || e->to == nullptr) && e->geom.size() == 0) {
357 continue;
358 }
359 if (e->from == nullptr) {
360 const std::string nid = e->id + ".begin";
361 e->from = getOrBuildNode(nid, e->geom.front(), nb.getNodeCont());
362 }
363 if (e->to == nullptr) {
364 const std::string nid = e->id + ".end";
365 e->to = getOrBuildNode(nid, e->geom.back(), nb.getNodeCont());
366 }
367 }
368
369 std::map<NBNode*, NBNode*> joinedNodes;
370 for (NodeSet& joined : joinedNodeIDs) {
371 Position joinedPos(0, 0);
372 for (NBNode* j : joined) {
373 joinedPos.add(j->getPosition());
374 }
375 joinedPos.mul(1. / (double)joined.size());
376 const std::string joinedID = nc.createClusterId(joined);
377 if (!nc.insert(joinedID, joinedPos)) {
378 throw ProcessError("Could not add node '" + joinedID + "'.");
379 }
380 NBNode* n = nc.retrieve(joinedID);
381 for (NBNode* j : joined) {
382 joinedNodes[j] = n;
383 }
384 }
385 for (std::map<std::string, OpenDriveEdge*>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
386 OpenDriveEdge* e = (*i).second;
387 if (joinedNodes.count(e->from) != 0) {
388 nc.extract(e->from, true);
389 e->from = joinedNodes[e->from];
390 }
391 if (joinedNodes.count(e->to) != 0) {
392 nc.extract(e->to, true);
393 e->to = joinedNodes[e->to];
394 }
395 }
396
397
398 // -------------------------
399 // edge building
400 // -------------------------
401 const double defaultSpeed = tc.getEdgeTypeSpeed("");
402 const bool saveOrigIDs = OptionsCont::getOptions().getBool("output.original-names");
403 const bool positionIDs = OptionsCont::getOptions().getBool("opendrive.position-ids");
404 // lane-id-map sumoEdge,sumoLaneIndex->odrLaneIndex
405 std::map<std::pair<NBEdge*, int>, int> laneIndexMap;
406 // build edges
407 for (std::map<std::string, OpenDriveEdge*>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
408 OpenDriveEdge* e = (*i).second;
409 if (e->geom.size() < 2) {
410 WRITE_WARNING("Ignoring road '" + e->id + "' without geometry.");
411 continue;
412 }
413 bool lanesBuilt = false;
414
415 // go along the lane sections, build a node in between of each pair
416
419
421 NBNode* sFrom = e->from;
422 NBNode* sTo = e->to;
423 int priorityR = e->getPriority(OPENDRIVE_TAG_RIGHT);
424 int priorityL = e->getPriority(OPENDRIVE_TAG_LEFT);
425 double sB = 0;
426 double sE = e->length;
427 // 0-length geometries are possible if only the inner points are represented
428 PositionVector geomWithOffset = e->geom;
429 if (e->laneOffsets.size() > 0) {
430 try {
431 geomWithOffset.move2sideCustom(e->laneOffsets);
432 //std::cout << " e=" << e->id << " offsets=" << e->laneOffsets << " geom=" << e->geom << " geom2=" << geomWithOffset << "\n";
433 } catch (InvalidArgument&) {
434 WRITE_WARNING("Could not apply laneOffsets for edge '" + e->id + "'");
435 }
436 }
437#ifdef DEBUG_SHAPE
438 if (DEBUG_COND3(e->id)) {
439 std::cout << " geomWithOffset=" << geomWithOffset << "\n";
440 }
441#endif
442 const double length2D = geomWithOffset.length2D();
443 double cF = length2D == 0 ? 1 : e->length / length2D;
444 NBEdge* prevRight = nullptr;
445 NBEdge* prevLeft = nullptr;
446
447 // starting at the same node as ending, and no lane sections?
448 if (sFrom == sTo && e->laneSections.size() == 1) {
449 // --> loop, split!
451 ls.s = e->length / 2.;
452 e->laneSections.push_back(ls);
453 WRITE_WARNING("Edge '" + e->id + "' has to be split as it connects same junctions.")
454 }
456 if (myMinWidth > 0) {
457 const double minDist = oc.getFloat("opendrive.curve-resolution");
458 splitMinWidths(e, tc, minDist);
459 }
460
461 // build along lane sections
462 int sectionIndex = 0;
463 for (std::vector<OpenDriveLaneSection>::iterator j = e->laneSections.begin(); j != e->laneSections.end(); ++j) {
464 // add internal node if needed
465 if (j == e->laneSections.end() - 1) {
466 sTo = e->to;
467 sE = e->length / cF;
468 } else {
469 double nextS = (j + 1)->s;
470 const std::string nodeID = e->id + (positionIDs ? "." + toString(nextS) : "#" + toString(sectionIndex + 1));
471 sTo = new NBNode(nodeID, geomWithOffset.positionAtOffset(nextS));
472 if (!nb.getNodeCont().insert(sTo)) {
473 throw ProcessError("Could not add node '" + sTo->getID() + "'.");
474 }
475 sE = nextS / cF;
476 }
477 PositionVector geom = geomWithOffset.getSubpart2D(sB, sE);
478 std::string id = e->id;
479 if (positionIDs) {
480 if (sFrom != e->from || sTo != e->to) {
481 id = id + "." + toString((*j).s);
482 } else if (e->laneSections.size() == 1) {
483 id = id + ".0.00";
484 }
485 } else if (e->laneSections.size() > 1) {
486 id = id + "#" + toString(sectionIndex++);
487 }
488#ifdef DEBUG_VARIABLE_WIDTHS
489 if (DEBUG_COND(e)) {
490 std::cout << " id=" << id << " sB=" << sB << " sE=" << sE << " geom=" << geom << "\n";
491 }
492#endif
493
494 // build lanes to right
495 NBEdge* currRight = nullptr;
496 if ((*j).rightLaneNumber > 0) {
497 std::vector<double> offsets(geom.size(), 0);
498 bool useOffsets = false;
499 PositionVector rightGeom = geom;
500#ifdef DEBUG_SHAPE
501 if (DEBUG_COND3(e->id)) {
502 gDebugFlag1 = true;
503 }
504#endif
505 rightGeom.move2side((*j).discardedInnerWidthRight);
506#ifdef DEBUG_SHAPE
507 if (DEBUG_COND3(e->id)) {
508 std::cout << " -" << id << "_geom=" << geom << " -" << id << "_rightGeom=" << rightGeom << "\n";
509 gDebugFlag1 = false;
510 }
511#endif
512 PositionVector laneGeom = rightGeom;
513 currRight = new NBEdge("-" + id, sFrom, sTo, (*j).rightType, defaultSpeed, NBEdge::UNSPECIFIED_FRICTION, (*j).rightLaneNumber, priorityR,
515 lanesBuilt = true;
516 std::vector<OpenDriveLane>& lanes = (*j).lanesByDir[OPENDRIVE_TAG_RIGHT];
517 std::sort(lanes.begin(), lanes.end(), LaneSorter());
518 for (std::vector<OpenDriveLane>::const_iterator k = lanes.begin(); k != lanes.end(); ++k) {
519 std::map<int, int>::const_iterator lp = (*j).laneMap.find((*k).id);
520 if (lp != (*j).laneMap.end()) {
521 int sumoLaneIndex = lp->second;
522 setLaneAttributes(e, currRight->getLaneStruct(sumoLaneIndex), *k, saveOrigIDs, tc);
523 laneIndexMap[std::make_pair(currRight, sumoLaneIndex)] = (*k).id;
524 if (useOffsets) {
525 PositionVector laneShape = laneGeom;
526 laneShape.move2sideCustom(offsets);
527 currRight->getLaneStruct(sumoLaneIndex).customShape = laneShape;
528 }
529 } else if (customLaneShapes) {
530 useOffsets = true;
531 }
532 if (customLaneShapes) {
533 addOffsets(false, laneGeom, (*k).widthData, e->id + "_" + toString((*k).id), offsets);
534 }
535 }
536 if (!nb.getEdgeCont().insert(currRight, myImportAllTypes)) {
537 throw ProcessError("Could not add edge '" + currRight->getID() + "'.");
538 }
539 if (nb.getEdgeCont().wasIgnored(id)) {
540 prevRight = nullptr;
541 } else {
542 // connect lane sections
543 if (prevRight != nullptr) {
544 std::map<int, int> connections = (*j).getInnerConnections(OPENDRIVE_TAG_RIGHT, *(j - 1));
545 for (std::map<int, int>::const_iterator k = connections.begin(); k != connections.end(); ++k) {
546#ifdef DEBUG_CONNECTIONS
547 if (DEBUG_COND(e)) {
548 std::cout << "addCon1 from=" << prevRight->getID() << "_" << (*k).first << " to=" << currRight->getID() << "_" << (*k).second << "\n";
549 }
550#endif
551 prevRight->addLane2LaneConnection((*k).first, currRight, (*k).second, NBEdge::Lane2LaneInfoType::VALIDATED);
552 }
553 }
554 prevRight = currRight;
555 }
556 }
557
558 // build lanes to left
559 NBEdge* currLeft = nullptr;
560 if ((*j).leftLaneNumber > 0) {
561 std::vector<double> offsets(geom.size(), 0);
562 bool useOffsets = false;
563 PositionVector leftGeom = geom;
564 leftGeom.move2side(-(*j).discardedInnerWidthLeft);
565 PositionVector laneGeom = leftGeom;
566#ifdef DEBUG_SHAPE
567 if (DEBUG_COND3(e->id)) {
568 std::cout << " " << id << "_geom=" << geom << " " << id << "_leftGeom=" << leftGeom << "\n";
569 }
570#endif
571 currLeft = new NBEdge(id, sTo, sFrom, (*j).leftType, defaultSpeed, NBEdge::UNSPECIFIED_FRICTION, (*j).leftLaneNumber, priorityL,
573 lanesBuilt = true;
574 std::vector<OpenDriveLane>& lanes = (*j).lanesByDir[OPENDRIVE_TAG_LEFT];
575 std::sort(lanes.begin(), lanes.end(), LaneSorter());
576 for (std::vector<OpenDriveLane>::const_iterator k = lanes.begin(); k != lanes.end(); ++k) {
577 std::map<int, int>::const_iterator lp = (*j).laneMap.find((*k).id);
578 if (lp != (*j).laneMap.end()) {
579 int sumoLaneIndex = lp->second;
580 setLaneAttributes(e, currLeft->getLaneStruct(sumoLaneIndex), *k, saveOrigIDs, tc);
581 laneIndexMap[std::make_pair(currLeft, sumoLaneIndex)] = (*k).id;
582 if (useOffsets) {
583 PositionVector laneShape = laneGeom;
584 laneShape.move2sideCustom(offsets);
585 currLeft->getLaneStruct(sumoLaneIndex).customShape = laneShape.reverse();
586 }
587 } else if (customLaneShapes) {
588 useOffsets = true;
589 }
590 if (customLaneShapes) {
591 addOffsets(true, laneGeom, (*k).widthData, e->id + "_" + toString((*k).id), offsets);
592 }
593 }
594 if (!nb.getEdgeCont().insert(currLeft, myImportAllTypes)) {
595 throw ProcessError("Could not add edge '" + currLeft->getID() + "'.");
596 }
597 if (nb.getEdgeCont().wasIgnored(id)) {
598 prevLeft = nullptr;
599 } else {
600 // connect lane sections
601 if (prevLeft != nullptr) {
602 std::map<int, int> connections = (*j).getInnerConnections(OPENDRIVE_TAG_LEFT, *(j - 1));
603 for (std::map<int, int>::const_iterator k = connections.begin(); k != connections.end(); ++k) {
604#ifdef DEBUG_CONNECTIONS
605 if (DEBUG_COND(e)) {
606 std::cout << "addCon2 from=" << currLeft->getID() << "_" << (*k).first << " to=" << prevLeft->getID() << "_" << (*k).second << "\n";
607 }
608#endif
609 currLeft->addLane2LaneConnection((*k).first, prevLeft, (*k).second, NBEdge::Lane2LaneInfoType::VALIDATED);
610 }
611 }
612 prevLeft = currLeft;
613 }
614 }
615 (*j).sumoID = id;
616
617
618 sB = sE;
619 sFrom = sTo;
620 }
621 // optionally write road objects
622 if (oc.isSet("polygon-output")) {
623 const bool writeGeo = GeoConvHelper::getLoaded().usingGeoProjection() && (
624 oc.isDefault("proj.plain-geo") || oc.getBool("proj.plain-geo"));
625 OutputDevice& dev = OutputDevice::getDevice(oc.getString("polygon-output"));
626 dev.writeXMLHeader("additional", "additional_file.xsd");
627 //SUMOPolygon poly("road_" + e->id, "road", RGBColor::BLUE, e->geom, true, false);
628 //poly.writeXML(dev, false);
629 for (auto& o : e->objects) {
630 Position ref = e->geom.positionAtOffset2D(o.s, -o.t);
631 if (o.radius >= 0) {
632 // cicrular shape
633 // GeoConvHelper::getFinal is not ready yet
635 PointOfInterest poly(o.id, o.type, RGBColor::YELLOW, ref, true, "", -1, false, 0);
636 poly.setParameter("name", o.name);
637 poly.writeXML(dev, writeGeo);
638 } else {
639 // rectangular shape
640 PositionVector centerLine;
641 centerLine.push_back(Position(-o.length / 2, 0));
642 centerLine.push_back(Position(o.length / 2, 0));
643 double roadHdg = e->geom.rotationAtOffset(o.s);
644 centerLine.rotate2D(roadHdg + o.hdg);
645 //PointOfInterest poiRef("ref_" + o.id, "", RGBColor::CYAN, ref, false, "", 0, 0, Shape::DEFAULT_LAYER + 2);
646 //poiRef.writeXML(dev, false);
647 centerLine.add(ref);
648 //SUMOPolygon polyCenter("center_" + o.id, "", RGBColor::MAGENTA, centerLine, true, false, Shape::DEFAULT_LAYER + 1);
649 //polyCenter.writeXML(dev, false);
650 centerLine.move2side(o.width / 2);
651 PositionVector shape = centerLine;
652 centerLine.move2side(-o.width);
653 shape.append(centerLine.reverse(), POSITION_EPS);
654 if (writeGeo) {
655 // GeoConvHelper::getFinal is not ready yet
656 for (Position& p : shape) {
658 }
659 }
660 SUMOPolygon poly(o.id, o.type, RGBColor::YELLOW, shape, true, true, 1);
661 poly.setParameter("name", o.name);
662 poly.writeXML(dev, writeGeo);
663 }
664 }
665 }
666 if (!lanesBuilt) {
667 WRITE_WARNINGF(TL("Edge '%' has no lanes."), e->id);
668 }
669 }
670
671 // -------------------------
672 // connections building
673 // -------------------------
674 // generate explicit lane-to-lane connections
675 for (std::map<std::string, OpenDriveEdge*>::iterator i = edges.begin(); i != edges.end(); ++i) {
676 setEdgeLinks2(*(*i).second, edges);
677 }
678 // compute connections across intersections, if any
679 std::vector<Connection> connections2;
680 for (std::map<std::string, OpenDriveEdge*>::iterator j = edges.begin(); j != edges.end(); ++j) {
681 const std::set<Connection>& conns = (*j).second->connections;
682
683 for (std::set<Connection>::const_iterator i = conns.begin(); i != conns.end(); ++i) {
684 if (innerEdges.find((*i).fromEdge) != innerEdges.end()) {
685 // connections starting at inner edges are processed by starting from outer edges
686 continue;
687 }
688 if (innerEdges.find((*i).toEdge) != innerEdges.end()) {
689 std::set<Connection> seen;
690 buildConnectionsToOuter(*i, innerEdges, edges, tc, connections2, seen);
691 } else {
692 connections2.push_back(*i);
693 }
694 }
695 }
696 // set connections
697 for (std::vector<Connection>::const_iterator i = connections2.begin(); i != connections2.end(); ++i) {
698#ifdef DEBUG_CONNECTIONS
699 std::cout << "connections2 " << (*i).getDescription() << "\n";
700#endif
701 std::string fromEdge = (*i).fromEdge;
702 if (edges.find(fromEdge) == edges.end()) {
703 WRITE_WARNING("While setting connections: from-edge '" + fromEdge + "' is not known.");
704 continue;
705 }
706 OpenDriveEdge* odFrom = edges[fromEdge];
707 int fromLane = (*i).fromLane;
708 bool fromLast = ((*i).fromCP == OPENDRIVE_CP_END) && ((*i).fromLane < 0);
709 fromEdge = fromLast ? odFrom->laneSections.back().sumoID : odFrom->laneSections[0].sumoID;
710
711 std::string toEdge = (*i).toEdge;
712 if (edges.find(toEdge) == edges.end()) {
713 WRITE_WARNING("While setting connections: to-edge '" + toEdge + "' is not known.");
714 continue;
715 }
716
717 OpenDriveEdge* odTo = edges[toEdge];
718 int toLane = (*i).toLane;
719 bool toLast = ((*i).toCP == OPENDRIVE_CP_END) || ((*i).toLane > 0);
720 toEdge = toLast ? odTo->laneSections.back().sumoID : odTo->laneSections[0].sumoID;
721
722 if (fromLane == UNSET_CONNECTION) {
723 continue;
724 }
725 if (fromLane < 0) {
726 fromEdge = revertID(fromEdge);
727 }
728 if (toLane == UNSET_CONNECTION) {
729 continue;
730 }
731 if (toLane < 0) {
732 toEdge = revertID(toEdge);
733 }
734 fromLane = fromLast ? odFrom->laneSections.back().laneMap[fromLane] : odFrom->laneSections[0].laneMap[fromLane];
735 toLane = toLast ? odTo->laneSections.back().laneMap[toLane] : odTo->laneSections[0].laneMap[toLane];
736 NBEdge* from = nb.getEdgeCont().retrieve(fromEdge);
737 NBEdge* to = nb.getEdgeCont().retrieve(toEdge);
738 if (from == nullptr) {
739 WRITE_WARNING("Could not find fromEdge representation of '" + fromEdge + "' in connection '" + (*i).origID + "'.");
740 }
741 if (to == nullptr) {
742 WRITE_WARNING("Could not find fromEdge representation of '" + toEdge + "' in connection '" + (*i).origID + "'.");
743 }
744 if (from == nullptr || to == nullptr) {
745 continue;
746 }
747
748#ifdef DEBUG_CONNECTIONS
749 if (DEBUG_COND2(from->getID())) {
750 std::cout << "addCon3 from=" << from->getID() << "_" << fromLane << " to=" << to->getID() << "_" << toLane << "\n";
751 }
752#endif
753 from->addLane2LaneConnection(fromLane, to, toLane, NBEdge::Lane2LaneInfoType::USER, false, false,
760 (*i).shape);
761
762 if ((*i).origID != "" && saveOrigIDs) {
763 // @todo: this is the most silly way to determine the connection
764 std::vector<NBEdge::Connection>& cons = from->getConnections();
765 for (std::vector<NBEdge::Connection>::iterator k = cons.begin(); k != cons.end(); ++k) {
766 if ((*k).fromLane == fromLane && (*k).toEdge == to && (*k).toLane == toLane) {
767 (*k).setParameter(SUMO_PARAM_ORIGID, (*i).origID + "_" + toString((*i).origLane));
768 break;
769 }
770 }
771 }
772 }
773
774
775 // -------------------------
776 // traffic lights
777 // -------------------------
778 std::map<std::string, std::string> tlsControlled;
779 for (std::map<std::string, OpenDriveEdge*>::iterator i = edges.begin(); i != edges.end(); ++i) {
780 OpenDriveEdge* e = (*i).second;
781 for (const OpenDriveSignal& signal : e->signals) {
782 int intType = -1;
783 try {
784 intType = StringUtils::toInt(signal.type);
785 } catch (NumberFormatException&) {}
786 if (intType < 1000001 || (intType > 1000013 && intType != 1000020) || intType == 1000008) {
787 // not a traffic_light (Section 6.11)
788 continue;
789 }
790 if (e->laneSections.size() == 0) {
791 WRITE_WARNING("Edge '" + e->id + "' has signals but no lane sections.");
792 continue;
793 }
794 std::vector<OpenDriveLaneSection>::iterator k = e->laneSections.begin();
795 bool found = false;
796 for (; k != e->laneSections.end() - 1 && !found;) {
797 if (signal.s > (*k).s && signal.s <= (*(k + 1)).s) {
798 found = true;
799 } else {
800 ++k;
801 }
802 }
803
804 std::string id = (*k).sumoID;
805 if (id == "") {
806 // traffic light on connecting road
807 if (e->junction != "") {
808 //WRITE_WARNING("Found a traffic light signal on an internal edge; will not build it (original edge id='" + e->id + "').");
809 std::string fromID, toID;
810 for (std::vector<OpenDriveLink>::const_iterator l = e->links.begin(); l != e->links.end(); ++l) {
811 if ((*l).linkType == OPENDRIVE_LT_PREDECESSOR && (*l).elementType == OPENDRIVE_ET_ROAD) {
812 if (fromID != "") {
813 WRITE_WARNING(TL("Ambiguous start of connection."));
814 }
815 const OpenDriveEdge* const ode = edges[(*l).elementID];
816 if ((*l).contactPoint == OPENDRIVE_CP_START) {
817 fromID = ode->laneSections[0].sumoID;
818 if (signal.orientation < 0) {
819 fromID = "-" + fromID;
820 }
821 } else {
822 fromID = ode->laneSections.back().sumoID;
823 if (signal.orientation > 0) {
824 fromID = "-" + fromID;
825 }
826 }
827 }
828 if ((*l).linkType == OPENDRIVE_LT_SUCCESSOR && (*l).elementType == OPENDRIVE_ET_ROAD) {
829 if (toID != "") {
830 WRITE_WARNING(TL("Ambiguous end of connection."));
831 }
832 const OpenDriveEdge* const ode = edges[(*l).elementID];
833 toID = (*l).contactPoint == OPENDRIVE_CP_START ? ode->laneSections[0].sumoID : ode->laneSections.back().sumoID;
834 }
835 }
836 // figure out the correct combination of directions
837 NBEdge* from;
838 NBEdge* to;
839 auto fromTo = retrieveSignalEdges(nb, fromID, toID, e->junction);
840 from = fromTo.first;
841 to = fromTo.second;
842 if (from == nullptr) {
843 WRITE_WARNINGF(TL("Could not find edge '%' for signal '%'."), fromID, signal.id);
844 continue;
845 }
846
847 // consider signal validity to determine direction
848 if (signal.maxLane != 0) {
849 bool fromForward = from->getID()[0] == '-';
850 bool lanesForward = signal.maxLane < 0;
851 if (fromForward != lanesForward) {
852 std::swap(fromID, toID);
853
854 const auto& signalFromTo = retrieveSignalEdges(nb, fromID, toID, e->junction);
855 from = signalFromTo.first;
856 to = signalFromTo.second;
857 if (from == nullptr) {
858 WRITE_WARNINGF(TL("Could not find edge '%' for signal '%'."), fromID, signal.id);
859 continue;
860 }
861 }
862 }
863 for (NBEdge::Connection& c : from->getConnections()) {
864 if (c.toEdge == to) {
865 int odLane = laneIndexMap[std::make_pair(from, c.fromLane)];
866 //std::cout << " fromLane=" << c.fromLane << " odLane=" << odLane << "\n";
867 if (signal.minLane == 0 || (signal.minLane <= odLane && signal.maxLane >= odLane)) {
868 if (c.knowsParameter("signalID")) {
869 c.setParameter("signalID", c.getParameter("signalID") + " " + signal.id);
870 } else {
871 c.setParameter("signalID", signal.id);
872 }
873 }
874 }
875 }
876 getTLSSecure(from, nb);
877 //std::cout << "odrEdge=" << e->id << " fromID=" << fromID << " toID=" << toID << " from=" << from->getID() << " to=" << to->getID()
878 // << " signal=" << signal.id << " minLane=" << signal.minLane << " maxLane=" << signal.maxLane << "\n";
879 } else {
880 WRITE_WARNINGF(TL("Found a traffic light signal on an unknown edge (original edge id='%')."), e->id);
881 continue;
882 }
883 } else {
884 // traffic light on normal road
885 if (signal.orientation == 1) {
886 // forward direction has negative lane indices and gets a negative prefix in sumo
887 id = "-" + id;
888 }
889 NBEdge* edge = nb.getEdgeCont().retrieve(id);
890 if (edge == nullptr) {
891 WRITE_WARNINGF(TL("Could not find edge '%' for signal '%'."), id, signal.id);
892 continue;
893 }
894 getTLSSecure(edge, nb);
896 for (NBEdge::Connection& c : edge->getConnections()) {
897 int odLane = laneIndexMap[std::make_pair(edge, c.fromLane)];
898 if (signal.minLane == 0 || (signal.minLane <= odLane && signal.maxLane >= odLane)) {
899 if (c.knowsParameter("signalID")) {
900 c.setParameter("signalID", c.getParameter("signalID") + " " + signal.id);
901 } else {
902 c.setParameter("signalID", signal.id);
903 }
904 }
905 }
906 //std::cout << "odrEdge=" << e->id << " sumoID=" << (*k).sumoID << " sumoEdge=" << edge->getID()
907 // << " signal=" << signal.id << " minLane=" << signal.minLane << " maxLane=" << signal.maxLane << "\n";
908 }
909 // @note: tls 'signalID' parameters are set via NBTrafficLightLogicCont::setOpenDriveSignalParameters
910 }
911 }
912
913 // -------------------------
914 // clean up
915 // -------------------------
916 for (std::map<std::string, OpenDriveEdge*>::iterator i = edges.begin(); i != edges.end(); ++i) {
917 delete (*i).second;
918 }
919}
920
921std::pair<NBEdge*, NBEdge*>
922NIImporter_OpenDrive::retrieveSignalEdges(NBNetBuilder& nb, const std::string& fromID, const std::string& toID, const std::string& junction) {
923 NBEdge* from;
924 NBEdge* to;
925 from = nb.getEdgeCont().retrieve(fromID);
926 if (from == nullptr || from->getToNode()->getID() != junction) {
927 from = nb.getEdgeCont().retrieve(fromID[0] == '-' ? fromID.substr(1) : "-" + fromID);
928 }
929 to = nb.getEdgeCont().retrieve(toID);
930 if (to == nullptr || to->getFromNode()->getID() != junction) {
931 to = nb.getEdgeCont().retrieve(toID[0] == '-' ? toID.substr(1) : "-" + toID);
932 }
933 return std::make_pair(from, to);
934}
935
936
939 NBNode* toNode = inEdge->getToNode();
940 if (!toNode->isTLControlled()) {
942 NBOwnTLDef* tlDef = new NBOwnTLDef(toNode->getID(), toNode, 0, type);
943 if (!nb.getTLLogicCont().insert(tlDef)) {
944 // actually, nothing should fail here
945 delete tlDef;
946 throw ProcessError();
947 }
948 toNode->addTrafficLight(tlDef);
949 //tlDef->setSinglePhase();
950 }
951 return *toNode->getControllingTLS().begin();
952}
953
954void
955NIImporter_OpenDrive::setLaneAttributes(const OpenDriveEdge* e, NBEdge::Lane& sumoLane, const OpenDriveLane& odLane, bool saveOrigIDs, const NBTypeCont& tc) {
956 if (saveOrigIDs) {
957 sumoLane.setParameter(SUMO_PARAM_ORIGID, e->id + "_" + toString(odLane.id));
958 }
959 sumoLane.speed = odLane.speed != 0 ? odLane.speed : tc.getEdgeTypeSpeed(odLane.type);
960 sumoLane.permissions = tc.getEdgeTypePermissions(odLane.type);
961 sumoLane.width = myImportWidths && odLane.width != NBEdge::UNSPECIFIED_WIDTH ? odLane.width : tc.getEdgeTypeWidth(odLane.type);
962 sumoLane.type = odLane.type;
963
964 const double widthResolution = tc.getEdgeTypeWidthResolution(odLane.type);
965 const double maxWidth = tc.getEdgeTypeMaxWidth(odLane.type);
966
967 const bool forbiddenNarrow = (sumoLane.width < myMinWidth
968 && (sumoLane.permissions & SVC_PASSENGER) != 0
969 && sumoLane.width < tc.getEdgeTypeWidth(odLane.type));
970
971 if (sumoLane.width >= 0 && widthResolution > 0) {
972 sumoLane.width = floor(sumoLane.width / widthResolution + 0.5) * widthResolution;
973 if (forbiddenNarrow && sumoLane.width >= myMinWidth) {
974 sumoLane.width -= widthResolution;
975 if (sumoLane.width <= 0) {
976 sumoLane.width = MAX2(POSITION_EPS, myMinWidth - POSITION_EPS);
977 }
978 } else if (sumoLane.width == 0) {
979 // round up when close to 0
980 sumoLane.width = widthResolution;
981 }
982 }
983 if (maxWidth > 0) {
984 sumoLane.width = MIN2(sumoLane.width, maxWidth);
985 }
986 if (forbiddenNarrow) {
987 // avoid narrow passenger car lanes (especially at sections with varying width)
989 }
990}
991
992void
994 const std::map<std::string, OpenDriveEdge*>& innerEdges,
995 const std::map<std::string, OpenDriveEdge*>& edges,
996 const NBTypeCont& tc,
997 std::vector<Connection>& into, std::set<Connection>& seen) {
998
999 OpenDriveEdge* dest = innerEdges.find(c.toEdge)->second;
1000#ifdef DEBUG_CONNECTIONS
1001 if (DEBUG_COND3(c.fromEdge)) {
1002 std::cout << " buildConnectionsToOuter " << c.getDescription() << "\n";
1003 std::cout << " dest=" << (dest == nullptr ? "NULL" : dest->id) << " seenlist=";
1004 for (std::set<Connection>::const_iterator i = seen.begin(); i != seen.end(); ++i) {
1005 std::cout << " " << (*i).fromEdge << "," << (*i).toEdge << " ";
1006 }
1007 std::cout << "\n";
1008 }
1009#endif
1010 if (dest == nullptr) {
1012 return;
1013 }
1014 seen.insert(c);
1015 for (const Connection& destCon : dest->connections) {
1016 auto innerEdgesIt = innerEdges.find(destCon.toEdge);
1017#ifdef DEBUG_CONNECTIONS
1018 if (DEBUG_COND3(c.fromEdge)) {
1019 std::cout << " toInner=" << (innerEdgesIt != innerEdges.end()) << " destCon " << (*i).getDescription() << "\n";
1020 }
1021#endif
1022 if (innerEdgesIt != innerEdges.end()) {
1023 std::vector<Connection> t;
1024 if (seen.count(destCon) == 0) {
1025 buildConnectionsToOuter(destCon, innerEdges, edges, tc, t, seen);
1026 for (std::vector<Connection>::const_iterator j = t.begin(); j != t.end(); ++j) {
1027 // @todo this section is unverified
1028 Connection cn = (*j);
1029 cn.fromEdge = c.fromEdge;
1030 cn.fromLane = c.fromLane;
1031 cn.fromCP = c.fromCP;
1032 cn.all = c.all; // @todo "all" is a hack trying to avoid the "from is zero" problem;
1034 cn.shape = innerEdgesIt->second->geom + c.shape;
1035 }
1036 into.push_back(cn);
1037 }
1038 } else {
1039 WRITE_WARNING("Circular connections in junction including roads '" + c.fromEdge + "' and '" + c.toEdge + "', loop size " + toString(seen.size()));
1040 }
1041 } else {
1042 int in = c.toLane;
1043 int out = destCon.fromLane;
1044 if (c.toCP == OPENDRIVE_CP_END) {
1045 // inner edge runs in reverse direction
1046 std::swap(in, out);
1047 }
1048#ifdef DEBUG_CONNECTIONS
1049 if (DEBUG_COND3(c.fromEdge)) {
1050 std::cout << " laneSectionsConnected dest=" << dest->id << " in=" << in << " out=" << out
1051 << " connected=" << laneSectionsConnected(dest, in, out) << "\n";
1052 }
1053#endif
1054
1055 if (laneSectionsConnected(dest, in, out)) {
1056 Connection cn = destCon;
1057 cn.fromEdge = c.fromEdge;
1058 cn.fromLane = c.fromLane;
1059 cn.fromCP = c.fromCP;
1060 cn.all = c.all;
1061 cn.origID = c.toEdge;
1062 cn.origLane = c.toLane;
1064 OpenDriveXMLTag lanesDir;
1065 cn.shape = dest->geom;
1066 // determine which lane of dest belongs to this connection
1067 int referenceLane = 0;
1068 int offsetFactor = 1;
1069 if (c.toCP == OPENDRIVE_CP_END) {
1070 offsetFactor = -1;
1071 lanesDir = OPENDRIVE_TAG_LEFT;
1072 for (const auto& destLane : dest->laneSections.front().lanesByDir[lanesDir]) {
1073 if (destLane.successor == c.fromLane) {
1074 referenceLane = destLane.id;
1075 break;
1076 }
1077 }
1078 } else {
1079 lanesDir = OPENDRIVE_TAG_RIGHT;
1080 for (const auto& destLane : dest->laneSections.front().lanesByDir[lanesDir]) {
1081 if (destLane.predecessor == c.fromLane) {
1082 referenceLane = destLane.id;
1083 break;
1084 }
1085 }
1086 }
1087 // compute offsets
1088 //if (cn.fromEdge == "1014000" && dest->id == "3001022") {
1089 // std::cout << "computeOffsets\n";
1090 //}
1091 std::vector<double> offsets(dest->geom.size(), 0);
1092 if (dest->laneOffsets.size() > 0) {
1093 offsets = dest->laneOffsets;
1094 }
1095#ifdef DEBUG_INTERNALSHAPES
1096 std::string destPred;
1097#endif
1098 double s = 0;
1099 int iShape = 0;
1100 for (int laneSectionIndex = 0; laneSectionIndex < (int)dest->laneSections.size(); laneSectionIndex++) {
1101 OpenDriveLaneSection& laneSection = dest->laneSections[laneSectionIndex];
1102 const double nextS = laneSectionIndex + 1 < (int)dest->laneSections.size() ? dest->laneSections[laneSectionIndex + 1].s : std::numeric_limits<double>::max();
1103 double sStart = s; // distance offset a the start of the current lane section
1104 double finalS = s; // final distance value after processing this segment
1105 int finalI = iShape;
1106 for (const OpenDriveLane& destLane : laneSection.lanesByDir[lanesDir]) {
1107 // each lane of the current segment repeats the same section of shape points and distance offsets
1108 double sectionS = 0;
1109 int i = iShape; // shape index at the start of the current lane section
1110 s = sStart;
1111#ifdef DEBUG_INTERNALSHAPES
1112 destPred += " lane=" + toString(destLane.id)
1113 + " pred=" + toString(destLane.predecessor)
1114 + " succ=" + toString(destLane.successor)
1115 + " wStart=" + (destLane.widthData.empty() ? "?" : toString(destLane.widthData.front().computeAt(0)))
1116 + " wEnd=" + (destLane.widthData.empty() ? "?" : toString(destLane.widthData.front().computeAt(cn.shape.length2D())))
1117 + " width=" + toString(destLane.width) + "\n";
1118#endif
1119 if (abs(destLane.id) <= abs(referenceLane)) {
1120 const double multiplier = offsetFactor * (destLane.id == referenceLane ? 0.5 : 1);
1121#ifdef DEBUG_INTERNALSHAPES
1122 destPred += " multiplier=" + toString(multiplier) + "\n";
1123#endif
1124 int widthDataIndex = 0;
1125 while (s < nextS && i < (int)cn.shape.size()) {
1126 if (i > 0) {
1127 const double dist = cn.shape[i - 1].distanceTo2D(cn.shape[i]);
1128 s += dist;
1129 sectionS += dist;
1130
1131 }
1132 while (widthDataIndex + 1 < (int)destLane.widthData.size()
1133 && sectionS >= destLane.widthData[widthDataIndex + 1].s) {
1134 widthDataIndex++;
1135 }
1136 double width = tc.getEdgeTypeWidth(destLane.type);
1137 if (destLane.widthData.size() > 0) {
1138 width = destLane.widthData[widthDataIndex].computeAt(sectionS);
1139 } else {
1140#ifdef DEBUG_INTERNALSHAPES
1141 std::cout << " missing width data at inner edge " << dest->id << " to=" << cn.toEdge << "_" << cn.toLane << " cp=" << cn.toCP << "\n";
1142#endif
1143 // use first width of the target lane
1144 OpenDriveEdge* const outerToEdge = edges.find(cn.toEdge)->second;
1145 OpenDriveLaneSection& toLaneSection = cn.toCP == OPENDRIVE_CP_END ? outerToEdge->laneSections.front() : outerToEdge->laneSections.back();
1146 const OpenDriveXMLTag laneDir = cn.toLane < 0 ? OPENDRIVE_TAG_RIGHT : OPENDRIVE_TAG_LEFT;
1147 for (const OpenDriveLane& outerToLane : toLaneSection.lanesByDir[laneDir]) {
1148 if (outerToLane.id == cn.toLane && outerToLane.width > 0) {
1149#ifdef DEBUG_INTERNALSHAPES
1150 std::cout << " using toLane width " << width << "\n";
1151#endif
1152 break;
1153 }
1154 }
1155 }
1156 offsets[i] += width * multiplier;
1157 //if (cn.fromEdge == "1014000" && dest->id == "3001022") {
1158 // std::cout << " i=" << i << " s=" << s << " lane=" << destLane.id << " rlane=" << referenceLane /*<< " nextS=" << nextS << */ << " lsIndex=" << laneSectionIndex << " wI=" << widthDataIndex << " wSize=" << destLane.widthData.size() << " m=" << multiplier << " o=" << offsets[i] << "\n";
1159 //}
1160 i++;
1161 }
1162 finalS = s;
1163 finalI = i;
1164 } else if (finalS == s) {
1165 // update finalS without changing offsets
1166 while (s < nextS && i < (int)cn.shape.size()) {
1167 if (i > 0) {
1168 const double dist = cn.shape[i - 1].distanceTo2D(cn.shape[i]);
1169 s += dist;
1170 finalS += dist;
1171
1172 }
1173 i++;
1174 }
1175 finalI = i;
1176
1177 }
1178 }
1179 // advance values for the next lane section
1180 iShape = finalI;
1181 s = finalS;
1182 }
1183 try {
1184 cn.shape.move2sideCustom(offsets);
1185 } catch (InvalidArgument&) {
1186 WRITE_WARNING("Could not import internal lane shape from edge '" + c.fromEdge + "' to edge '" + c.toEdge);
1187 cn.shape.clear();
1188 }
1189#ifdef DEBUG_INTERNALSHAPES
1190 std::cout << "internalShape "
1191 << c.getDescription()
1192 << " dest=" << dest->id
1193 << " refLane=" << referenceLane
1194 << " destPred\n" << destPred
1195 << " offsets=" << offsets
1196 << "\n shape=" << dest->geom
1197 << "\n shape2=" << cn.shape
1198 << "\n";
1199#endif
1200 if (c.toCP == OPENDRIVE_CP_END) {
1201 cn.shape = cn.shape.reverse();
1202 }
1203 }
1204#ifdef DEBUG_CONNECTIONS
1205 if (DEBUG_COND3(c.fromEdge)) {
1206 std::cout << " added connection\n";
1207 }
1208#endif
1209 into.push_back(cn);
1210 }
1211 }
1212 }
1213}
1214
1215
1216bool
1218 if (edge->laneSections.size() == 1) {
1219 return in == out;
1220 } else {
1221 // there could be spacing lanes (type 'none') that lead to a shift in lane index
1222 for (auto it = edge->laneSections.begin(); it + 1 < edge->laneSections.end(); it++) {
1223 OpenDriveLaneSection& laneSection = *it;
1224 if (laneSection.lanesByDir.find(OPENDRIVE_TAG_RIGHT) != laneSection.lanesByDir.end()) {
1225 for (OpenDriveLane& lane : laneSection.lanesByDir.find(OPENDRIVE_TAG_RIGHT)->second) {
1226 if (lane.id == in) {
1227 in = lane.successor;
1228 break;
1229 }
1230 }
1231 }
1232 if (laneSection.lanesByDir.find(OPENDRIVE_TAG_LEFT) != laneSection.lanesByDir.end()) {
1233 for (OpenDriveLane& lane : laneSection.lanesByDir.find(OPENDRIVE_TAG_LEFT)->second) {
1234 if (lane.id == in) {
1235 in = lane.successor;
1236 break;
1237 }
1238 }
1239 }
1240 }
1241 return in == out;
1242 }
1243}
1244
1245
1246void
1247NIImporter_OpenDrive::setEdgeLinks2(OpenDriveEdge& e, const std::map<std::string, OpenDriveEdge*>& edges) {
1248 for (std::vector<OpenDriveLink>::iterator i = e.links.begin(); i != e.links.end(); ++i) {
1249 OpenDriveLink& l = *i;
1250 if (l.elementType != OPENDRIVE_ET_ROAD) {
1251 // we assume that links to nodes are later given as connections to edges
1252 continue;
1253 }
1254 // get the right direction of the connected edge
1255 std::string connectedEdge = l.elementID;
1256 std::string edgeID = e.id;
1257
1259 const std::map<int, int>& laneMap = laneSection.laneMap;
1260#ifdef DEBUG_CONNECTIONS
1261 if (DEBUG_COND(&e)) {
1262 std::cout << "edge=" << e.id << " eType=" << l.elementType << " lType=" << l.linkType << " connectedEdge=" << connectedEdge << " laneSection=" << laneSection.s << " map:\n";
1263 std::cout << joinToString(laneMap, "\n", ":") << "\n";
1264 }
1265#endif
1266 if (laneSection.lanesByDir.find(OPENDRIVE_TAG_RIGHT) != laneSection.lanesByDir.end()) {
1267 const std::vector<OpenDriveLane>& lanes = laneSection.lanesByDir.find(OPENDRIVE_TAG_RIGHT)->second;
1268 for (std::vector<OpenDriveLane>::const_iterator j = lanes.begin(); j != lanes.end(); ++j) {
1269 if (!myImportAllTypes && laneMap.find((*j).id) == laneMap.end()) {
1270 continue;
1271 }
1272 Connection c; // @todo: give Connection a new name and a constructor
1273 c.fromEdge = e.id;
1274 c.fromLane = (*j).id;
1276 c.toLane = l.linkType == OPENDRIVE_LT_SUCCESSOR ? (*j).successor : (*j).predecessor;
1277 c.toEdge = connectedEdge;
1278 c.toCP = l.contactPoint;
1279 c.all = false;
1283 std::swap(c.fromCP, c.toCP);
1284 }
1285 if (edges.find(c.fromEdge) == edges.end()) {
1286 WRITE_ERROR("While setting connections: incoming road '" + c.fromEdge + "' is not known.");
1287 } else {
1288 OpenDriveEdge* src = edges.find(c.fromEdge)->second;
1289 src->connections.insert(c);
1290#ifdef DEBUG_CONNECTIONS
1291 if (DEBUG_COND(src)) {
1292 std::cout << "insertConRight from=" << src->id << "_" << c.fromLane << " to=" << c.toEdge << "_" << c.toLane << "\n";
1293 }
1294#endif
1295 }
1296 }
1297 }
1298 if (laneSection.lanesByDir.find(OPENDRIVE_TAG_LEFT) != laneSection.lanesByDir.end()) {
1299 const std::vector<OpenDriveLane>& lanes = laneSection.lanesByDir.find(OPENDRIVE_TAG_LEFT)->second;
1300 for (std::vector<OpenDriveLane>::const_iterator j = lanes.begin(); j != lanes.end(); ++j) {
1301 if (!myImportAllTypes && laneMap.find((*j).id) == laneMap.end()) {
1302 continue;
1303 }
1304 Connection c;
1305 c.toEdge = e.id;
1306 c.toLane = (*j).id;
1308 c.fromLane = l.linkType == OPENDRIVE_LT_SUCCESSOR ? (*j).successor : (*j).predecessor;
1309 c.fromEdge = connectedEdge;
1310 c.fromCP = l.contactPoint;
1311 c.all = false;
1315 std::swap(c.fromCP, c.toCP);
1316 }
1317 if (edges.find(c.fromEdge) == edges.end()) {
1318 WRITE_ERROR("While setting connections: incoming road '" + c.fromEdge + "' is not known.");
1319 } else {
1320 OpenDriveEdge* src = edges.find(c.fromEdge)->second;
1321 src->connections.insert(c);
1322#ifdef DEBUG_CONNECTIONS
1323 if (DEBUG_COND(src)) {
1324 std::cout << "insertConLeft from=" << src->id << "_" << c.fromLane << " to=" << c.toEdge << "_" << c.toLane << "\n";
1325 }
1326#endif
1327 }
1328 }
1329 }
1330 }
1331}
1332
1333
1334std::string NIImporter_OpenDrive::revertID(const std::string& id) {
1335 if (id[0] == '-') {
1336 return id.substr(1);
1337 }
1338 return "-" + id;
1339}
1340
1341
1342NBNode*
1343NIImporter_OpenDrive::getOrBuildNode(const std::string& id, const Position& pos,
1344 NBNodeCont& nc) {
1345 if (nc.retrieve(id) == nullptr) {
1346 // not yet built; build now
1347 if (!nc.insert(id, pos)) {
1348 // !!! clean up
1349 throw ProcessError("Could not add node '" + id + "'.");
1350 }
1351 }
1352 return nc.retrieve(id);
1353}
1354
1355
1356void
1358 const std::string& nodeID, NIImporter_OpenDrive::LinkType lt, std::vector<NodeSet>& joinedNodeIDs) {
1359 NBNode* n = nc.retrieve(nodeID);
1360 if (n == nullptr) {
1361 throw ProcessError("Could not find node '" + nodeID + "'.");
1362 }
1363 NBNode* toJoin = nullptr;
1364 if (lt == OPENDRIVE_LT_SUCCESSOR) {
1365 if (e.to != nullptr && e.to != n) {
1366 toJoin = e.to;
1367 }
1368 e.to = n;
1369 } else {
1370 if (e.from != nullptr && e.from != n) {
1371 toJoin = e.from;
1372 }
1373 e.from = n;
1374 }
1375 if (toJoin != nullptr) {
1376 // join nodes
1377 NodeSet* set1 = nullptr;
1378 NodeSet* set2 = nullptr;
1379 for (NodeSet& joined : joinedNodeIDs) {
1380 if (joined.count(toJoin) != 0) {
1381 set1 = &joined;
1382 }
1383 if (joined.count(n) != 0) {
1384 set2 = &joined;
1385 }
1386 }
1387 if (set1 == nullptr && set2 == nullptr) {
1388 joinedNodeIDs.push_back(NodeSet());
1389 joinedNodeIDs.back().insert(n);
1390 joinedNodeIDs.back().insert(toJoin);
1391 } else if (set1 == nullptr && set2 != nullptr) {
1392 set2->insert(toJoin);
1393 } else if (set1 != nullptr && set2 == nullptr) {
1394 set1->insert(n);
1395 } else {
1396 set1->insert(set2->begin(), set2->end());
1397 joinedNodeIDs.erase(std::find(joinedNodeIDs.begin(), joinedNodeIDs.end(), *set2));
1398 }
1399 }
1400}
1401
1402bool
1404 if (e.elevations.size() > 1) {
1405 return true;
1406 }
1407 for (OpenDriveElevation& el : e.elevations) {
1408 if (el.c != 0 || el.d != 0) {
1409 return true;
1410 }
1411 }
1412 return false;
1413}
1414
1415void
1416NIImporter_OpenDrive::computeShapes(std::map<std::string, OpenDriveEdge*>& edges) {
1418 const double res = oc.getFloat("opendrive.curve-resolution");
1419 for (std::map<std::string, OpenDriveEdge*>::iterator i = edges.begin(); i != edges.end(); ++i) {
1420 OpenDriveEdge& e = *(*i).second;
1422 const double lineRes = hasNonLinearElevation(e) ? res : -1;
1423 Position last;
1424 for (std::vector<OpenDriveGeometry>::iterator j = e.geometries.begin(); j != e.geometries.end(); ++j) {
1425 OpenDriveGeometry& g = *j;
1426 PositionVector geom;
1427 switch (g.type) {
1429 break;
1430 case OPENDRIVE_GT_LINE:
1431 geom = geomFromLine(e, g, lineRes);
1432 break;
1434 geom = geomFromSpiral(e, g, res);
1435 break;
1436 case OPENDRIVE_GT_ARC:
1437 geom = geomFromArc(e, g, res);
1438 break;
1439 case OPENDRIVE_GT_POLY3:
1440 geom = geomFromPoly(e, g, res);
1441 break;
1443 geom = geomFromParamPoly(e, g, res);
1444 break;
1445 default:
1446 break;
1447 }
1448 if (e.geom.size() > 0 && prevType == OPENDRIVE_GT_LINE) {
1449 // remove redundant end point of the previous geometry segment
1450 // (the start point of the current segment should have the same value)
1451 // this avoids geometry errors due to imprecision
1452 if (!e.geom.back().almostSame(geom.front())) {
1453 const int index = (int)(j - e.geometries.begin());
1454 WRITE_WARNINGF(TL("Mismatched geometry for edge '%' between geometry segments % and %."), e.id, index - 1, index);
1455 }
1456 e.geom.pop_back();
1457 }
1458 //std::cout << " adding geometry to road=" << e.id << " old=" << e.geom << " new=" << geom << "\n";
1459 for (PositionVector::iterator k = geom.begin(); k != geom.end(); ++k) {
1460 last = *k;
1462 }
1463 prevType = g.type;
1464 }
1465 if (e.geom.size() == 1 && e.geom.front() != last) {
1466 // avoid length-1 geometry due to almostSame check
1467 e.geom.push_back(last);
1468 }
1469#ifdef DEBUG_SHAPE
1470 if (DEBUG_COND3(e.id)) {
1471 std::cout << " initialGeom=" << e.geom << "\n";
1472 }
1473#endif
1474 if (oc.exists("geometry.min-dist") && !oc.isDefault("geometry.min-dist")) {
1475 // simplify geometry for both directions consistently but ensure
1476 // that start and end angles are preserved
1477 if (e.geom.size() > 4) {
1478 e.geom.removeDoublePoints(oc.getFloat("geometry.min-dist"), true, 1, 1, true);
1479 }
1480 }
1481#ifdef DEBUG_SHAPE
1482 if (DEBUG_COND3(e.id)) {
1483 std::cout << " reducedGeom=" << e.geom << "\n";
1484 }
1485#endif
1487 WRITE_ERROR("Unable to project coordinates for edge '" + e.id + "'.");
1488 }
1489 // add z-data
1490 int k = 0;
1491 double pos = 0;
1492 //std::cout << " edge=" << e.id << " geom.size=" << e.geom.size() << " geom.len=" << e.geom.length2D() << " ele.size=" << e.elevations.size() << "\n";
1493 if (!oc.getBool("flatten")) {
1494 for (std::vector<OpenDriveElevation>::iterator j = e.elevations.begin(); j != e.elevations.end(); ++j) {
1495 const OpenDriveElevation& el = *j;
1496 const double sNext = (j + 1) == e.elevations.end() ? std::numeric_limits<double>::max() : (*(j + 1)).s;
1497 while (k < (int)e.geom.size() && pos < sNext) {
1498 const double z = el.computeAt(pos);
1499 //std::cout << " edge=" << e.id << " k=" << k << " sNext=" << sNext << " pos=" << pos << " z=" << z << " el.s=" << el.s << " el.a=" << el.a << " el.b=" << el.b << " el.c=" << el.c << " el.d=" << el.d << "\n";
1500 e.geom[k].add(0, 0, z);
1501 k++;
1502 if (k < (int)e.geom.size()) {
1503 // XXX pos understimates the actual position since the
1504 // actual geometry between k-1 and k could be curved
1505 pos += e.geom[k - 1].distanceTo2D(e.geom[k]);
1506 }
1507 }
1508 }
1509 }
1510 // add laneoffset
1511 if (e.offsets.size() > 0) {
1513 }
1514 //std::cout << " loaded geometry " << e.id << "=" << e.geom << "\n";
1515 }
1516}
1517
1518
1519std::vector<double>
1520NIImporter_OpenDrive::discretizeOffsets(PositionVector& geom, const std::vector<OpenDriveLaneOffset>& offsets, const std::string& id) {
1521 UNUSED_PARAMETER(id);
1522 std::vector<double> laneOffsets;
1523 // make sure there are intermediate points for each offset-section
1524 for (const OpenDriveLaneOffset& el : offsets) {
1525 // check wether we need to insert a new point at dist
1526 Position pS = geom.positionAtOffset2D(el.s);
1527 int iS = geom.indexOfClosest(pS);
1528 // prevent close spacing to reduce impact of rounding errors in z-axis
1529 if (pS.distanceTo2D(geom[iS]) > POSITION_EPS) {
1530 geom.insertAtClosest(pS, false);
1531 //std::cout << " edge=" << e.id << " inserting pos=" << pS << " s=" << el.s << " iS=" << iS << " dist=" << pS.distanceTo2D(geom[iS]) << "\n";
1532 }
1533 }
1534 // XXX add further points for sections with non-constant offset
1535 // shift each point orthogonally by the specified offset
1536 int kk = 0;
1537 double ppos = 0;
1538 for (auto j = offsets.begin(); j != offsets.end(); ++j) {
1539 const OpenDriveLaneOffset& el = *j;
1540 const double sNext = (j + 1) == offsets.end() ? std::numeric_limits<double>::max() : (*(j + 1)).s;
1541 while (kk < (int)geom.size() && ppos < sNext) {
1542 const double offset = el.computeAt(ppos);
1543 laneOffsets.push_back(fabs(offset) > POSITION_EPS ? -offset : 0);
1544 kk++;
1545 if (kk < (int)geom.size()) {
1546 // XXX pos understimates the actual position since the
1547 // actual geometry between k-1 and k could be curved
1548 ppos += geom[kk - 1].distanceTo2D(geom[kk]);
1549 }
1550 }
1551 }
1552 return laneOffsets;
1553}
1554
1555
1556void
1557NIImporter_OpenDrive::addOffsets(bool left, PositionVector& geom, const std::vector<OpenDriveWidth>& offsets, const std::string& id, std::vector<double>& result) {
1558 UNUSED_PARAMETER(id);
1559 // make sure there are intermediate points for each offset-section
1560 for (const OpenDriveLaneOffset& el : offsets) {
1561 // check wether we need to insert a new point at dist
1562 Position pS = geom.positionAtOffset2D(el.s);
1563 int iS = geom.indexOfClosest(pS);
1564 // prevent close spacing to reduce impact of rounding errors in z-axis
1565 if (pS.distanceTo2D(geom[iS]) > POSITION_EPS) {
1566 //std::cout << " edge=" << id << " inserting pos=" << pS << " s=" << el.s << " iS=" << iS << " dist=" << pS.distanceTo2D(geom[iS]) << "\n";
1567 int at = geom.insertAtClosest(pS, false);
1568 double interpolatedOffset = 0;
1569 if (at == 0) {
1570 interpolatedOffset = result.front();
1571 } else if (at == (int)geom.size() - 1) {
1572 interpolatedOffset = result.back();
1573 } else {
1574 interpolatedOffset = (result[at - 1] + result[at]) / 2;
1575 }
1576 result.insert(result.begin() + at, interpolatedOffset);
1577 }
1578 }
1579 // shift each point orthogonally by the specified offset
1580 int kk = 0;
1581 double ppos = 0;
1582 const int sign = left ? -1 : 1;
1583 for (auto j = offsets.begin(); j != offsets.end(); ++j) {
1584 const OpenDriveWidth& el = *j;
1585 const double sNext = (j + 1) == offsets.end() ? std::numeric_limits<double>::max() : (*(j + 1)).s;
1586 while (kk < (int)geom.size() && ppos < sNext) {
1587 const double offset = el.computeAt(ppos);
1588 result[kk] += fabs(offset) > POSITION_EPS ? sign * offset : 0;
1589 kk++;
1590 if (kk < (int)geom.size()) {
1591 // XXX pos understimates the actual position since the
1592 // actual geometry between k-1 and k could be curved
1593 ppos += geom[kk - 1].distanceTo2D(geom[kk]);
1594 }
1595 }
1596 }
1597}
1598
1599
1600void
1601NIImporter_OpenDrive::revisitLaneSections(const NBTypeCont& tc, std::map<std::string, OpenDriveEdge*>& edges) {
1602 for (std::map<std::string, OpenDriveEdge*>::iterator i = edges.begin(); i != edges.end(); ++i) {
1603 OpenDriveEdge& e = *(*i).second;
1604#ifdef DEBUG_VARIABLE_SPEED
1605 if (DEBUG_COND(&e)) {
1606 gDebugFlag1 = true;
1607 std::cout << "revisitLaneSections e=" << e.id << "\n";
1608 }
1609#endif
1610 std::vector<OpenDriveLaneSection>& laneSections = e.laneSections;
1611 // split by speed limits
1612 std::vector<OpenDriveLaneSection> newSections;
1613 for (std::vector<OpenDriveLaneSection>::iterator j = laneSections.begin(); j != laneSections.end(); ++j) {
1614 std::vector<OpenDriveLaneSection> splitSections;
1615 bool splitBySpeed = (*j).buildSpeedChanges(tc, splitSections);
1616 if (!splitBySpeed) {
1617 newSections.push_back(*j);
1618 } else {
1619 std::copy(splitSections.begin(), splitSections.end(), back_inserter(newSections));
1620 }
1621 }
1622
1623 e.laneSections = newSections;
1624 laneSections = e.laneSections;
1625 double lastS = -1;
1626 // check whether the lane sections are in the right order
1627 bool sorted = true;
1628 for (std::vector<OpenDriveLaneSection>::const_iterator j = laneSections.begin(); j != laneSections.end() && sorted; ++j) {
1629 if ((*j).s <= lastS) {
1630 sorted = false;
1631 }
1632 lastS = (*j).s;
1633 }
1634 if (!sorted) {
1635 WRITE_WARNING("The sections of edge '" + e.id + "' are not sorted properly.");
1636 sort(e.laneSections.begin(), e.laneSections.end(), sections_by_s_sorter());
1637 }
1638 // check whether no duplicates of s-value occure
1639 lastS = -1;
1640 laneSections = e.laneSections;
1641 for (std::vector<OpenDriveLaneSection>::iterator j = laneSections.begin(); j != laneSections.end();) {
1642 bool simlarToLast = fabs((*j).s - lastS) < POSITION_EPS;
1643 lastS = (*j).s;
1644 // keep all lane sections for connecting roads because they are
1645 // needed to establish connectivity (laneSectionsConnected)
1646 if (simlarToLast && !e.isInner) {
1647 WRITE_WARNINGF(TL("Almost duplicate s-value '%' for lane sections occurred at edge '%'; second entry was removed."), toString(lastS), e.id);
1648 j = laneSections.erase(j);
1649 } else {
1650 ++j;
1651 }
1652 }
1653#ifdef DEBUG_VARIABLE_SPEED
1654 gDebugFlag1 = false;
1655#endif
1656 }
1657}
1658
1659
1663 PositionVector ret;
1664 Position start(g.x, g.y);
1665 Position end = calculateStraightEndPoint(g.hdg, g.length, start);
1666 if (resolution > 0 && g.length > 0) {
1667 const int numPoints = (int)ceil(g.length / resolution) + 1;
1668 double dx = (end.x() - start.x()) / (numPoints - 1);
1669 double dy = (end.y() - start.y()) / (numPoints - 1);
1670 for (int i = 0; i < numPoints; i++) {
1671 ret.push_back(Position(g.x + i * dx, g.y + i * dy));
1672 }
1673 } else {
1674 ret.push_back(start);
1675 ret.push_back(end);
1676 }
1677 return ret;
1678}
1679
1680
1684 PositionVector ret;
1685 double curveStart = g.params[0];
1686 double curveEnd = g.params[1];
1687 try {
1688 double cDot = (curveEnd - curveStart) / g.length;
1689 if (cDot == 0 || g.length == 0) {
1690 WRITE_WARNINGF(TL("Could not compute spiral geometry for edge '%' (cDot=% length=%)."), e.id, toString(cDot), toString(g.length));
1691 ret.push_back(Position(g.x, g.y));
1692 return ret;
1693 }
1694 double sStart = curveStart / cDot;
1695 double sEnd = curveEnd / cDot;
1696 double x = 0;
1697 double y = 0;
1698 double t = 0;
1699 double tStart = 0;
1700 double s;
1701 odrSpiral(sStart, cDot, &x, &y, &tStart);
1702 for (s = sStart; s <= sEnd; s += resolution) {
1703 odrSpiral(s, cDot, &x, &y, &t);
1704 ret.push_back(Position(x, y));
1705 }
1706 if (s != sEnd /*&& ret.size() == 1*/) {
1707 odrSpiral(sEnd, cDot, &x, &y, &t);
1708 ret.push_back(Position(x, y));
1709 }
1710 //if (s != sEnd && ret.size() > 2) {
1711 // ret.pop_back();
1712 //}
1713 assert(ret.size() >= 2);
1714 assert(ret[0] != ret[1]);
1715 // shift start to coordinate origin
1716 PositionVector ret1 = ret;
1717 ret.add(ret.front() * -1);
1718 // rotate
1719 PositionVector ret2 = ret;
1720 ret.rotate2D(g.hdg - tStart);
1721#ifdef DEBUG_SPIRAL
1722 std::cout
1723 << std::setprecision(4)
1724 << "edge=" << e.id << " s=" << g.s
1725 << " cStart=" << curveStart
1726 << " cEnd=" << curveEnd
1727 << " cDot=" << cDot
1728 << " sStart=" << sStart
1729 << " sEnd=" << sEnd
1730 << " g.hdg=" << GeomHelper::naviDegree(g.hdg)
1731 << " tStart=" << GeomHelper::naviDegree(tStart)
1732 << "\n beforeShift=" << ret1
1733 << "\n beforeRot=" << ret2
1734 << "\n";
1735#endif
1736 // shift to geometry start
1737 ret.add(g.x, g.y, 0);
1738 } catch (const std::runtime_error& error) {
1739 WRITE_WARNING("Could not compute spiral geometry for edge '" + e.id + "' (" + error.what() + ").");
1740 ret.push_back(Position(g.x, g.y));
1741 }
1742 return ret.getSubpart2D(0, g.length);
1743}
1744
1745
1749 PositionVector ret;
1750 double centerX = g.x;
1751 double centerY = g.y;
1752 // left: positive value
1753 double curvature = g.params[0];
1754 double radius = 1. / curvature;
1755 // center point
1756 calculateCurveCenter(&centerX, &centerY, radius, g.hdg);
1757 double endX = g.x;
1758 double endY = g.y;
1759 double startX = g.x;
1760 double startY = g.y;
1761 double geo_posS = g.s;
1762 double geo_posE = g.s;
1763 bool end = false;
1764 do {
1765 geo_posE += resolution;
1766 if (geo_posE - g.s > g.length) {
1767 geo_posE = g.s + g.length;
1768 }
1769 if (geo_posE - g.s > g.length) {
1770 geo_posE = g.s + g.length;
1771 }
1772 calcPointOnCurve(&endX, &endY, centerX, centerY, radius, geo_posE - geo_posS);
1773 ret.push_back(Position(startX, startY));
1774
1775 startX = endX;
1776 startY = endY;
1777 geo_posS = geo_posE;
1778
1779 if (geo_posE - (g.s + g.length) < 0.001 && geo_posE - (g.s + g.length) > -0.001) {
1780 end = true;
1781 }
1782 } while (!end);
1783 ret.push_back(Position(startX, startY));
1784 return ret.getSubpart2D(0, g.length);
1785}
1786
1787
1791 const double s = sin(g.hdg);
1792 const double c = cos(g.hdg);
1793 PositionVector ret;
1794 for (double off = 0; off < g.length + 2.; off += resolution) {
1795 double x = off;
1796 double y = g.params[0] + g.params[1] * off + g.params[2] * pow(off, 2.) + g.params[3] * pow(off, 3.);
1797 double xnew = x * c - y * s;
1798 double ynew = x * s + y * c;
1799 ret.push_back(Position(g.x + xnew, g.y + ynew));
1800 }
1801 return ret.getSubpart2D(0, g.length);
1802}
1803
1804
1808 const double s = sin(g.hdg);
1809 const double c = cos(g.hdg);
1810 const double pMax = g.params[8] <= 0 ? g.length : g.params[8];
1811 const double pStep = pMax / ceil(g.length / resolution);
1812 PositionVector ret;
1813 for (double p = 0; p <= pMax + pStep; p += pStep) {
1814 double x = g.params[0] + g.params[1] * p + g.params[2] * pow(p, 2.) + g.params[3] * pow(p, 3.);
1815 double y = g.params[4] + g.params[5] * p + g.params[6] * pow(p, 2.) + g.params[7] * pow(p, 3.);
1816 double xnew = x * c - y * s;
1817 double ynew = x * s + y * c;
1818 ret.push_back(Position(g.x + xnew, g.y + ynew));
1819 }
1820 return ret.getSubpart2D(0, g.length);
1821}
1822
1823
1825NIImporter_OpenDrive::calculateStraightEndPoint(double hdg, double length, const Position& start) {
1826 double normx = 1.0f;
1827 double normy = 0.0f;
1828 double x2 = normx * cos(hdg) - normy * sin(hdg);
1829 double y2 = normx * sin(hdg) + normy * cos(hdg);
1830 normx = x2 * length;
1831 normy = y2 * length;
1832 return Position(start.x() + normx, start.y() + normy);
1833}
1834
1835
1836void
1837NIImporter_OpenDrive::calculateCurveCenter(double* ad_x, double* ad_y, double ad_radius, double ad_hdg) {
1838 double normX = 1.0;
1839 double normY = 0.0;
1840 double tmpX;
1841 double turn;
1842 if (ad_radius > 0) {
1843 turn = -1.0;
1844 } else {
1845 turn = 1.0;
1846 }
1847
1848 tmpX = normX;
1849 normX = normX * cos(ad_hdg) + normY * sin(ad_hdg);
1850 normY = tmpX * sin(ad_hdg) + normY * cos(ad_hdg);
1851
1852 tmpX = normX;
1853 normX = turn * normY;
1854 normY = -turn * tmpX;
1855
1856 normX = fabs(ad_radius) * normX;
1857 normY = fabs(ad_radius) * normY;
1858
1859 *ad_x += normX;
1860 *ad_y += normY;
1861}
1862
1863
1864void
1865NIImporter_OpenDrive::calcPointOnCurve(double* ad_x, double* ad_y, double ad_centerX, double ad_centerY,
1866 double ad_r, double ad_length) {
1867 double rotAngle = ad_length / fabs(ad_r);
1868 double vx = *ad_x - ad_centerX;
1869 double vy = *ad_y - ad_centerY;
1870 double tmpx;
1871
1872 double turn;
1873 if (ad_r > 0) {
1874 turn = -1; //left
1875 } else {
1876 turn = 1; //right
1877 }
1878 tmpx = vx;
1879 vx = vx * cos(rotAngle) + turn * vy * sin(rotAngle);
1880 vy = -1 * turn * tmpx * sin(rotAngle) + vy * cos(rotAngle);
1881 *ad_x = vx + ad_centerX;
1882 *ad_y = vy + ad_centerY;
1883}
1884
1885
1886// ---------------------------------------------------------------------------
1887// section
1888// ---------------------------------------------------------------------------
1890 lanesByDir[OPENDRIVE_TAG_LEFT] = std::vector<OpenDriveLane>();
1891 lanesByDir[OPENDRIVE_TAG_RIGHT] = std::vector<OpenDriveLane>();
1892 lanesByDir[OPENDRIVE_TAG_CENTER] = std::vector<OpenDriveLane>();
1893}
1894
1895
1896void
1898 discardedInnerWidthRight = 0;
1899 int sumoLane = 0;
1900 bool singleType = true;
1901 std::vector<std::string> types;
1902 const std::vector<OpenDriveLane>& dirLanesR = lanesByDir.find(OPENDRIVE_TAG_RIGHT)->second;
1903 for (std::vector<OpenDriveLane>::const_reverse_iterator i = dirLanesR.rbegin(); i != dirLanesR.rend(); ++i) {
1904 if (myImportAllTypes || (tc.knows((*i).type) && !tc.getEdgeTypeShallBeDiscarded((*i).type))) {
1905 discardedInnerWidthRight = 0;
1906 laneMap[(*i).id] = sumoLane++;
1907 types.push_back((*i).type);
1908 if (types.front() != types.back()) {
1909 singleType = false;
1910 }
1911 } else {
1912 discardedInnerWidthRight += (*i).width;
1913 }
1914 }
1915 discardedInnerWidthLeft = 0;
1916 rightLaneNumber = sumoLane;
1917 rightType = sumoLane > 0 ? (singleType ? types.front() : joinToString(types, "|")) : "";
1918 sumoLane = 0;
1919 singleType = true;
1920 types.clear();
1921 const std::vector<OpenDriveLane>& dirLanesL = lanesByDir.find(OPENDRIVE_TAG_LEFT)->second;
1922 for (std::vector<OpenDriveLane>::const_iterator i = dirLanesL.begin(); i != dirLanesL.end(); ++i) {
1923 if (myImportAllTypes || (tc.knows((*i).type) && !tc.getEdgeTypeShallBeDiscarded((*i).type))) {
1924 discardedInnerWidthLeft = 0;
1925 laneMap[(*i).id] = sumoLane++;
1926 types.push_back((*i).type);
1927 if (types.front() != types.back()) {
1928 singleType = false;
1929 }
1930 } else {
1931 discardedInnerWidthLeft += (*i).width;
1932 }
1933 }
1934 leftLaneNumber = sumoLane;
1935 leftType = sumoLane > 0 ? (singleType ? types.front() : joinToString(types, "|")) : "";
1936}
1937
1938
1939std::map<int, int>
1941 std::map<int, int> ret;
1942 const std::vector<OpenDriveLane>& dirLanes = lanesByDir.find(dir)->second;
1943 for (std::vector<OpenDriveLane>::const_reverse_iterator i = dirLanes.rbegin(); i != dirLanes.rend(); ++i) {
1944 std::map<int, int>::const_iterator toP = laneMap.find((*i).id);
1945 if (toP == laneMap.end()) {
1946 // the current lane is not available in SUMO
1947 continue;
1948 }
1949 int to = (*toP).second;
1950 int from = UNSET_CONNECTION;
1951 if ((*i).predecessor != UNSET_CONNECTION) {
1952 from = (*i).predecessor;
1953 }
1954 if (from != UNSET_CONNECTION) {
1955 std::map<int, int>::const_iterator fromP = prev.laneMap.find(from);
1956 if (fromP != prev.laneMap.end()) {
1957 from = (*fromP).second;
1958 } else {
1959 from = UNSET_CONNECTION;
1960 }
1961 }
1962 if (from != UNSET_CONNECTION && to != UNSET_CONNECTION) {
1963 if (ret.find(from) != ret.end()) {
1964// WRITE_WARNING(TL("double connection"));
1965 }
1966 if (dir == OPENDRIVE_TAG_LEFT) {
1967 std::swap(from, to);
1968 }
1969 ret[from] = to;
1970 } else {
1971// WRITE_WARNING(TL("missing connection"));
1972 }
1973 }
1974 return ret;
1975}
1976
1977
1980 OpenDriveLaneSection ret(*this);
1981 ret.s += startPos;
1982 for (int k = 0; k < (int)ret.lanesByDir[OPENDRIVE_TAG_RIGHT].size(); ++k) {
1984 l.speed = 0;
1985 std::vector<std::pair<double, double> >::const_iterator i = std::find_if(l.speeds.begin(), l.speeds.end(), same_position_finder(startPos));
1986 if (i != l.speeds.end()) {
1987 l.speed = (*i).second;
1988 }
1989 }
1990 for (int k = 0; k < (int)ret.lanesByDir[OPENDRIVE_TAG_LEFT].size(); ++k) {
1992 std::vector<std::pair<double, double> >::const_iterator i = std::find_if(l.speeds.begin(), l.speeds.end(), same_position_finder(startPos));
1993 l.speed = 0;
1994 if (i != l.speeds.end()) {
1995 l.speed = (*i).second;
1996 }
1997 }
1998 return ret;
1999}
2000
2001
2002bool
2003NIImporter_OpenDrive::OpenDriveLaneSection::buildSpeedChanges(const NBTypeCont& tc, std::vector<OpenDriveLaneSection>& newSections) {
2004 std::set<double> speedChangePositions;
2005 // collect speed change positions and apply initial speed to the begin
2006 for (std::vector<OpenDriveLane>::iterator k = lanesByDir[OPENDRIVE_TAG_RIGHT].begin(); k != lanesByDir[OPENDRIVE_TAG_RIGHT].end(); ++k) {
2007 for (std::vector<std::pair<double, double> >::const_iterator l = (*k).speeds.begin(); l != (*k).speeds.end(); ++l) {
2008 speedChangePositions.insert((*l).first);
2009 if ((*l).first == 0) {
2010 (*k).speed = (*l).second;
2011 }
2012 }
2013 }
2014 for (std::vector<OpenDriveLane>::iterator k = lanesByDir[OPENDRIVE_TAG_LEFT].begin(); k != lanesByDir[OPENDRIVE_TAG_LEFT].end(); ++k) {
2015 for (std::vector<std::pair<double, double> >::const_iterator l = (*k).speeds.begin(); l != (*k).speeds.end(); ++l) {
2016 speedChangePositions.insert((*l).first);
2017 if ((*l).first == 0) {
2018 (*k).speed = (*l).second;
2019 }
2020 }
2021 }
2022 // do nothing if there is none
2023 if (speedChangePositions.size() == 0) {
2024 return false;
2025 }
2026 if (*speedChangePositions.begin() > 0) {
2027 speedChangePositions.insert(0);
2028 }
2029#ifdef DEBUG_VARIABLE_SPEED
2030 if (gDebugFlag1) std::cout
2031 << " buildSpeedChanges sectionStart=" << s
2032 << " speedChangePositions=" << joinToString(speedChangePositions, ", ")
2033 << "\n";
2034#endif
2035 for (std::set<double>::iterator i = speedChangePositions.begin(); i != speedChangePositions.end(); ++i) {
2036 if (i == speedChangePositions.begin()) {
2037 newSections.push_back(*this);
2038 } else {
2039 newSections.push_back(buildLaneSection(*i));
2040 }
2041 }
2042 // propagate speeds
2043 for (int i = 0; i != (int)newSections.size(); ++i) {
2044 for (auto& k : newSections[i].lanesByDir) {
2045 for (int j = 0; j != (int)k.second.size(); ++j) {
2046 OpenDriveLane& l = k.second[j];
2047 if (l.speed != 0) {
2048 continue;
2049 }
2050 if (i > 0) {
2051 l.speed = newSections[i - 1].lanesByDir[k.first][j].speed;
2052 } else {
2053 tc.getEdgeTypeSpeed(l.type);
2054 }
2055 }
2056 }
2057 }
2058 return true;
2059}
2060
2061
2062
2063// ---------------------------------------------------------------------------
2064// edge
2065// ---------------------------------------------------------------------------
2066int
2068 // for signal interpretations see https://de.wikipedia.org/wiki/Bildtafel_der_Verkehrszeichen_in_der_Bundesrepublik_Deutschland_seit_2013
2069 int prio = 1;
2070 for (std::vector<OpenDriveSignal>::const_iterator i = signals.begin(); i != signals.end(); ++i) {
2071 int tmp = 1;
2072 if ((*i).type == "301" || (*i).type == "306") { // priority road or local priority
2073 tmp = 2;
2074 }
2075 if ((*i).type == "205" /*|| (*i).type == "206"*/) { // yield or stop
2076 tmp = 0;
2077 }
2078 if (tmp != 1 && dir == OPENDRIVE_TAG_RIGHT && (*i).orientation > 0) {
2079 prio = tmp;
2080 }
2081 if (tmp != 1 && dir == OPENDRIVE_TAG_LEFT && (*i).orientation < 0) {
2082 prio = tmp;
2083 }
2084
2085 }
2086 return prio;
2087}
2088
2089
2090
2091// ---------------------------------------------------------------------------
2092// loader methods
2093// ---------------------------------------------------------------------------
2094NIImporter_OpenDrive::NIImporter_OpenDrive(const NBTypeCont& tc, std::map<std::string, OpenDriveEdge*>& edges)
2096 myTypeContainer(tc), myCurrentEdge("", "", "", -1), myEdges(edges), myOffset(0, 0) {
2097}
2098
2099
2101}
2102
2103
2104void
2106 const SUMOSAXAttributes& attrs) {
2107 bool ok = true;
2108 switch (element) {
2109 case OPENDRIVE_TAG_HEADER: {
2110 /*
2111 int majorVersion = attrs.get<int>(OPENDRIVE_ATTR_REVMAJOR, nullptr, ok);
2112 int minorVersion = attrs.get<int>(OPENDRIVE_ATTR_REVMINOR, nullptr, ok);
2113 if (majorVersion != 1 || minorVersion != 2) {
2114 // TODO: leave note of exceptions
2115 WRITE_WARNING("Given openDrive file '" + getFileName() + "' uses version " + toString(majorVersion) + "." + toString(minorVersion) + ";\n Version 1.2 is supported.");
2116 }
2117 */
2118 }
2119 break;
2120 case OPENDRIVE_TAG_OFFSET: {
2121 double x = attrs.get<double>(OPENDRIVE_ATTR_X, "offset", ok);
2122 double y = attrs.get<double>(OPENDRIVE_ATTR_Y, "offset", ok);
2123 myOffset.set(x, y);
2126 }
2127 }
2128 break;
2129 case OPENDRIVE_TAG_ROAD: {
2130 std::string id = attrs.get<std::string>(OPENDRIVE_ATTR_ID, nullptr, ok);
2131 std::string streetName = attrs.getOpt<std::string>(OPENDRIVE_ATTR_NAME, nullptr, ok, "", false);
2132 std::string junction = attrs.get<std::string>(OPENDRIVE_ATTR_JUNCTION, id.c_str(), ok);
2133 double length = attrs.get<double>(OPENDRIVE_ATTR_LENGTH, id.c_str(), ok);
2134 myCurrentEdge = OpenDriveEdge(id, streetName, junction, length);
2135 }
2136 break;
2138 if (myElementStack.size() >= 2 && myElementStack[myElementStack.size() - 2] == OPENDRIVE_TAG_ROAD) {
2139 std::string elementType = attrs.get<std::string>(OPENDRIVE_ATTR_ELEMENTTYPE, myCurrentEdge.id.c_str(), ok);
2140 std::string elementID = attrs.get<std::string>(OPENDRIVE_ATTR_ELEMENTID, myCurrentEdge.id.c_str(), ok);
2141 std::string contactPoint = attrs.hasAttribute(OPENDRIVE_ATTR_CONTACTPOINT)
2142 ? attrs.get<std::string>(OPENDRIVE_ATTR_CONTACTPOINT, myCurrentEdge.id.c_str(), ok)
2143 : "end";
2144 addLink(OPENDRIVE_LT_PREDECESSOR, elementType, elementID, contactPoint);
2145 }
2146 if (myElementStack.size() >= 2 && myElementStack[myElementStack.size() - 2] == OPENDRIVE_TAG_LANE) {
2147 int no = attrs.get<int>(OPENDRIVE_ATTR_ID, myCurrentEdge.id.c_str(), ok);
2148 OpenDriveLane& l = myCurrentEdge.laneSections.back().lanesByDir[myCurrentLaneDirection].back();
2149 l.predecessor = no;
2150 }
2151 }
2152 break;
2154 if (myElementStack.size() >= 2 && myElementStack[myElementStack.size() - 2] == OPENDRIVE_TAG_ROAD) {
2155 std::string elementType = attrs.get<std::string>(OPENDRIVE_ATTR_ELEMENTTYPE, myCurrentEdge.id.c_str(), ok);
2156 std::string elementID = attrs.get<std::string>(OPENDRIVE_ATTR_ELEMENTID, myCurrentEdge.id.c_str(), ok);
2157 std::string contactPoint = attrs.hasAttribute(OPENDRIVE_ATTR_CONTACTPOINT)
2158 ? attrs.get<std::string>(OPENDRIVE_ATTR_CONTACTPOINT, myCurrentEdge.id.c_str(), ok)
2159 : "start";
2160 addLink(OPENDRIVE_LT_SUCCESSOR, elementType, elementID, contactPoint);
2161 }
2162 if (myElementStack.size() >= 2 && myElementStack[myElementStack.size() - 2] == OPENDRIVE_TAG_LANE) {
2163 int no = attrs.get<int>(OPENDRIVE_ATTR_ID, myCurrentEdge.id.c_str(), ok);
2164 OpenDriveLane& l = myCurrentEdge.laneSections.back().lanesByDir[myCurrentLaneDirection].back();
2165 l.successor = no;
2166 }
2167 }
2168 break;
2170 double length = attrs.get<double>(OPENDRIVE_ATTR_LENGTH, myCurrentEdge.id.c_str(), ok);
2171 double s = attrs.get<double>(OPENDRIVE_ATTR_S, myCurrentEdge.id.c_str(), ok);
2172 double x = attrs.get<double>(OPENDRIVE_ATTR_X, myCurrentEdge.id.c_str(), ok);
2173 double y = attrs.get<double>(OPENDRIVE_ATTR_Y, myCurrentEdge.id.c_str(), ok);
2174 double hdg = attrs.get<double>(OPENDRIVE_ATTR_HDG, myCurrentEdge.id.c_str(), ok);
2175 myCurrentEdge.geometries.push_back(OpenDriveGeometry(length, s, x, y, hdg));
2176 }
2177 break;
2179 double s = attrs.get<double>(OPENDRIVE_ATTR_S, myCurrentEdge.id.c_str(), ok);
2180 double a = attrs.get<double>(OPENDRIVE_ATTR_A, myCurrentEdge.id.c_str(), ok);
2181 double b = attrs.get<double>(OPENDRIVE_ATTR_B, myCurrentEdge.id.c_str(), ok);
2182 double c = attrs.get<double>(OPENDRIVE_ATTR_C, myCurrentEdge.id.c_str(), ok);
2183 double d = attrs.get<double>(OPENDRIVE_ATTR_D, myCurrentEdge.id.c_str(), ok);
2184 myCurrentEdge.elevations.push_back(OpenDriveElevation(s, a, b, c, d));
2185 }
2186 break;
2187 case OPENDRIVE_TAG_LINE: {
2188 if (myElementStack.size() > 0 && myElementStack.back() == OPENDRIVE_TAG_GEOMETRY) {
2189 std::vector<double> vals;
2191 }
2192 }
2193 break;
2194 case OPENDRIVE_TAG_SPIRAL: {
2195 std::vector<double> vals;
2196 vals.push_back(attrs.get<double>(OPENDRIVE_ATTR_CURVSTART, myCurrentEdge.id.c_str(), ok));
2197 vals.push_back(attrs.get<double>(OPENDRIVE_ATTR_CURVEND, myCurrentEdge.id.c_str(), ok));
2199 }
2200 break;
2201 case OPENDRIVE_TAG_ARC: {
2202 std::vector<double> vals;
2203 vals.push_back(attrs.get<double>(OPENDRIVE_ATTR_CURVATURE, myCurrentEdge.id.c_str(), ok));
2205 }
2206 break;
2207 case OPENDRIVE_TAG_POLY3: {
2208 std::vector<double> vals;
2209 vals.push_back(attrs.get<double>(OPENDRIVE_ATTR_A, myCurrentEdge.id.c_str(), ok));
2210 vals.push_back(attrs.get<double>(OPENDRIVE_ATTR_B, myCurrentEdge.id.c_str(), ok));
2211 vals.push_back(attrs.get<double>(OPENDRIVE_ATTR_C, myCurrentEdge.id.c_str(), ok));
2212 vals.push_back(attrs.get<double>(OPENDRIVE_ATTR_D, myCurrentEdge.id.c_str(), ok));
2214 }
2215 break;
2217 std::vector<double> vals;
2218 vals.push_back(attrs.get<double>(OPENDRIVE_ATTR_AU, myCurrentEdge.id.c_str(), ok));
2219 vals.push_back(attrs.get<double>(OPENDRIVE_ATTR_BU, myCurrentEdge.id.c_str(), ok));
2220 vals.push_back(attrs.get<double>(OPENDRIVE_ATTR_CU, myCurrentEdge.id.c_str(), ok));
2221 vals.push_back(attrs.get<double>(OPENDRIVE_ATTR_DU, myCurrentEdge.id.c_str(), ok));
2222 vals.push_back(attrs.get<double>(OPENDRIVE_ATTR_AV, myCurrentEdge.id.c_str(), ok));
2223 vals.push_back(attrs.get<double>(OPENDRIVE_ATTR_BV, myCurrentEdge.id.c_str(), ok));
2224 vals.push_back(attrs.get<double>(OPENDRIVE_ATTR_CV, myCurrentEdge.id.c_str(), ok));
2225 vals.push_back(attrs.get<double>(OPENDRIVE_ATTR_DV, myCurrentEdge.id.c_str(), ok));
2226 const std::string pRange = attrs.getOpt<std::string>(OPENDRIVE_ATTR_PRANGE, myCurrentEdge.id.c_str(), ok, "normalized", false);
2227 if (pRange == "normalized") {
2228 vals.push_back(1.0);
2229 } else if (pRange == "arcLength") {
2230 vals.push_back(-1.0);
2231 } else {
2232 WRITE_WARNING("Ignoring invalid pRange value '" + pRange + "' for road '" + myCurrentEdge.id + "'.");
2233 vals.push_back(1.0);
2234 }
2236 }
2237 break;
2239 double s = attrs.get<double>(OPENDRIVE_ATTR_S, myCurrentEdge.id.c_str(), ok);
2240 if (myCurrentEdge.laneSections.size() > 0) {
2241 myCurrentEdge.laneSections.back().length = s - myCurrentEdge.laneSections.back().s;
2242 }
2244 // possibly updated by the next laneSection
2245 myCurrentEdge.laneSections.back().length = myCurrentEdge.length - s;
2246 }
2247 break;
2249 double s = attrs.get<double>(OPENDRIVE_ATTR_S, myCurrentEdge.id.c_str(), ok);
2250 double a = attrs.get<double>(OPENDRIVE_ATTR_A, myCurrentEdge.id.c_str(), ok);
2251 double b = attrs.get<double>(OPENDRIVE_ATTR_B, myCurrentEdge.id.c_str(), ok);
2252 double c = attrs.get<double>(OPENDRIVE_ATTR_C, myCurrentEdge.id.c_str(), ok);
2253 double d = attrs.get<double>(OPENDRIVE_ATTR_D, myCurrentEdge.id.c_str(), ok);
2254 myCurrentEdge.offsets.push_back(OpenDriveLaneOffset(s, a, b, c, d));
2255 }
2256 break;
2257 case OPENDRIVE_TAG_LEFT:
2259 break;
2262 break;
2265 break;
2266 case OPENDRIVE_TAG_LANE: {
2267 std::string type = attrs.get<std::string>(OPENDRIVE_ATTR_TYPE, myCurrentEdge.id.c_str(), ok);
2268 int id = attrs.get<int>(OPENDRIVE_ATTR_ID, myCurrentEdge.id.c_str(), ok);
2269 std::string level = attrs.hasAttribute(OPENDRIVE_ATTR_LEVEL)
2270 ? attrs.get<std::string>(OPENDRIVE_ATTR_LEVEL, myCurrentEdge.id.c_str(), ok)
2271 : "";
2273 ls.lanesByDir[myCurrentLaneDirection].push_back(OpenDriveLane(id, level, type));
2274 }
2275 break;
2276 case OPENDRIVE_TAG_SIGNAL: {
2277 std::string id = attrs.get<std::string>(OPENDRIVE_ATTR_ID, myCurrentEdge.id.c_str(), ok);
2278 std::string type = attrs.get<std::string>(OPENDRIVE_ATTR_TYPE, myCurrentEdge.id.c_str(), ok);
2279 std::string name = attrs.getOpt<std::string>(OPENDRIVE_ATTR_NAME, myCurrentEdge.id.c_str(), ok, "", false);
2280 const std::string orientation = attrs.get<std::string>(OPENDRIVE_ATTR_ORIENTATION, myCurrentEdge.id.c_str(), ok);
2281 int orientationCode = orientation == "-" ? -1 : orientation == "+" ? 1 : 0;
2282 double s = attrs.get<double>(OPENDRIVE_ATTR_S, myCurrentEdge.id.c_str(), ok);
2283 bool dynamic = attrs.get<std::string>(OPENDRIVE_ATTR_DYNAMIC, myCurrentEdge.id.c_str(), ok) == "no" ? false : true;
2284 OpenDriveSignal signal = OpenDriveSignal(id, type, name, orientationCode, dynamic, s);
2285 myCurrentEdge.signals.push_back(signal);
2286 mySignals[id] = signal;
2287 }
2288 break;
2290 std::string id = attrs.get<std::string>(OPENDRIVE_ATTR_ID, myCurrentEdge.id.c_str(), ok);
2291 const std::string orientation = attrs.get<std::string>(OPENDRIVE_ATTR_ORIENTATION, myCurrentEdge.id.c_str(), ok);
2292 int orientationCode = orientation == "-" ? -1 : orientation == "+" ? 1 : 0;
2293 double s = attrs.get<double>(OPENDRIVE_ATTR_S, myCurrentEdge.id.c_str(), ok);
2294 OpenDriveSignal signal = OpenDriveSignal(id, "", "", orientationCode, false, s);
2295 myCurrentEdge.signals.push_back(signal);
2296 }
2297 break;
2299 int fromLane = attrs.get<int>(OPENDRIVE_ATTR_FROMLANE, myCurrentEdge.id.c_str(), ok);
2300 int toLane = attrs.get<int>(OPENDRIVE_ATTR_TOLANE, myCurrentEdge.id.c_str(), ok);
2301 if (myElementStack.size() >= 1 && (myElementStack.back() == OPENDRIVE_TAG_SIGNAL
2303 myCurrentEdge.signals.back().minLane = fromLane;
2304 myCurrentEdge.signals.back().maxLane = toLane;
2305 }
2306 }
2307 break;
2309 myCurrentJunctionID = attrs.get<std::string>(OPENDRIVE_ATTR_ID, myCurrentJunctionID.c_str(), ok);
2310 break;
2312 std::string id = attrs.get<std::string>(OPENDRIVE_ATTR_ID, myCurrentJunctionID.c_str(), ok);
2315 std::string cp = attrs.get<std::string>(OPENDRIVE_ATTR_CONTACTPOINT, myCurrentJunctionID.c_str(), ok);
2317 myConnectionWasEmpty = true;
2318 }
2319 break;
2321 int from = attrs.get<int>(OPENDRIVE_ATTR_FROM, myCurrentJunctionID.c_str(), ok);
2322 int to = attrs.get<int>(OPENDRIVE_ATTR_TO, myCurrentJunctionID.c_str(), ok);
2323 Connection c;
2325 c.toEdge = myCurrentConnectingRoad;
2326 c.fromLane = from;
2327 c.toLane = to;
2328 c.fromCP = OPENDRIVE_CP_END;
2329 c.toCP = myCurrentContactPoint;
2330 c.all = false;
2331 if (myEdges.find(c.fromEdge) == myEdges.end()) {
2332 WRITE_ERROR("In laneLink-element: incoming road '" + c.fromEdge + "' is not known.");
2333 } else {
2334 OpenDriveEdge* e = myEdges.find(c.fromEdge)->second;
2335 e->connections.insert(c);
2336 myConnectionWasEmpty = false;
2337 }
2338 }
2339 break;
2340 case OPENDRIVE_TAG_WIDTH: {
2341 if (myElementStack.size() >= 2 && myElementStack[myElementStack.size() - 1] == OPENDRIVE_TAG_LANE) {
2342 const double s = attrs.get<double>(OPENDRIVE_ATTR_SOFFSET, myCurrentEdge.id.c_str(), ok);
2343 const double a = attrs.get<double>(OPENDRIVE_ATTR_A, myCurrentEdge.id.c_str(), ok);
2344 const double b = attrs.get<double>(OPENDRIVE_ATTR_B, myCurrentEdge.id.c_str(), ok);
2345 const double c = attrs.get<double>(OPENDRIVE_ATTR_C, myCurrentEdge.id.c_str(), ok);
2346 const double d = attrs.get<double>(OPENDRIVE_ATTR_D, myCurrentEdge.id.c_str(), ok);
2347 OpenDriveLane& l = myCurrentEdge.laneSections.back().lanesByDir[myCurrentLaneDirection].back();
2348 l.width = MAX2(l.width, a);
2349 l.widthData.push_back(OpenDriveWidth(s, a, b, c, d));
2350#ifdef DEBUG_VARIABLE_WIDTHS
2351 if (DEBUG_COND(&myCurrentEdge)) {
2352 std::cout << " road=" << myCurrentEdge.id
2353 << std::setprecision(gPrecision)
2354 << " junction=" << myCurrentEdge.junction
2355 << " section=" << myCurrentEdge.laneSections.size() - 1
2356 << " dir=" << myCurrentLaneDirection << " lane=" << l.id
2357 << " type=" << l.type
2358 << " width=" << l.width
2359 << " a=" << a
2360 << " b=" << b
2361 << " c=" << c
2362 << " d=" << d
2363 << " s=" << s
2364 << " entries=" << l.widthData.size()
2365 << "\n";
2366 }
2367#endif
2368 }
2369 }
2370 break;
2371 case OPENDRIVE_TAG_SPEED: {
2372 if (myElementStack.size() >= 2 && myElementStack[myElementStack.size() - 1] == OPENDRIVE_TAG_LANE) {
2373 double speed = attrs.get<double>(OPENDRIVE_ATTR_MAX, myCurrentEdge.id.c_str(), ok);
2374 double pos = attrs.get<double>(OPENDRIVE_ATTR_SOFFSET, myCurrentEdge.id.c_str(), ok);
2375 // required for xodr v1.4
2376 const std::string unit = attrs.getOpt<std::string>(OPENDRIVE_ATTR_UNIT, myCurrentEdge.id.c_str(), ok, "", false);
2377 // now convert the speed to reasonable default SI [m/s]
2378 if (!unit.empty()) {
2379 // something to be done at all ?
2380 if (unit == "km/h") {
2381 speed /= 3.6;
2382 }
2383 if (unit == "mph") {
2384 speed *= 1.609344 / 3.6;
2385 }
2386 // IGNORING unknown units.
2387 }
2388 myCurrentEdge.laneSections.back().lanesByDir[myCurrentLaneDirection].back().speeds.push_back(std::make_pair(pos, speed));
2389 }
2390 }
2391 break;
2392 case OPENDRIVE_TAG_OBJECT: {
2393 if (!attrs.hasAttribute(OPENDRIVE_ATTR_ID)) {
2394 WRITE_WARNING("Ignoring object without id at edge '" + toString(myCurrentEdge.id) + "'.");
2395 break;
2396 }
2398 o.id = attrs.get<std::string>(OPENDRIVE_ATTR_ID, 0, ok);
2399 o.type = attrs.getOpt<std::string>(OPENDRIVE_ATTR_TYPE, o.id.c_str(), ok, "", false);
2400 o.name = attrs.getOpt<std::string>(OPENDRIVE_ATTR_NAME, o.id.c_str(), ok, "", false);
2401 o.s = attrs.get<double>(OPENDRIVE_ATTR_S, o.id.c_str(), ok);
2402 o.t = attrs.get<double>(OPENDRIVE_ATTR_T, o.id.c_str(), ok);
2403 o.width = attrs.getOpt<double>(OPENDRIVE_ATTR_WIDTH, o.id.c_str(), ok, -1);
2404 o.length = attrs.getOpt<double>(OPENDRIVE_ATTR_LENGTH, o.id.c_str(), ok, -1);
2405 o.radius = attrs.getOpt<double>(OPENDRIVE_ATTR_RADIUS, o.id.c_str(), ok, -1);
2406 o.hdg = attrs.getOpt<double>(OPENDRIVE_ATTR_HDG, o.id.c_str(), ok, 0);
2407 myCurrentEdge.objects.push_back(o);
2408 }
2409 break;
2410 case OPENDRIVE_TAG_REPEAT: {
2411 if (myCurrentEdge.objects.empty()) {
2412 WRITE_ERROR("Repeat without object at edge '" + toString(myCurrentEdge.id) + "'.");
2413 ok = false;
2414 } else {
2416 const std::string baseID = o.id;
2417 double dist = attrs.get<double>(OPENDRIVE_ATTR_DISTANCE, o.id.c_str(), ok);
2418 if (dist == 0) {
2419 // continuous feature. Split into parts (XXX exmport as a single polygon #5235)
2420 dist = OptionsCont::getOptions().getFloat("opendrive.curve-resolution");
2421 }
2422
2423 myCurrentEdge.objects.pop_back();
2424 const double length = attrs.get<double>(OPENDRIVE_ATTR_LENGTH, o.id.c_str(), ok);
2425 o.s = attrs.getOpt<double>(OPENDRIVE_ATTR_S, o.id.c_str(), ok, o.s);
2426 double wStart = attrs.getOpt<double>(OPENDRIVE_ATTR_WIDTHSTART, o.id.c_str(), ok, o.width);
2427 double wEnd = attrs.getOpt<double>(OPENDRIVE_ATTR_WIDTHEND, o.id.c_str(), ok, o.width);
2428 double tStart = attrs.getOpt<double>(OPENDRIVE_ATTR_TSTART, o.id.c_str(), ok, o.t);
2429 double tEnd = attrs.getOpt<double>(OPENDRIVE_ATTR_TEND, o.id.c_str(), ok, o.t);
2430 int index = 0;
2431 for (double x = 0; x <= length + NUMERICAL_EPS; x += dist) {
2432 o.id = baseID + "#" + toString(index++);
2433 const double a = x / length;
2434 o.width = wStart * (1 - a) + wEnd * a;
2435 o.t = tStart * (1 - a) + tEnd * a;
2436 myCurrentEdge.objects.push_back(o);
2437 o.s += dist;
2438 }
2439 }
2440 }
2441 break;
2442 default:
2443 break;
2444 }
2445 myElementStack.push_back(element);
2446}
2447
2448
2449void
2450NIImporter_OpenDrive::myCharacters(int element, const std::string& cdata) {
2451 if (element == OPENDRIVE_TAG_GEOREFERENCE) {
2452 size_t i = cdata.find("+proj");
2453 if (i != std::string::npos) {
2454 const std::string proj = cdata.substr(i);
2455 if (proj != "") {
2456 GeoConvHelper* result = nullptr;
2457 Boundary convBoundary;
2458 Boundary origBoundary;
2459 // XXX read values from the header
2460 convBoundary.add(Position(0, 0));
2461 origBoundary.add(Position(0, 0));
2462 try {
2463 result = new GeoConvHelper(proj, myOffset, origBoundary, convBoundary);
2464 GeoConvHelper::setLoaded(*result);
2465 } catch (ProcessError& e) {
2466 WRITE_ERROR("Could not set projection (" + std::string(e.what()) + "). This can be ignored with --ignore-errors.");
2467 }
2468 }
2469 } else {
2470 WRITE_WARNING("geoReference format '" + cdata + "' currently not supported");
2471 }
2472 needsCharacterData(false);
2473 }
2474}
2475
2476
2477void
2479 myElementStack.pop_back();
2480 switch (element) {
2481 case OPENDRIVE_TAG_ROAD:
2483 break;
2486 Connection c;
2489 c.fromLane = 0;
2490 c.toLane = 0;
2493 c.all = true;
2494 if (myEdges.find(c.fromEdge) == myEdges.end()) {
2495 WRITE_ERROR("In laneLink-element: incoming road '" + c.fromEdge + "' is not known.");
2496 } else {
2497 OpenDriveEdge* e = myEdges.find(c.fromEdge)->second;
2498 e->connections.insert(c);
2499 }
2500 }
2501 break;
2503 myCurrentEdge.laneSections.back().buildLaneMapping(myTypeContainer);
2504 }
2505 break;
2506 default:
2507 break;
2508 }
2509}
2510
2511
2512
2513void
2514NIImporter_OpenDrive::addLink(LinkType lt, const std::string& elementType,
2515 const std::string& elementID,
2516 const std::string& contactPoint) {
2517 OpenDriveLink l(lt, elementID);
2518 // elementType
2519 if (elementType == "road") {
2521 } else if (elementType == "junction") {
2523 }
2524 // contact point
2525 if (contactPoint == "start") {
2527 } else if (contactPoint == "end") {
2529 }
2530 // add
2531 myCurrentEdge.links.push_back(l);
2532}
2533
2534
2535void
2536NIImporter_OpenDrive::addGeometryShape(GeometryType type, const std::vector<double>& vals) {
2537 // checks
2538 if (myCurrentEdge.geometries.size() == 0) {
2539 throw ProcessError("Mismatching paranthesis in geometry definition for road '" + myCurrentEdge.id + "'");
2540 }
2542 if (last.type != OPENDRIVE_GT_UNKNOWN) {
2543 throw ProcessError("Double geometry information for road '" + myCurrentEdge.id + "'");
2544 }
2545 // set
2546 last.type = type;
2547 last.params = vals;
2548}
2549
2550
2551bool
2553 if (c1.fromEdge != c2.fromEdge) {
2554 return c1.fromEdge < c2.fromEdge;
2555 }
2556 if (c1.toEdge != c2.toEdge) {
2557 return c1.toEdge < c2.toEdge;
2558 }
2559 if (c1.fromLane != c2.fromLane) {
2560 return c1.fromLane < c2.fromLane;
2561 }
2562 return c1.toLane < c2.toLane;
2563}
2564
2565void
2567#ifdef DEBUG_VARIABLE_WIDTHS
2568 if (DEBUG_COND(e)) {
2569 gDebugFlag1 = true;
2570 std::cout << "sanitizeWidths e=" << e->id << " sections=" << e->laneSections.size() << "\n";
2571 }
2572#endif
2573 for (OpenDriveLaneSection& sec : e->laneSections) {
2574 // filter widths within the current section (#5888).
2575 // @note, Short laneSections could also be worth filtering alltogether
2576 if (sec.rightLaneNumber > 0) {
2578 }
2579 if (sec.leftLaneNumber > 0) {
2581 }
2582 }
2583}
2584
2585void
2586NIImporter_OpenDrive::sanitizeWidths(std::vector<OpenDriveLane>& lanes, double length) {
2587 for (OpenDriveLane& l : lanes) {
2588 if (l.widthData.size() > 0) {
2589 auto& wd = l.widthData;
2590 const double threshold = POSITION_EPS;
2591 double maxNoShort = -std::numeric_limits<double>::max();
2592 double seen = 0;
2593 for (int i = 0; i < (int)wd.size(); i++) {
2594 const double wdLength = i < (int)wd.size() - 1 ? wd[i + 1].s - wd[i].s : length - seen;
2595 seen += wdLength;
2596 if (wdLength > threshold) {
2597 maxNoShort = MAX2(maxNoShort, wd[i].a);
2598 }
2599 }
2600 if (maxNoShort > 0) {
2601 l.width = maxNoShort;
2602 }
2603 }
2604 }
2605}
2606
2607
2608void
2610 std::vector<OpenDriveLaneSection> newSections;
2611#ifdef DEBUG_VARIABLE_WIDTHS
2612 if (DEBUG_COND(e)) {
2613 gDebugFlag1 = true;
2614 std::cout << "splitMinWidths e=" << e->id << " sections=" << e->laneSections.size() << "\n";
2615 }
2616#endif
2617 for (std::vector<OpenDriveLaneSection>::iterator j = e->laneSections.begin(); j != e->laneSections.end(); ++j) {
2618 OpenDriveLaneSection& sec = *j;
2619 std::vector<double> splitPositions;
2620 const double sectionEnd = (j + 1) == e->laneSections.end() ? e->length : (*(j + 1)).s;
2621 const int section = (int)(j - e->laneSections.begin());
2622#ifdef DEBUG_VARIABLE_WIDTHS
2623 if (DEBUG_COND(e)) {
2624 std::cout << " findWidthSplit section=" << section << " sectionStart=" << sec.s << " sectionOrigStart=" << sec.sOrig << " sectionEnd=" << sectionEnd << "\n";
2625 }
2626#endif
2627 if (sec.rightLaneNumber > 0) {
2628 findWidthSplit(tc, sec.lanesByDir[OPENDRIVE_TAG_RIGHT], section, sec.sOrig, sectionEnd, splitPositions);
2629 }
2630 if (sec.leftLaneNumber > 0) {
2631 findWidthSplit(tc, sec.lanesByDir[OPENDRIVE_TAG_LEFT], section, sec.sOrig, sectionEnd, splitPositions);
2632 }
2633 newSections.push_back(sec);
2634 std::sort(splitPositions.begin(), splitPositions.end());
2635 // filter out tiny splits
2636 double prevSplit = sec.s;
2637 for (std::vector<double>::iterator it = splitPositions.begin(); it != splitPositions.end();) {
2638 if ((*it) - prevSplit < minDist || sectionEnd - (*it) < minDist) {
2639 // avoid tiny (or duplicate) splits
2640#ifdef DEBUG_VARIABLE_WIDTHS
2641 if (DEBUG_COND(e)) {
2642 std::cout << " skip close split=" << (*it) << " prevSplit=" << prevSplit << "\n";
2643 }
2644#endif
2645 it = splitPositions.erase(it);
2646 } else if ((*it) < sec.s) {
2647 // avoid splits for another section
2648#ifdef DEBUG_VARIABLE_WIDTHS
2649 if (DEBUG_COND(e)) {
2650 std::cout << " skip early split=" << (*it) << " s=" << sec.s << "\n";
2651 }
2652#endif
2653 it = splitPositions.erase(it);
2654 } else {
2655 prevSplit = *it;
2656 it++;
2657 }
2658 }
2659
2660 if (splitPositions.size() > 0) {
2661#ifdef DEBUG_VARIABLE_WIDTHS
2662 if (DEBUG_COND(e)) {
2663 std::cout << " road=" << e->id << " splitMinWidths section=" << section
2664 << " start=" << sec.s
2665 << " origStart=" << sec.sOrig
2666 << " end=" << sectionEnd << " minDist=" << minDist
2667 << " splitPositions=" << toString(splitPositions) << "\n";
2668 }
2669#endif
2670#ifdef DEBUG_VARIABLE_WIDTHS
2671 if (DEBUG_COND(e)) {
2672 std::cout << "first section...\n";
2673 }
2674#endif
2675 recomputeWidths(newSections.back(), sec.sOrig, splitPositions.front(), sec.sOrig, sectionEnd);
2676 for (std::vector<double>::iterator it = splitPositions.begin(); it != splitPositions.end(); ++it) {
2677 OpenDriveLaneSection secNew = sec;
2678 secNew.s = *it;
2679#ifdef DEBUG_VARIABLE_WIDTHS
2680 if (DEBUG_COND(e)) {
2681 std::cout << "splitAt " << secNew.s << "\n";
2682 }
2683#endif
2684 newSections.push_back(secNew);
2685 if (secNew.rightLaneNumber > 0) {
2686 setStraightConnections(newSections.back().lanesByDir[OPENDRIVE_TAG_RIGHT]);
2687 }
2688 if (secNew.leftLaneNumber > 0) {
2689 setStraightConnections(newSections.back().lanesByDir[OPENDRIVE_TAG_LEFT]);
2690 }
2691 double end = (it + 1) == splitPositions.end() ? sectionEnd : *(it + 1);
2692 recomputeWidths(newSections.back(), secNew.s, end, sec.sOrig, sectionEnd);
2693 }
2694 }
2695 }
2696 gDebugFlag1 = false;
2697 e->laneSections = newSections;
2698}
2699
2700
2701void
2702NIImporter_OpenDrive::findWidthSplit(const NBTypeCont& tc, std::vector<OpenDriveLane>& lanes,
2703 int section, double sectionStart, double sectionEnd,
2704 std::vector<double>& splitPositions) {
2705 UNUSED_PARAMETER(section);
2706 for (std::vector<OpenDriveLane>::iterator k = lanes.begin(); k != lanes.end(); ++k) {
2707 OpenDriveLane& l = *k;
2709 if (l.widthData.size() > 0 && tc.knows(l.type) && !tc.getEdgeTypeShallBeDiscarded(l.type) && permissions != 0) {
2710 double sPrev = l.widthData.front().s;
2711 double wPrev = l.widthData.front().computeAt(sPrev);
2712 if (gDebugFlag1) std::cout
2713 << "findWidthSplit section=" << section
2714 << " sectionStart=" << sectionStart
2715 << " sectionEnd=" << sectionEnd
2716 << " lane=" << l.id
2717 << " type=" << l.type
2718 << " widthEntries=" << l.widthData.size() << "\n"
2719 << " s=" << sPrev
2720 << " w=" << wPrev
2721 << "\n";
2722 for (std::vector<OpenDriveWidth>::iterator it_w = l.widthData.begin(); it_w != l.widthData.end(); ++it_w) {
2723 double sEnd = (it_w + 1) != l.widthData.end() ? (*(it_w + 1)).s : sectionEnd - sectionStart;
2724 double w = (*it_w).computeAt(sEnd);
2725 if (gDebugFlag1) std::cout
2726 << " sEnd=" << sEnd
2727 << " s=" << (*it_w).s
2728 << " a=" << (*it_w).a << " b=" << (*it_w).b << " c=" << (*it_w).c << " d=" << (*it_w).d
2729 << " w=" << w
2730 << "\n";
2731 const double changeDist = fabs(myMinWidth - wPrev);
2732 if (((wPrev < myMinWidth) && (w > myMinWidth))
2733 || ((wPrev > myMinWidth) && (w < myMinWidth))) {
2734 double splitPos = sPrev + (sEnd - sPrev) / fabs(w - wPrev) * changeDist;
2735 double wSplit = (*it_w).computeAt(splitPos);
2736 if (gDebugFlag1) {
2737 std::cout << " candidate splitPos=" << splitPos << " w=" << wSplit << "\n";
2738 }
2739 // ensure that the thin part is actually thin enough
2740 while (wSplit > myMinWidth) {
2741 if (wPrev < myMinWidth) {
2742 // getting wider
2743 splitPos -= POSITION_EPS;
2744 if (splitPos < sPrev) {
2745 if (gDebugFlag1) {
2746 std::cout << " aborting search splitPos=" << splitPos << " wSplit=" << wSplit << " sPrev=" << sPrev << " wPrev=" << wPrev << "\n";
2747 }
2748 splitPos = sPrev;
2749 break;
2750 }
2751 } else {
2752 // getting thinner
2753 splitPos += POSITION_EPS;
2754 if (splitPos > sEnd) {
2755 if (gDebugFlag1) {
2756 std::cout << " aborting search splitPos=" << splitPos << " wSplit=" << wSplit << " sEnd=" << sEnd << " w=" << w << "\n";
2757 }
2758 splitPos = sEnd;
2759 break;
2760 }
2761 }
2762 wSplit = (*it_w).computeAt(splitPos);
2763 if (gDebugFlag1) {
2764 std::cout << " refined splitPos=" << splitPos << " w=" << wSplit << "\n";
2765 }
2766 }
2767 splitPositions.push_back(sectionStart + splitPos);
2768 }
2769 // //wPrev = wSplit;
2770 //} else if ((fabs(wPrev) < NUMERICAL_EPS && w > POSITION_EPS)
2771 // || (wPrev > POSITION_EPS && fabs(w) < NUMERICAL_EPS)) {
2772 // splitPositions.push_back(sectionStart + sPrev);
2773 // if (gDebugFlag1) std::cout << " laneDisappears candidate splitPos=" << sPrev << " wPrev=" << wPrev << " w=" << w<< "\n";
2774 //}
2775 wPrev = w;
2776 sPrev = sEnd;
2777 }
2778 }
2779 }
2780}
2781
2782
2783void
2784NIImporter_OpenDrive::setStraightConnections(std::vector<OpenDriveLane>& lanes) {
2785 for (std::vector<OpenDriveLane>::iterator k = lanes.begin(); k != lanes.end(); ++k) {
2786 (*k).predecessor = (*k).id;
2787 }
2788}
2789
2790
2791void
2792NIImporter_OpenDrive::recomputeWidths(OpenDriveLaneSection& sec, double start, double end, double sectionStart, double sectionEnd) {
2793 if (sec.rightLaneNumber > 0) {
2794 recomputeWidths(sec.lanesByDir[OPENDRIVE_TAG_RIGHT], start, end, sectionStart, sectionEnd);
2795 }
2796 if (sec.leftLaneNumber > 0) {
2797 recomputeWidths(sec.lanesByDir[OPENDRIVE_TAG_LEFT], start, end, sectionStart, sectionEnd);
2798 }
2799}
2800
2801
2802void
2803NIImporter_OpenDrive::recomputeWidths(std::vector<OpenDriveLane>& lanes, double start, double end, double sectionStart, double sectionEnd) {
2804 for (std::vector<OpenDriveLane>::iterator k = lanes.begin(); k != lanes.end(); ++k) {
2805 OpenDriveLane& l = *k;
2806 if (l.widthData.size() > 0) {
2807#ifdef DEBUG_VARIABLE_WIDTHS
2808 if (gDebugFlag1) std::cout
2809 << "recomputeWidths lane=" << l.id
2810 << " type=" << l.type
2811 << " start=" << start
2812 << " end=" << end
2813 << " sectionStart=" << sectionStart
2814 << " sectionEnd=" << sectionEnd
2815 << " widthEntries=" << l.widthData.size() << "\n"
2816 << "\n";
2817#endif
2818 l.width = 0;
2819 double sPrev = l.widthData.front().s;
2820 double sPrevAbs = sPrev + sectionStart;
2821 for (std::vector<OpenDriveWidth>::iterator it_w = l.widthData.begin(); it_w != l.widthData.end(); ++it_w) {
2822 double sEnd = (it_w + 1) != l.widthData.end() ? (*(it_w + 1)).s : sectionEnd - sectionStart;
2823 double sEndAbs = sEnd + sectionStart;
2824#ifdef DEBUG_VARIABLE_WIDTHS
2825 if (gDebugFlag1) std::cout
2826 << " sPrev=" << sPrev << " sPrevAbs=" << sPrevAbs
2827 << " sEnd=" << sEnd << " sEndAbs=" << sEndAbs
2828 << " widthData s=" << (*it_w).s
2829 << " a=" << (*it_w).a
2830 << " b=" << (*it_w).b
2831 << " c=" << (*it_w).c
2832 << " d=" << (*it_w).d
2833 << "\n";
2834#endif
2835 if (sPrevAbs <= start && sEndAbs >= start) {
2836#ifdef DEBUG_VARIABLE_WIDTHS
2837 if (gDebugFlag1) {
2838 std::cout << " atStart=" << start << " pos=" << start - sectionStart << " w=" << (*it_w).computeAt(start - sectionStart) << "\n";
2839 }
2840#endif
2841 l.width = MAX2(l.width, (*it_w).computeAt(start - sectionStart));
2842 }
2843 if (sPrevAbs <= end && sEndAbs >= end) {
2844#ifdef DEBUG_VARIABLE_WIDTHS
2845 if (gDebugFlag1) {
2846 std::cout << " atEnd=" << end << " pos=" << end - sectionStart << " w=" << (*it_w).computeAt(end - sectionStart) << "\n";
2847 }
2848#endif
2849 l.width = MAX2(l.width, (*it_w).computeAt(end - sectionStart));
2850 }
2851 if (start <= sPrevAbs && end >= sPrevAbs) {
2852#ifdef DEBUG_VARIABLE_WIDTHS
2853 if (gDebugFlag1) {
2854 std::cout << " atSPrev=" << sPrev << " w=" << (*it_w).computeAt(sPrev) << "\n";
2855 }
2856#endif
2857 l.width = MAX2(l.width, (*it_w).computeAt(sPrev));
2858 }
2859 if (start <= sEndAbs && end >= sEndAbs) {
2860#ifdef DEBUG_VARIABLE_WIDTHS
2861 if (gDebugFlag1) {
2862 std::cout << " atSEnd=" << sEnd << " w=" << (*it_w).computeAt(sEnd) << "\n";
2863 }
2864#endif
2865 l.width = MAX2(l.width, (*it_w).computeAt(sEnd));
2866 }
2867#ifdef DEBUG_VARIABLE_WIDTHS
2868 if (gDebugFlag1) {
2869 std::cout << " sPrev=" << sPrev << " sEnd=" << sEnd << " l.width=" << l.width << "\n";
2870 }
2871#endif
2872 sPrev = sEnd;
2873 sPrevAbs = sEndAbs;
2874 }
2875 }
2876 }
2877}
2878
2879
2880/****************************************************************************/
#define WRITE_WARNINGF(...)
Definition: MsgHandler.h:266
#define WRITE_ERROR(msg)
Definition: MsgHandler.h:274
#define WRITE_WARNING(msg)
Definition: MsgHandler.h:265
#define TL(string)
Definition: MsgHandler.h:282
#define PROGRESS_DONE_MESSAGE()
Definition: MsgHandler.h:270
#define PROGRESS_BEGIN_MESSAGE(msg)
Definition: MsgHandler.h:269
std::set< NBNode *, ComparatorIdLess > NodeSet
Definition: NBCont.h:52
@ KEEPCLEAR_UNSPECIFIED
Definition: NBCont.h:61
#define DEBUG_COND3(roadID)
#define DEBUG_COND(road)
bool operator<(const NIImporter_OpenDrive::Connection &c1, const NIImporter_OpenDrive::Connection &c2)
#define DEBUG_COND2(edgeID)
#define UNSET_CONNECTION
@ SVC_PASSENGER
vehicle is a passenger car (a "normal" car)
@ SVC_BICYCLE
vehicle is a bicycle
@ SVC_EMERGENCY
public emergency vehicles
@ SVC_AUTHORITY
authorities vehicles
@ SVC_PEDESTRIAN
pedestrian
int SVCPermissions
bitset where each bit declares whether a certain SVC may use this edge/lane
TrafficLightType
const std::string SUMO_PARAM_ORIGID
int gPrecision
the precision for floating point outputs
Definition: StdDefs.cpp:25
bool gDebugFlag1
global utility flags for debugging
Definition: StdDefs.cpp:33
#define UNUSED_PARAMETER(x)
Definition: StdDefs.h:30
T MIN2(T a, T b)
Definition: StdDefs.h:71
T MAX2(T a, T b)
Definition: StdDefs.h:77
std::string joinToString(const std::vector< T > &v, const T_BETWEEN &between, std::streamsize accuracy=gPrecision)
Definition: ToString.h:282
std::string toString(const T &t, std::streamsize accuracy=gPrecision)
Definition: ToString.h:46
A class that stores a 2D geometrical boundary.
Definition: Boundary.h:39
void add(double x, double y, double z=0)
Makes the boundary include the given coordinate.
Definition: Boundary.cpp:78
static bool isReadable(std::string path)
Checks whether the given file is readable.
Definition: FileHelpers.cpp:51
A handler which converts occuring elements and attributes into enums.
void needsCharacterData(const bool value=true)
void error(const XERCES_CPP_NAMESPACE::SAXParseException &exception)
Handler for XML-errors.
void setFileName(const std::string &name)
Sets the current file name.
static methods for processing the coordinates conversion for the current net
Definition: GeoConvHelper.h:53
void cartesian2geo(Position &cartesian) const
Converts the given cartesian (shifted) position to its geo (lat/long) representation.
void moveConvertedBy(double x, double y)
Shifts the converted boundary by the given amounts.
static int getNumLoaded()
Definition: GeoConvHelper.h:93
static void setLoaded(const GeoConvHelper &loaded)
sets the coordinate transformation loaded from a location element
bool usingGeoProjection() const
Returns whether a transformation from geo to metric coordinates will be performed.
static GeoConvHelper & getLoaded()
the coordinate transformation that was loaded fron an input file
Definition: GeoConvHelper.h:89
static double naviDegree(const double angle)
Definition: GeomHelper.cpp:192
NBEdge * retrieve(const std::string &id, bool retrieveExtracted=false) const
Returns the edge that has the given id.
Definition: NBEdgeCont.cpp:274
bool wasIgnored(std::string id) const
Returns whether the edge with the id was ignored during parsing.
Definition: NBEdgeCont.h:495
bool insert(NBEdge *edge, bool ignorePrunning=false)
Adds an edge to the dictionary.
Definition: NBEdgeCont.cpp:177
The representation of a single edge during network building.
Definition: NBEdge.h:92
const std::vector< Connection > & getConnections() const
Returns the connections.
Definition: NBEdge.h:1043
NBNode * getToNode() const
Returns the destination node of the edge.
Definition: NBEdge.h:552
static const double UNSPECIFIED_FRICTION
unspecified lane friction
Definition: NBEdge.h:366
Lane & getLaneStruct(int lane)
Definition: NBEdge.h:1426
bool addLane2LaneConnection(int fromLane, NBEdge *dest, int toLane, Lane2LaneInfoType type, bool mayUseSameDestination=false, bool mayDefinitelyPass=false, KeepClear keepClear=KEEPCLEAR_UNSPECIFIED, double contPos=UNSPECIFIED_CONTPOS, double visibility=UNSPECIFIED_VISIBILITY_DISTANCE, double speed=UNSPECIFIED_SPEED, double friction=UNSPECIFIED_FRICTION, double length=myDefaultConnectionLength, const PositionVector &customShape=PositionVector::EMPTY, const bool uncontrolled=UNSPECIFIED_CONNECTION_UNCONTROLLED, SVCPermissions permissions=SVC_UNSPECIFIED, const bool indirectLeft=false, const std::string &edgeType="", SVCPermissions changeLeft=SVC_UNSPECIFIED, SVCPermissions changeRight=SVC_UNSPECIFIED, bool postProcess=false)
Adds a connection between the specified this edge's lane and an approached one.
Definition: NBEdge.cpp:1092
const std::string & getID() const
Definition: NBEdge.h:1526
static const double UNSPECIFIED_LOADED_LENGTH
no length override given
Definition: NBEdge.h:375
static const double UNSPECIFIED_CONTPOS
unspecified internal junction position
Definition: NBEdge.h:369
static const double UNSPECIFIED_VISIBILITY_DISTANCE
unspecified foe visibility for connections
Definition: NBEdge.h:372
static const double UNSPECIFIED_SPEED
unspecified lane speed
Definition: NBEdge.h:363
@ USER
The connection was given by the user.
@ VALIDATED
The connection was computed and validated.
NBNode * getFromNode() const
Returns the origin node of the edge.
Definition: NBEdge.h:545
static const double UNSPECIFIED_WIDTH
unspecified lane width
Definition: NBEdge.h:357
static const double UNSPECIFIED_OFFSET
unspecified lane offset
Definition: NBEdge.h:360
Instance responsible for building networks.
Definition: NBNetBuilder.h:107
static bool transformCoordinates(PositionVector &from, bool includeInBoundary=true, GeoConvHelper *from_srs=nullptr)
NBNodeCont & getNodeCont()
Returns a reference to the node container.
Definition: NBNetBuilder.h:144
NBEdgeCont & getEdgeCont()
Definition: NBNetBuilder.h:139
NBTypeCont & getTypeCont()
Returns a reference to the type container.
Definition: NBNetBuilder.h:149
NBTrafficLightLogicCont & getTLLogicCont()
Returns a reference to the traffic light logics container.
Definition: NBNetBuilder.h:154
Container for nodes during the netbuilding process.
Definition: NBNodeCont.h:58
std::string createClusterId(const NodeSet &cluster, const std::string &prefix="cluster_")
generate id from cluster node ids
Definition: NBNodeCont.h:136
bool insert(const std::string &id, const Position &position, NBDistrict *district=0)
Inserts a node into the map.
Definition: NBNodeCont.cpp:91
NBNode * retrieve(const std::string &id) const
Returns the node with the given name.
Definition: NBNodeCont.cpp:120
bool extract(NBNode *node, bool remember=false)
Removes the given node but does not delete it.
Definition: NBNodeCont.cpp:161
Represents a single node (junction) during network building.
Definition: NBNode.h:66
const std::set< NBTrafficLightDefinition * > & getControllingTLS() const
Returns the traffic lights that were assigned to this node (The set of tls that control this node)
Definition: NBNode.h:326
void addTrafficLight(NBTrafficLightDefinition *tlDef)
Adds a traffic light to the list of traffic lights that control this node.
Definition: NBNode.cpp:365
bool isTLControlled() const
Returns whether this node is controlled by any tls.
Definition: NBNode.h:321
A traffic light logics which must be computed (only nodes/edges are given)
Definition: NBOwnTLDef.h:44
The base class for traffic light logic definitions.
bool insert(NBTrafficLightDefinition *logic, bool forceInsert=false)
Adds a logic definition to the dictionary.
A storage for available edgeTypes of edges.
Definition: NBTypeCont.h:52
double getEdgeTypeMaxWidth(const std::string &edgeType) const
Returns the maximum edge/lane widths of the given edgeType.
Definition: NBTypeCont.cpp:533
bool getEdgeTypeShallBeDiscarded(const std::string &edgeType) const
Returns the information whether edges of this edgeType shall be discarded.
Definition: NBTypeCont.cpp:523
double getEdgeTypeSpeed(const std::string &edgeType) const
Returns the maximal velocity for the given edgeType [m/s].
Definition: NBTypeCont.cpp:500
double getEdgeTypeWidth(const std::string &edgeType) const
Returns the lane width for the given edgeType [m].
Definition: NBTypeCont.cpp:561
SVCPermissions getEdgeTypePermissions(const std::string &edgeType) const
Returns allowed vehicle classes for the given edgeType.
Definition: NBTypeCont.cpp:549
double getEdgeTypeWidthResolution(const std::string &edgeType) const
Returns the resolution for interpreting edge/lane widths of the given edgeType.
Definition: NBTypeCont.cpp:528
bool knows(const std::string &edgeType) const
Returns whether the named edgeType is in the container.
Definition: NBTypeCont.cpp:303
A class for sorting lane sections by their s-value.
Importer for networks stored in openDrive format.
static void loadNetwork(const OptionsCont &oc, NBNetBuilder &nb)
Loads content of the optionally given SUMO file.
static void recomputeWidths(OpenDriveLaneSection &sec, double start, double end, double sectionStart, double sectionEnd)
static std::vector< double > discretizeOffsets(PositionVector &geom, const std::vector< OpenDriveLaneOffset > &offsets, const std::string &id)
transform Poly3 into a list of offsets, adding intermediate points to geom if needed
static void addOffsets(bool left, PositionVector &geom, const std::vector< OpenDriveLaneOffset > &offsets, const std::string &id, std::vector< double > &result)
static std::pair< NBEdge *, NBEdge * > retrieveSignalEdges(NBNetBuilder &nb, const std::string &fromID, const std::string &toID, const std::string &junction)
static PositionVector geomFromParamPoly(const OpenDriveEdge &e, const OpenDriveGeometry &g, double resolution)
void myEndElement(int element)
Called when a closing tag occurs.
static void calcPointOnCurve(double *ad_x, double *ad_y, double ad_centerX, double ad_centerY, double ad_r, double ad_length)
OpenDriveXMLTag
Numbers representing openDrive-XML - element names.
void addGeometryShape(GeometryType type, const std::vector< double > &vals)
static void setStraightConnections(std::vector< OpenDriveLane > &lanes)
static void setLaneAttributes(const OpenDriveEdge *e, NBEdge::Lane &sumoLane, const OpenDriveLane &odLane, bool saveOrigIDs, const NBTypeCont &tc)
std::vector< int > myElementStack
static void buildConnectionsToOuter(const Connection &c, const std::map< std::string, OpenDriveEdge * > &innerEdges, const std::map< std::string, OpenDriveEdge * > &edges, const NBTypeCont &tc, std::vector< Connection > &into, std::set< Connection > &seen)
void myStartElement(int element, const SUMOSAXAttributes &attrs)
Called on the opening of a tag;.
static StringBijection< int >::Entry openDriveAttrs[]
The names of openDrive-XML attributes (for passing to GenericSAXHandler)
std::map< std::string, OpenDriveSignal > & getSignals()
std::map< std::string, OpenDriveSignal > mySignals
static bool laneSectionsConnected(OpenDriveEdge *edge, int in, int out)
void addLink(LinkType lt, const std::string &elementType, const std::string &elementID, const std::string &contactPoint)
static PositionVector geomFromSpiral(const OpenDriveEdge &e, const OpenDriveGeometry &g, double resolution)
static PositionVector geomFromLine(const OpenDriveEdge &e, const OpenDriveGeometry &g, double resolution)
static NBNode * getOrBuildNode(const std::string &id, const Position &pos, NBNodeCont &nc)
Builds a node or returns the already built.
const NBTypeCont & myTypeContainer
NIImporter_OpenDrive(const NBTypeCont &tc, std::map< std::string, OpenDriveEdge * > &edges)
Constructor.
static Position calculateStraightEndPoint(double hdg, double length, const Position &start)
OpenDriveXMLTag myCurrentLaneDirection
static PositionVector geomFromPoly(const OpenDriveEdge &e, const OpenDriveGeometry &g, double resolution)
static void revisitLaneSections(const NBTypeCont &tc, std::map< std::string, OpenDriveEdge * > &edges)
Rechecks lane sections of the given edges.
static void sanitizeWidths(OpenDriveEdge *e)
GeometryType
OpenDrive geometry type enumeration.
static void computeShapes(std::map< std::string, OpenDriveEdge * > &edges)
Computes a polygon representation of each edge's geometry.
static void calculateCurveCenter(double *ad_x, double *ad_y, double ad_radius, double ad_hdg)
static std::string revertID(const std::string &id)
static void setEdgeLinks2(OpenDriveEdge &e, const std::map< std::string, OpenDriveEdge * > &edges)
static void splitMinWidths(OpenDriveEdge *e, const NBTypeCont &tc, double minDist)
void myCharacters(int element, const std::string &chars)
Callback method for characters to implement by derived classes.
static NBTrafficLightDefinition * getTLSSecure(NBEdge *inEdge, NBNetBuilder &nb)
static StringBijection< int >::Entry openDriveTags[]
The names of openDrive-XML elements (for passing to GenericSAXHandler)
Poly3 OpenDriveElevation
LaneOffset has the same fields as Elevation.
ContactPoint myCurrentContactPoint
static void findWidthSplit(const NBTypeCont &tc, std::vector< OpenDriveLane > &lanes, int section, double sectionStart, double sectionEnd, std::vector< double > &splitPositions)
static PositionVector geomFromArc(const OpenDriveEdge &e, const OpenDriveGeometry &g, double resolution)
static void setNodeSecure(NBNodeCont &nc, OpenDriveEdge &e, const std::string &nodeID, NIImporter_OpenDrive::LinkType lt, std::vector< NodeSet > &joinedNodeIDs)
LinkType
OpenDrive link type enumeration.
static bool hasNonLinearElevation(OpenDriveEdge &e)
std::map< std::string, OpenDriveEdge * > & myEdges
const std::string & getID() const
Returns the id.
Definition: Named.h:74
A storage for options typed value containers)
Definition: OptionsCont.h:89
bool isSet(const std::string &name, bool failOnNonExistant=true) const
Returns the information whether the named option is set.
double getFloat(const std::string &name) const
Returns the double-value of the named option (only for Option_Float)
std::string getString(const std::string &name) const
Returns the string-value of the named option (only for Option_String)
bool isDefault(const std::string &name) const
Returns the information whether the named option has still the default value.
bool exists(const std::string &name) const
Returns the information whether the named option is known.
bool getBool(const std::string &name) const
Returns the boolean-value of the named option (only for Option_Bool)
const StringVector & getStringVector(const std::string &name) const
Returns the list of string-value of the named option (only for Option_StringVector)
static OptionsCont & getOptions()
Retrieves the options.
Definition: OptionsCont.cpp:59
bool isUsableFileList(const std::string &name) const
Checks whether the named option is usable as a file list (with at least a single file)
Static storage of an output device and its base (abstract) implementation.
Definition: OutputDevice.h:61
static OutputDevice & getDevice(const std::string &name, bool usePrefix=true)
Returns the described OutputDevice.
bool writeXMLHeader(const std::string &rootElement, const std::string &schemaFile, std::map< SumoXMLAttr, std::string > attrs=std::map< SumoXMLAttr, std::string >(), bool includeConfig=true)
Writes an XML header with optional configuration.
virtual void setParameter(const std::string &key, const std::string &value)
Sets a parameter.
A point-of-interest.
void writeXML(OutputDevice &out, const bool geo=false, const double zOffset=0., const std::string laneID="", const double pos=0., const bool friendlyPos=false, const double posLat=0.) const
A point in 2D or 3D with translation and scaling methods.
Definition: Position.h:37
void set(double x, double y)
set positions x and y
Definition: Position.h:85
double distanceTo2D(const Position &p2) const
returns the euclidean distance in the x-y-plane
Definition: Position.h:252
double x() const
Returns the x-position.
Definition: Position.h:55
void add(const Position &pos)
Adds the given position to this one.
Definition: Position.h:125
void mul(double val)
Multiplies both positions with the given value.
Definition: Position.h:105
double y() const
Returns the y-position.
Definition: Position.h:60
A list of positions.
double length2D() const
Returns the length.
void append(const PositionVector &v, double sameThreshold=2.0)
void move2sideCustom(std::vector< double > amount, double maxExtension=100)
move position vector to side using a custom offset for each geometry point
void rotate2D(double angle)
double rotationAtOffset(double pos) const
Returns the rotation at the given length.
Position positionAtOffset(double pos, double lateralOffset=0) const
Returns the position at the given length.
void add(double xoff, double yoff, double zoff)
int indexOfClosest(const Position &p, bool twoD=false) const
void move2side(double amount, double maxExtension=100)
move position vector to side using certain ammount
PositionVector getSubpart2D(double beginOffset, double endOffset) const
get subpart of a position vector in two dimensions (Z is ignored)
Boundary getBoxBoundary() const
Returns a boundary enclosing this list of lines.
int insertAtClosest(const Position &p, bool interpolateZ)
inserts p between the two closest positions
void push_back_noDoublePos(const Position &p)
insert in back a non double position
void removeDoublePoints(double minDist=POSITION_EPS, bool assertLength=false, int beginOffset=0, int endOffset=0, bool resample=false)
Removes positions if too near.
PositionVector reverse() const
reverse position vector
Position positionAtOffset2D(double pos, double lateralOffset=0) const
Returns the position at the given length.
static const RGBColor YELLOW
Definition: RGBColor.h:188
void writeXML(OutputDevice &out, bool geo=false) const
Definition: SUMOPolygon.cpp:88
Encapsulated SAX-Attributes.
T getOpt(int attr, const char *objectid, bool &ok, T defaultValue=T(), bool report=true) const
Tries to read given attribute assuming it is an int.
T get(int attr, const char *objectid, bool &ok, bool report=true) const
Tries to read given attribute assuming it is an int.
virtual bool hasAttribute(int id) const =0
Returns the information whether the named (by its enum-value) attribute is within the current list.
static StringBijection< TrafficLightType > TrafficLightTypes
traffic light types
T get(const std::string &str) const
static int toInt(const std::string &sData)
converts a string into the integer value described by it by calling the char-type converter,...
static bool runParser(GenericSAXHandler &handler, const std::string &file, const bool isNet=false, const bool isRoute=false)
Runs the given handler on the given file; returns if everything's ok.
Definition: XMLSubSys.cpp:137
NLOHMANN_BASIC_JSON_TPL_DECLARATION void swap(nlohmann::NLOHMANN_BASIC_JSON_TPL &j1, nlohmann::NLOHMANN_BASIC_JSON_TPL &j2) noexcept(//NOLINT(readability-inconsistent-declaration-parameter-name) is_nothrow_move_constructible< nlohmann::NLOHMANN_BASIC_JSON_TPL >::value &&//NOLINT(misc-redundant-expression) is_nothrow_move_assignable< nlohmann::NLOHMANN_BASIC_JSON_TPL >::value)
exchanges the values of two JSON objects
Definition: json.hpp:21884
static double cn[6]
Definition: odrSpiral.cpp:68
void odrSpiral(double s, double cDot, double *x, double *y, double *t)
Definition: odrSpiral.cpp:236
A structure which describes a connection between edges or lanes.
Definition: NBEdge.h:201
An (internal) definition of a single lane of an edge.
Definition: NBEdge.h:143
double width
This lane's width.
Definition: NBEdge.h:176
PositionVector customShape
A custom shape for this lane set by the user.
Definition: NBEdge.h:189
std::string type
the type of this lane
Definition: NBEdge.h:192
double speed
The speed allowed on this lane.
Definition: NBEdge.h:151
SVCPermissions permissions
List of vehicle types that are allowed on this lane.
Definition: NBEdge.h:157
A connection between two roads.
Representation of an openDrive "link".
double length
The length of the edge.
std::string id
The id of the edge.
std::string junction
The id of the junction the edge belongs to.
std::string streetName
The road name of the edge.
int getPriority(OpenDriveXMLTag dir) const
Returns the edge's priority, regarding the direction.
std::vector< OpenDriveLink > links
std::vector< OpenDriveSignal > signals
std::vector< OpenDriveLaneSection > laneSections
std::vector< OpenDriveLaneOffset > offsets
std::vector< OpenDriveObject > objects
std::vector< OpenDriveGeometry > geometries
std::vector< OpenDriveElevation > elevations
Representation of an OpenDrive geometry part.
std::vector< std::pair< double, double > > speeds
List of positions/speeds of speed changes.
std::vector< OpenDriveWidth > widthData
double speed
The lane's speed (set in post-processing)
double length
The length of this lane section.
std::map< OpenDriveXMLTag, std::vector< OpenDriveLane > > lanesByDir
The lanes, sorted by their direction.
std::map< int, int > laneMap
A mapping from OpenDrive to SUMO-index (the first is signed, the second unsigned)
OpenDriveLaneSection buildLaneSection(double startPos)
int rightLaneNumber
The number of lanes on the right and on the left side, respectively.
double sOrig
The original starting offset of this lane section (differs from s if the section had to be split)
void buildLaneMapping(const NBTypeCont &tc)
Build the mapping from OpenDrive to SUMO lanes.
std::map< int, int > getInnerConnections(OpenDriveXMLTag dir, const OpenDriveLaneSection &prev)
Returns the links from the previous to this lane section.
bool buildSpeedChanges(const NBTypeCont &tc, std::vector< OpenDriveLaneSection > &newSections)
double s
The starting offset of this lane section.
double computeAt(double pos) const