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