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