SUMO - Simulation of Urban MObility
NIImporter_OpenDrive.cpp
Go to the documentation of this file.
1 /****************************************************************************/
10 // Importer for networks stored in openDrive format
11 /****************************************************************************/
12 // SUMO, Simulation of Urban MObility; see http://sumo.dlr.de/
13 // Copyright (C) 2001-2017 DLR (http://www.dlr.de/) and contributors
14 /****************************************************************************/
15 //
16 // This file is part of SUMO.
17 // SUMO is free software: you can redistribute it and/or modify
18 // it under the terms of the GNU General Public License as published by
19 // the Free Software Foundation, either version 3 of the License, or
20 // (at your option) any later version.
21 //
22 /****************************************************************************/
23 
24 
25 // ===========================================================================
26 // included modules
27 // ===========================================================================
28 #ifdef _MSC_VER
29 #include <windows_config.h>
30 #else
31 #include <config.h>
32 #endif
33 #include <string>
34 #include <cmath>
35 #include <iterator>
39 #include <utils/common/ToString.h>
42 #include <netbuild/NBEdge.h>
43 #include <netbuild/NBEdgeCont.h>
44 #include <netbuild/NBNode.h>
45 #include <netbuild/NBNodeCont.h>
46 #include <netbuild/NBNetBuilder.h>
47 #include <netbuild/NBOwnTLDef.h>
55 #include <utils/xml/XMLSubSys.h>
56 #include <utils/geom/Boundary.h>
57 #include "NILoader.h"
58 #include "NIImporter_OpenDrive.h"
59 
60 
61 // ===========================================================================
62 // definitions
63 // ===========================================================================
64 
65 // ===========================================================================
66 // static variables
67 // ===========================================================================
92 
94 };
95 
96 
137 
139 };
140 
141 
144 
145 // ===========================================================================
146 // method definitions
147 // ===========================================================================
148 // ---------------------------------------------------------------------------
149 // static methods (interface in this case)
150 // ---------------------------------------------------------------------------
151 void
153  // check whether the option is set (properly)
154  if (!oc.isUsableFileList("opendrive-files")) {
155  return;
156  }
157  // prepare types
158  myImportAllTypes = oc.getBool("opendrive.import-all-lanes");
159  myImportWidths = !oc.getBool("opendrive.ignore-widths");
160  NBTypeCont& tc = nb.getTypeCont();
161  // build the handler
162  std::map<std::string, OpenDriveEdge*> edges;
163  NIImporter_OpenDrive handler(nb.getTypeCont(), edges);
164  // parse file(s)
165  std::vector<std::string> files = oc.getStringVector("opendrive-files");
166  for (std::vector<std::string>::const_iterator file = files.begin(); file != files.end(); ++file) {
167  if (!FileHelpers::isReadable(*file)) {
168  WRITE_ERROR("Could not open opendrive file '" + *file + "'.");
169  return;
170  }
171  handler.setFileName(*file);
172  PROGRESS_BEGIN_MESSAGE("Parsing opendrive from '" + *file + "'");
173  XMLSubSys::runParser(handler, *file);
175  }
176  // split inner/outer edges
177  std::map<std::string, OpenDriveEdge*> innerEdges, outerEdges;
178  for (std::map<std::string, OpenDriveEdge*>::iterator i = edges.begin(); i != edges.end(); ++i) {
179  if ((*i).second->isInner) {
180  innerEdges[(*i).first] = (*i).second;
181  } else {
182  outerEdges[(*i).first] = (*i).second;
183  }
184  }
185 
186  // convert geometries into a discretised representation
187  computeShapes(edges);
188  // check whether lane sections are valid and whether further must be introduced
189  revisitLaneSections(tc, edges);
190 
191  // -------------------------
192  // node building
193  // -------------------------
194  // build nodes#1
195  // look at all links which belong to a node, collect their bounding boxes
196  // and place the node in the middle of this bounding box
197  std::map<std::string, Boundary> posMap;
198  std::map<std::string, std::string> edge2junction;
199  // compute node positions
200  for (std::map<std::string, OpenDriveEdge*>::iterator i = innerEdges.begin(); i != innerEdges.end(); ++i) {
201  OpenDriveEdge* e = (*i).second;
202  assert(e->junction != "-1" && e->junction != "");
203  edge2junction[e->id] = e->junction;
204  if (posMap.find(e->junction) == posMap.end()) {
205  posMap[e->junction] = Boundary();
206  }
207  posMap[e->junction].add(e->geom.getBoxBoundary());
208  }
209  // build nodes
210  for (std::map<std::string, Boundary>::iterator i = posMap.begin(); i != posMap.end(); ++i) {
211  //std::cout << " import node=" << (*i).first << " z=" << (*i).second.getCenter() << " boundary=" << (*i).second << "\n";
212  if (!nb.getNodeCont().insert((*i).first, (*i).second.getCenter())) {
213  throw ProcessError("Could not add node '" + (*i).first + "'.");
214  }
215  }
216  // assign built nodes
217  for (std::map<std::string, OpenDriveEdge*>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
218  OpenDriveEdge* e = (*i).second;
219  for (std::vector<OpenDriveLink>::iterator j = e->links.begin(); j != e->links.end(); ++j) {
220  OpenDriveLink& l = *j;
221  const std::string& nid = l.elementID;
222  if (l.elementType != OPENDRIVE_ET_ROAD) {
223  if (nb.getNodeCont().retrieve(nid) == 0) {
224  // not yet seen, build (possibly a junction without connections)
225  Position pos = l.linkType == OPENDRIVE_LT_SUCCESSOR ? e->geom[-1] : e->geom[0];
226  if (!nb.getNodeCont().insert(nid, pos)) {
227  throw ProcessError("Could not build node '" + nid + "'.");
228  }
229  }
230  // set node information
232  continue;
233  }
234  if (edge2junction.find(l.elementID) != edge2junction.end()) {
235  // set node information of an internal road
236  setNodeSecure(nb.getNodeCont(), *e, edge2junction[l.elementID], l.linkType);
237  continue;
238  }
239  }
240  }
241  // we should now have all nodes set for links which are not outer edge-to-outer edge links
242 
243 
244  // build nodes#2
245  // build nodes for all outer edge-to-outer edge connections
246  for (std::map<std::string, OpenDriveEdge*>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
247  OpenDriveEdge* e = (*i).second;
248  for (std::vector<OpenDriveLink>::iterator j = e->links.begin(); j != e->links.end(); ++j) {
249  OpenDriveLink& l = *j;
250  if (l.elementType != OPENDRIVE_ET_ROAD || edge2junction.find(l.elementID) != edge2junction.end()) {
251  // is a connection to an internal edge, or a node, skip
252  continue;
253  }
254  // we have a direct connection between to external edges
255  std::string id1 = e->id;
256  std::string id2 = l.elementID;
257  if (id1 < id2) {
258  std::swap(id1, id2);
259  }
260  std::string nid = id1 + "." + id2;
261  if (nb.getNodeCont().retrieve(nid) == 0) {
262  // not yet seen, build
263  Position pos = l.linkType == OPENDRIVE_LT_SUCCESSOR ? e->geom[-1] : e->geom[0];
264  if (!nb.getNodeCont().insert(nid, pos)) {
265  throw ProcessError("Could not build node '" + nid + "'.");
266  }
267  }
268  /* debug-stuff
269  else {
270  Position pos = l.linkType==OPENDRIVE_LT_SUCCESSOR ? e.geom[e.geom.size()-1] : e.geom[0];
271  cout << nid << " " << pos << " " << nb.getNodeCont().retrieve(nid)->getPosition() << endl;
272  }
273  */
274  setNodeSecure(nb.getNodeCont(), *e, nid, l.linkType);
275  }
276  }
277  // we should now have start/end nodes for all outer edge-to-outer edge connections
278 
279 
280  // build nodes#3
281  // assign further nodes generated from inner-edges
282  // these nodes have not been assigned earlier, because the connections are referenced in inner-edges
283  for (std::map<std::string, OpenDriveEdge*>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
284  OpenDriveEdge* e = (*i).second;
285  if (e->to != 0 && e->from != 0) {
286  continue;
287  }
288  for (std::map<std::string, OpenDriveEdge*>::iterator j = innerEdges.begin(); j != innerEdges.end(); ++j) {
289  OpenDriveEdge* ie = (*j).second;
290  for (std::vector<OpenDriveLink>::iterator k = ie->links.begin(); k != ie->links.end(); ++k) {
291  OpenDriveLink& il = *k;
292  if (il.elementType != OPENDRIVE_ET_ROAD || il.elementID != e->id) {
293  // not conneted to the currently investigated outer edge
294  continue;
295  }
296  std::string nid = edge2junction[ie->id];
297  if (il.contactPoint == OPENDRIVE_CP_START) {
299  } else {
301  }
302  }
303  }
304 
305  }
306 
307  // build start/end nodes which were not defined previously
308  for (std::map<std::string, OpenDriveEdge*>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
309  OpenDriveEdge* e = (*i).second;
310  if (e->from == 0) {
311  const std::string nid = e->id + ".begin";
312  e->from = getOrBuildNode(nid, e->geom.front(), nb.getNodeCont());
313  }
314  if (e->to == 0) {
315  const std::string nid = e->id + ".end";
316  e->to = getOrBuildNode(nid, e->geom.back(), nb.getNodeCont());
317  }
318  }
319 
320 
321  // -------------------------
322  // edge building
323  // -------------------------
324  double defaultSpeed = tc.getSpeed("");
325  // build edges
326  for (std::map<std::string, OpenDriveEdge*>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
327  OpenDriveEdge* e = (*i).second;
328  bool lanesBuilt = false;
329 
330  // go along the lane sections, build a node in between of each pair
331 
334 
336  NBNode* sFrom = e->from;
337  NBNode* sTo = e->to;
338  int priorityR = e->getPriority(OPENDRIVE_TAG_RIGHT);
339  int priorityL = e->getPriority(OPENDRIVE_TAG_LEFT);
340  double sB = 0;
341  double sE = e->length;
342  // 0-length geometries are possible if only the inner points are represented
343  const double length2D = e->geom.length2D();
344  double cF = length2D == 0 ? 1 : e->length / length2D;
345  NBEdge* prevRight = 0;
346  NBEdge* prevLeft = 0;
347 
348  // starting at the same node as ending, and no lane sections?
349  if (sFrom == sTo && e->laneSections.size() == 1) {
350  // --> loop, split!
352  ls.s = e->length / 2.;
353  e->laneSections.push_back(ls);
354  WRITE_WARNING("Edge '" + e->id + "' has to be split as it connects same junctions.")
355  }
356 
357  // build along lane sections
358  for (std::vector<OpenDriveLaneSection>::iterator j = e->laneSections.begin(); j != e->laneSections.end(); ++j) {
359  // add internal node if needed
360  if (j == e->laneSections.end() - 1) {
361  sTo = e->to;
362  sE = e->length / cF;
363  } else {
364  double nextS = (j + 1)->s;
365  sTo = new NBNode(e->id + "." + toString(nextS), e->geom.positionAtOffset(nextS));
366  if (!nb.getNodeCont().insert(sTo)) {
367  throw ProcessError("Could not add node '" + sTo->getID() + "'.");
368  }
369  sE = nextS / cF;
370  }
371  PositionVector geom = e->geom.getSubpart2D(sB, sE);
372  std::string id = e->id;
373  if (sFrom != e->from || sTo != e->to) {
374  id = id + "." + toString((*j).s);
375  } else if (e->laneSections.size() == 1) {
376  id = id + ".0.00";
377  }
378 
379  // build lanes to right
380  NBEdge* currRight = 0;
381  if ((*j).rightLaneNumber > 0) {
382  currRight = new NBEdge("-" + id, sFrom, sTo, (*j).rightType, defaultSpeed, (*j).rightLaneNumber, priorityR,
384  lanesBuilt = true;
385  const std::vector<OpenDriveLane>& lanes = (*j).lanesByDir[OPENDRIVE_TAG_RIGHT];
386  for (std::vector<OpenDriveLane>::const_iterator k = lanes.begin(); k != lanes.end(); ++k) {
387  std::map<int, int>::const_iterator lp = (*j).laneMap.find((*k).id);
388  if (lp != (*j).laneMap.end()) {
389  int sumoLaneIndex = lp->second;
390  NBEdge::Lane& sumoLane = currRight->getLaneStruct(sumoLaneIndex);
391  const OpenDriveLane& odLane = *k;
392 
393  sumoLane.origID = e->id + "_" + toString((*k).id);
394  sumoLane.speed = odLane.speed != 0 ? odLane.speed : tc.getSpeed(odLane.type);
395  sumoLane.permissions = tc.getPermissions(odLane.type);
396  sumoLane.width = myImportWidths && odLane.width != 0 ? odLane.width : tc.getWidth(odLane.type);
397  }
398  }
399  if (!nb.getEdgeCont().insert(currRight, myImportAllTypes)) {
400  throw ProcessError("Could not add edge '" + currRight->getID() + "'.");
401  }
402  if (nb.getEdgeCont().wasIgnored(id)) {
403  prevRight = 0;
404  } else {
405  // connect lane sections
406  if (prevRight != 0) {
407  std::map<int, int> connections = (*j).getInnerConnections(OPENDRIVE_TAG_RIGHT, *(j - 1));
408  for (std::map<int, int>::const_iterator k = connections.begin(); k != connections.end(); ++k) {
409  prevRight->addLane2LaneConnection((*k).first, currRight, (*k).second, NBEdge::L2L_VALIDATED);
410  }
411  }
412  prevRight = currRight;
413  }
414  }
415 
416  // build lanes to left
417  NBEdge* currLeft = 0;
418  if ((*j).leftLaneNumber > 0) {
419  currLeft = new NBEdge(id, sTo, sFrom, (*j).leftType, defaultSpeed, (*j).leftLaneNumber, priorityL,
421  lanesBuilt = true;
422  const std::vector<OpenDriveLane>& lanes = (*j).lanesByDir[OPENDRIVE_TAG_LEFT];
423  for (std::vector<OpenDriveLane>::const_iterator k = lanes.begin(); k != lanes.end(); ++k) {
424  std::map<int, int>::const_iterator lp = (*j).laneMap.find((*k).id);
425  if (lp != (*j).laneMap.end()) {
426  int sumoLaneIndex = lp->second;
427  NBEdge::Lane& sumoLane = currLeft->getLaneStruct(sumoLaneIndex);
428  const OpenDriveLane& odLane = *k;
429 
430  sumoLane.origID = e->id + "_" + toString((*k).id);
431  sumoLane.speed = odLane.speed != 0 ? odLane.speed : tc.getSpeed(odLane.type);
432  sumoLane.permissions = tc.getPermissions(odLane.type);
433  sumoLane.width = myImportWidths && odLane.width != 0 ? odLane.width : tc.getWidth(odLane.type);
434  }
435  }
436  if (!nb.getEdgeCont().insert(currLeft, myImportAllTypes)) {
437  throw ProcessError("Could not add edge '" + currLeft->getID() + "'.");
438  }
439  if (nb.getEdgeCont().wasIgnored(id)) {
440  prevLeft = 0;
441  } else {
442  // connect lane sections
443  if (prevLeft != 0) {
444  std::map<int, int> connections = (*j).getInnerConnections(OPENDRIVE_TAG_LEFT, *(j - 1));
445  for (std::map<int, int>::const_iterator k = connections.begin(); k != connections.end(); ++k) {
446  currLeft->addLane2LaneConnection((*k).first, prevLeft, (*k).second, NBEdge::L2L_VALIDATED);
447  }
448  }
449  prevLeft = currLeft;
450  }
451  }
452  (*j).sumoID = id;
453 
454 
455  sB = sE;
456  sFrom = sTo;
457  }
458  if (!lanesBuilt) {
459  WRITE_WARNING("Edge '" + e->id + "' has no lanes.");
460  }
461  }
462 
463  // -------------------------
464  // connections building
465  // -------------------------
466  // generate explicit lane-to-lane connections
467  for (std::map<std::string, OpenDriveEdge*>::iterator i = edges.begin(); i != edges.end(); ++i) {
468  setEdgeLinks2(*(*i).second, edges);
469  }
470  // compute connections across intersections, if any
471  std::vector<Connection> connections2;
472  for (std::map<std::string, OpenDriveEdge*>::iterator j = edges.begin(); j != edges.end(); ++j) {
473  const std::set<Connection>& conns = (*j).second->connections;
474 
475  for (std::set<Connection>::const_iterator i = conns.begin(); i != conns.end(); ++i) {
476  if (innerEdges.find((*i).fromEdge) != innerEdges.end()) {
477  // connections starting at inner edges are processed by starting from outer edges
478  continue;
479  }
480  if (innerEdges.find((*i).toEdge) != innerEdges.end()) {
481  std::set<Connection> seen;
482  buildConnectionsToOuter(*i, innerEdges, connections2, seen);
483  } else {
484  connections2.push_back(*i);
485  }
486  }
487  }
488  // set connections
489  for (std::vector<Connection>::const_iterator i = connections2.begin(); i != connections2.end(); ++i) {
490  std::string fromEdge = (*i).fromEdge;
491  if (edges.find(fromEdge) == edges.end()) {
492  WRITE_WARNING("While setting connections: from-edge '" + fromEdge + "' is not known.");
493  continue;
494  }
495  OpenDriveEdge* odFrom = edges[fromEdge];
496  int fromLane = (*i).fromLane;
497  bool fromLast = ((*i).fromCP == OPENDRIVE_CP_END) ^ ((*i).fromLane > 0 && !(*i).all);
498  fromEdge = fromLast ? odFrom->laneSections.back().sumoID : odFrom->laneSections[0].sumoID;
499 
500  std::string toEdge = (*i).toEdge;
501  if (edges.find(toEdge) == edges.end()) {
502  WRITE_WARNING("While setting connections: to-edge '" + toEdge + "' is not known.");
503  continue;
504  }
505 
506  OpenDriveEdge* odTo = edges[toEdge];
507  int toLane = (*i).toLane;
508  bool toLast = ((*i).toCP == OPENDRIVE_CP_END) || ((*i).toLane > 0);
509  toEdge = toLast ? odTo->laneSections.back().sumoID : odTo->laneSections[0].sumoID;
510 
511  if (fromLane == UNSET_CONNECTION) {
512  fromLane = toLast ? odTo->laneSections.back().laneMap.begin()->first : odTo->laneSections[0].laneMap.begin()->first;
513  }
514  if (fromLane < 0) {
515  fromEdge = revertID(fromEdge);
516  }
517  if (toLane == UNSET_CONNECTION) {
518  toLane = toLast ? odTo->laneSections.back().laneMap.begin()->first : odTo->laneSections[0].laneMap.begin()->first;
519  }
520  if (toLane < 0) {
521  toEdge = revertID(toEdge);
522  }
523  fromLane = fromLast ? odFrom->laneSections.back().laneMap[fromLane] : odFrom->laneSections[0].laneMap[fromLane];
524  toLane = toLast ? odTo->laneSections.back().laneMap[toLane] : odTo->laneSections[0].laneMap[toLane];
525  NBEdge* from = nb.getEdgeCont().retrieve(fromEdge);
526  NBEdge* to = nb.getEdgeCont().retrieve(toEdge);
527  if (from == 0) {
528  WRITE_WARNING("Could not find fromEdge representation of '" + fromEdge + "' in connection '" + (*i).origID + "'.");
529  }
530  if (to == 0) {
531  WRITE_WARNING("Could not find fromEdge representation of '" + toEdge + "' in connection '" + (*i).origID + "'.");
532  }
533  if (from == 0 || to == 0) {
534  continue;
535  }
536 
537  from->addLane2LaneConnection(fromLane, to, toLane, NBEdge::L2L_USER);
538 
539  if ((*i).origID != "") {
540  // @todo: this is the most silly way to determine the connection
541  std::vector<NBEdge::Connection>& cons = from->getConnections();
542  for (std::vector<NBEdge::Connection>::iterator k = cons.begin(); k != cons.end(); ++k) {
543  if ((*k).fromLane == fromLane && (*k).toEdge == to && (*k).toLane == toLane) {
544  (*k).origID = (*i).origID + "_" + toString((*i).origLane);
545  break;
546  }
547  }
548  }
549  }
550 
551 
552  // -------------------------
553  // traffic lights
554  // -------------------------
555  std::map<std::string, std::string> tlsControlled;
556  for (std::map<std::string, OpenDriveEdge*>::iterator i = edges.begin(); i != edges.end(); ++i) {
557  OpenDriveEdge* e = (*i).second;
558  for (std::vector<OpenDriveSignal>::const_iterator j = e->signals.begin(); j != e->signals.end(); ++j) {
559  if ((*j).type != "1000001") { // traffic_light (Section 6.11)
560  continue;
561  }
562  std::vector<OpenDriveLaneSection>::iterator k = e->laneSections.begin();
563  bool found = false;
564  for (; k != e->laneSections.end() - 1 && !found;) {
565  if ((*j).s > (*k).s && (*j).s <= (*(k + 1)).s) {
566  found = true;
567  } else {
568  ++k;
569  }
570  }
571 
572  // @todo: major problem, currently, still not completely solved:
573  // inner edges may have traffic lights, too. Nice on one hand, as directions can be recognized
574  // but hard to follow backwards
575  std::string id = (*k).sumoID;
576  if (id == "") {
577  if (e->junction != "") {
578  //WRITE_WARNING("Found a traffic light signal on an internal edge; will not build it (original edge id='" + e->id + "').");
579  std::string fromID, toID;
580  for (std::vector<OpenDriveLink>::const_iterator l = e->links.begin(); l != e->links.end(); ++l) {
581  if ((*l).linkType == OPENDRIVE_LT_PREDECESSOR && (*l).elementType == OPENDRIVE_ET_ROAD) {
582  if (fromID != "") {
583  WRITE_WARNING("Ambigous start of connection.");
584  }
585  fromID = (*l).elementID;
586  OpenDriveEdge* e = edges[fromID];
587  fromID = (*l).contactPoint == OPENDRIVE_CP_START ? e->laneSections[0].sumoID : e->laneSections.back().sumoID;
588  }
589  if ((*l).linkType == OPENDRIVE_LT_SUCCESSOR && (*l).elementType == OPENDRIVE_ET_ROAD) {
590  if (toID != "") {
591  WRITE_WARNING("Ambigous end of connection.");
592  }
593  toID = (*l).elementID;
594  OpenDriveEdge* e = edges[toID];
595  toID = (*l).contactPoint == OPENDRIVE_CP_START ? e->laneSections[0].sumoID : e->laneSections.back().sumoID;
596  }
597  }
598  id = fromID + "->" + toID;
599  } else {
600  WRITE_WARNING("Found a traffic light signal on an unknown edge (original edge id='" + e->id + "').");
601  continue;
602  }
603  }
604 
605  if ((*j).orientation > 0) {
606  id = "-" + id;
607  }
608  tlsControlled[id] = (*j).name;
609  }
610  }
611 
612  for (std::map<std::string, std::string>::iterator i = tlsControlled.begin(); i != tlsControlled.end(); ++i) {
613  std::string id = (*i).first;
614  if (id.find("->") != std::string::npos) {
615  id = id.substr(0, id.find("->"));
616  }
617  NBEdge* e = nb.getEdgeCont().retrieve(id);
618  if (e == 0) {
619  WRITE_WARNING("Could not find edge '" + id + "' while building its traffic light.");
620  continue;
621  }
622  NBNode* toNode = e->getToNode();
623  if (!toNode->isTLControlled()) {
625  NBOwnTLDef* tlDef = new NBOwnTLDef(toNode->getID(), toNode, 0, type);
626  if (!nb.getTLLogicCont().insert(tlDef)) {
627  // actually, nothing should fail here
628  delete tlDef;
629  throw ProcessError();
630  }
631  toNode->addTrafficLight(tlDef);
632  //tlDef->setSinglePhase();
633  }
634  NBTrafficLightDefinition* tlDef = *toNode->getControllingTLS().begin();
635  tlDef->addParameter("connection:" + id, (*i).second);
636  }
637 
638  // -------------------------
639  // clean up
640  // -------------------------
641  for (std::map<std::string, OpenDriveEdge*>::iterator i = edges.begin(); i != edges.end(); ++i) {
642  delete(*i).second;
643  }
644 }
645 
646 
647 void
648 NIImporter_OpenDrive::buildConnectionsToOuter(const Connection& c, const std::map<std::string, OpenDriveEdge*>& innerEdges, std::vector<Connection>& into, std::set<Connection>& seen) {
649  //std::cout << "buildConnectionsToOuter "
650  // << " seen=" << seen.size()
651  // << " from=" << c.fromEdge
652  // << " to=" << c.toEdge
653  // << " fromLane=" << c.fromLane
654  // << " toLane=" << c.toLane
655  // << " fromCP=" << c.fromCP
656  // << " toCP=" << c.toCP
657  // << " all=" << c.all
658  // << " origID=" << c.origID
659  // << " origLane=" << c.origLane
660  // << " seenlist=";
661  //for (std::set<Connection>::const_iterator i = seen.begin(); i != seen.end(); ++i) {
662  // std::cout << (*i).fromEdge << "," << (*i).toEdge << " ";
663  //}
664  //std::cout << "\n";
665 
666  OpenDriveEdge* dest = innerEdges.find(c.toEdge)->second;
667  if (dest == 0) {
669  return;
670  }
671  seen.insert(c);
672  const std::set<Connection>& conts = dest->connections;
673  for (std::set<Connection>::const_iterator i = conts.begin(); i != conts.end(); ++i) {
674  if (innerEdges.find((*i).toEdge) != innerEdges.end()) {
675  std::vector<Connection> t;
676  if (seen.count(*i) == 0) {
677  buildConnectionsToOuter(*i, innerEdges, t, seen);
678  for (std::vector<Connection>::const_iterator j = t.begin(); j != t.end(); ++j) {
679  // @todo this section is unverified
680  Connection cn = (*j);
681  cn.fromEdge = c.fromEdge;
682  cn.fromLane = c.fromLane;
683  cn.fromCP = c.fromCP;
684  cn.all = c.all; // @todo "all" is a hack trying to avoid the "from is zero" problem;
685  into.push_back(cn);
686  }
687  } else {
688  WRITE_WARNING("Circular connections in junction including roads '" + c.fromEdge + "' and '" + c.toEdge + "', loop size " + toString(seen.size()));
689  }
690  } else {
691  if ((*i).fromLane == c.toLane) {
692  Connection cn = (*i);
693  cn.fromEdge = c.fromEdge;
694  cn.fromLane = c.fromLane;
695  cn.fromCP = c.fromCP;
696  cn.all = c.all;
697  cn.origID = c.toEdge;
698  cn.origLane = c.toLane;
699  into.push_back(cn);
700  }
701  }
702  }
703 }
704 
705 
706 void
707 NIImporter_OpenDrive::setEdgeLinks2(OpenDriveEdge& e, const std::map<std::string, OpenDriveEdge*>& edges) {
708  for (std::vector<OpenDriveLink>::iterator i = e.links.begin(); i != e.links.end(); ++i) {
709  OpenDriveLink& l = *i;
710  if (l.elementType != OPENDRIVE_ET_ROAD) {
711  // we assume that links to nodes are later given as connections to edges
712  continue;
713  }
714  // get the right direction of the connected edge
715  std::string connectedEdge = l.elementID;
716  std::string edgeID = e.id;
717 
718  OpenDriveLaneSection& laneSection = l.linkType == OPENDRIVE_LT_SUCCESSOR ? e.laneSections.back() : e.laneSections[0];
719  const std::map<int, int>& laneMap = laneSection.laneMap;
720  if (laneSection.lanesByDir.find(OPENDRIVE_TAG_RIGHT) != laneSection.lanesByDir.end()) {
721  const std::vector<OpenDriveLane>& lanes = laneSection.lanesByDir.find(OPENDRIVE_TAG_RIGHT)->second;
722  for (std::vector<OpenDriveLane>::const_iterator j = lanes.begin(); j != lanes.end(); ++j) {
723  if (!myImportAllTypes && laneMap.find((*j).id) == laneMap.end()) {
724  continue;
725  }
726  Connection c; // @todo: give Connection a new name and a constructor
727  c.fromEdge = e.id;
728  c.fromLane = (*j).id;
730  c.toLane = l.linkType == OPENDRIVE_LT_SUCCESSOR ? (*j).successor : (*j).predecessor;
731  c.toEdge = connectedEdge;
732  c.toCP = l.contactPoint;
733  c.all = false;
734  if (l.linkType != OPENDRIVE_LT_SUCCESSOR) {
735  std::swap(c.fromEdge, c.toEdge);
736  std::swap(c.fromLane, c.toLane);
737  std::swap(c.fromCP, c.toCP);
738  }
739  if (edges.find(c.fromEdge) == edges.end()) {
740  WRITE_ERROR("While setting connections: incoming road '" + c.fromEdge + "' is not known.");
741  } else {
742  OpenDriveEdge* src = edges.find(c.fromEdge)->second;
743  src->connections.insert(c);
744  }
745  }
746  }
747  if (laneSection.lanesByDir.find(OPENDRIVE_TAG_LEFT) != laneSection.lanesByDir.end()) {
748  const std::vector<OpenDriveLane>& lanes = laneSection.lanesByDir.find(OPENDRIVE_TAG_LEFT)->second;
749  for (std::vector<OpenDriveLane>::const_iterator j = lanes.begin(); j != lanes.end(); ++j) {
750  if (!myImportAllTypes && laneMap.find((*j).id) == laneMap.end()) {
751  continue;
752  }
753  Connection c;
754  c.toEdge = e.id;
755  c.toLane = (*j).id;
757  c.fromLane = l.linkType == OPENDRIVE_LT_SUCCESSOR ? (*j).successor : (*j).predecessor;
758  c.fromEdge = connectedEdge;
759  c.fromCP = l.contactPoint;
760  c.all = false;
761  if (l.linkType != OPENDRIVE_LT_SUCCESSOR) {
762  std::swap(c.fromEdge, c.toEdge);
763  std::swap(c.fromLane, c.toLane);
764  std::swap(c.fromCP, c.toCP);
765  }
766  if (edges.find(c.fromEdge) == edges.end()) {
767  WRITE_ERROR("While setting connections: incoming road '" + c.fromEdge + "' is not known.");
768  } else {
769  OpenDriveEdge* src = edges.find(c.fromEdge)->second;
770  src->connections.insert(c);
771  }
772  }
773  }
774  }
775 }
776 
777 
778 std::string NIImporter_OpenDrive::revertID(const std::string& id) {
779  if (id[0] == '-') {
780  return id.substr(1);
781  }
782  return "-" + id;
783 }
784 
785 
786 NBNode*
787 NIImporter_OpenDrive::getOrBuildNode(const std::string& id, const Position& pos,
788  NBNodeCont& nc) {
789  if (nc.retrieve(id) == 0) {
790  // not yet built; build now
791  if (!nc.insert(id, pos)) {
792  // !!! clean up
793  throw ProcessError("Could not add node '" + id + "'.");
794  }
795  }
796  return nc.retrieve(id);
797 }
798 
799 
800 void
802  const std::string& nodeID, NIImporter_OpenDrive::LinkType lt) {
803  NBNode* n = nc.retrieve(nodeID);
804  if (n == 0) {
805  throw ProcessError("Could not find node '" + nodeID + "'.");
806  }
807  if (lt == OPENDRIVE_LT_SUCCESSOR) {
808  if (e.to != 0 && e.to != n) {
809  throw ProcessError("Edge '" + e.id + "' has two end nodes.");
810  }
811  e.to = n;
812  } else {
813  if (e.from != 0 && e.from != n) {
814  throw ProcessError("Edge '" + e.id + "' has two start nodes.");
815  }
816  e.from = n;
817  }
818 }
819 
820 
821 void
822 NIImporter_OpenDrive::computeShapes(std::map<std::string, OpenDriveEdge*>& edges) {
824  const double res = oc.getFloat("opendrive.curve-resolution");
825  for (std::map<std::string, OpenDriveEdge*>::iterator i = edges.begin(); i != edges.end(); ++i) {
826  OpenDriveEdge& e = *(*i).second;
828  for (std::vector<OpenDriveGeometry>::iterator j = e.geometries.begin(); j != e.geometries.end(); ++j) {
829  OpenDriveGeometry& g = *j;
830  PositionVector geom;
831  switch (g.type) {
833  break;
834  case OPENDRIVE_GT_LINE:
835  geom = geomFromLine(e, g);
836  break;
837  case OPENDRIVE_GT_SPIRAL:
838  geom = geomFromSpiral(e, g, res);
839  break;
840  case OPENDRIVE_GT_ARC:
841  geom = geomFromArc(e, g, res);
842  break;
843  case OPENDRIVE_GT_POLY3:
844  geom = geomFromPoly(e, g, res);
845  break;
847  geom = geomFromParamPoly(e, g, res);
848  break;
849  default:
850  break;
851  }
852  if (e.geom.size() > 0 && prevType == OPENDRIVE_GT_LINE) {
853  // remove redundant end point of the previous geometry segment
854  // (the start point of the current segment should have the same value)
855  // this avoids geometry errors due to imprecision
856  if (!e.geom.back().almostSame(geom.front())) {
857  const int index = (int)(j - e.geometries.begin());
858  WRITE_WARNING("Mismatched geometry for edge '" + e.id + "' between geometry segments " + toString(index - 1) + " and " + toString(index) + ".");
859  }
860  e.geom.pop_back();
861  }
862  //std::cout << " adding geometry to road=" << e.id << " old=" << e.geom << " new=" << geom << "\n";
863  for (PositionVector::iterator k = geom.begin(); k != geom.end(); ++k) {
865  }
866  prevType = g.type;
867  }
868  if (oc.exists("geometry.min-dist") && !oc.isDefault("geometry.min-dist")) {
869  e.geom.removeDoublePoints(oc.getFloat("geometry.min-dist"), true);
870  }
872  WRITE_ERROR("Unable to project coordinates for edge '" + e.id + "'.");
873  }
874  // add z-data
875  int k = 0;
876  double pos = 0;
877  for (std::vector<OpenDriveElevation>::iterator j = e.elevations.begin(); j != e.elevations.end(); ++j) {
878  const OpenDriveElevation& el = *j;
879  const double sNext = (j + 1) == e.elevations.end() ? std::numeric_limits<double>::max() : (*(j + 1)).s;
880  while (k < (int)e.geom.size() && pos < sNext) {
881  const double ds = pos - el.s;
882  const double z = el.a + el.b * ds + el.c * ds * ds + el.d * ds * ds * ds;
883  //std::cout << " edge=" << e.id << " k=" << k << " sNext=" << sNext << " pos=" << pos << " z=" << z << " ds=" << ds << " el.s=" << el.s << "el.a=" << el.a << " el.b=" << el.b << " el.c=" << el.c << " el.d=" << el.d << "\n";
884  e.geom[k].add(0, 0, z);
885  k++;
886  if (k < (int)e.geom.size()) {
887  // XXX pos understimates the actual position since the
888  // actual geometry between k-1 and k could be curved
889  pos += e.geom[k - 1].distanceTo2D(e.geom[k]);
890  }
891  }
892  }
893  // add laneoffset
894  if (e.offsets.size() > 0) {
895  // make sure there are intermediate points for each offset-section
896  for (std::vector<OpenDriveLaneOffset>::iterator j = e.offsets.begin(); j != e.offsets.end(); ++j) {
897  const OpenDriveLaneOffset& el = *j;
898  // check wether we need to insert a new point at dist
899  Position pS = e.geom.positionAtOffset2D(el.s);
900  int iS = e.geom.indexOfClosest(pS);
901  // prevent close spacing to reduce impact of rounding errors in z-axis
902  if (pS.distanceTo2D(e.geom[iS]) > POSITION_EPS) {
903  e.geom.insertAtClosest(pS);
904  //std::cout << " edge=" << e.id << " inserting pos=" << pS << " s=" << el.s << " iS=" << iS << " dist=" << pS.distanceTo2D(e.geom[iS]) << "\n";
905  }
906  }
907  // XXX add further points for sections with non-constant offset
908  // shift each point orthogonally by the specified offset
909  int k = 0;
910  double pos = 0;
911  PositionVector geom2;
912  for (std::vector<OpenDriveLaneOffset>::iterator j = e.offsets.begin(); j != e.offsets.end(); ++j) {
913  const OpenDriveLaneOffset& el = *j;
914  const double sNext = (j + 1) == e.offsets.end() ? std::numeric_limits<double>::max() : (*(j + 1)).s;
915  while (k < (int)e.geom.size() && pos < sNext) {
916  const double ds = pos - el.s;
917  const double offset = el.a + el.b * ds + el.c * ds * ds + el.d * ds * ds * ds;
918  //std::cout << " edge=" << e.id << " k=" << k << " sNext=" << sNext << " pos=" << pos << " offset=" << offset << " ds=" << ds << " el.s=" << el.s << "el.a=" << el.a << " el.b=" << el.b << " el.c=" << el.c << " el.d=" << el.d << "\n";
919  if (fabs(offset) > POSITION_EPS) {
920  try {
921  PositionVector tmp = e.geom;
922  // XXX shifting the whole geometry is inefficient. could also use positionAtOffset(lateralOffset=...)
923  tmp.move2side(-offset);
924  //std::cout << " edge=" << e.id << " k=" << k << " offset=" << offset << " geom[k]=" << e.geom[k] << " tmp[k]=" << tmp[k] << " gSize=" << e.geom.size() << " tSize=" << tmp.size() << " geom=" << e.geom << " tmp=" << tmp << "\n";
925  geom2.push_back(tmp[k]);
926  } catch (InvalidArgument&) {
927  geom2.push_back(e.geom[k]);
928  }
929  } else {
930  geom2.push_back(e.geom[k]);
931  }
932  k++;
933  if (k < (int)e.geom.size()) {
934  // XXX pos understimates the actual position since the
935  // actual geometry between k-1 and k could be curved
936  pos += e.geom[k - 1].distanceTo2D(e.geom[k]);
937  }
938  }
939  }
940  assert(e.geom.size() == geom2.size());
941  e.geom = geom2;
942  }
943  //std::cout << " loaded geometry " << e.id << "=" << e.geom << "\n";
944  }
945 }
946 
947 
948 void
949 NIImporter_OpenDrive::revisitLaneSections(const NBTypeCont& tc, std::map<std::string, OpenDriveEdge*>& edges) {
950  for (std::map<std::string, OpenDriveEdge*>::iterator i = edges.begin(); i != edges.end(); ++i) {
951  OpenDriveEdge& e = *(*i).second;
952  std::vector<OpenDriveLaneSection>& laneSections = e.laneSections;
953  // split by speed limits
954  std::vector<OpenDriveLaneSection> newSections;
955  for (std::vector<OpenDriveLaneSection>::iterator j = laneSections.begin(); j != laneSections.end(); ++j) {
956  std::vector<OpenDriveLaneSection> splitSections;
957  bool splitBySpeed = (*j).buildSpeedChanges(tc, splitSections);
958  if (!splitBySpeed) {
959  newSections.push_back(*j);
960  } else {
961  std::copy(splitSections.begin(), splitSections.end(), back_inserter(newSections));
962  }
963  }
964 
965  e.laneSections = newSections;
966  laneSections = e.laneSections;
967  double lastS = -1;
968  // check whether the lane sections are in the right order
969  bool sorted = true;
970  for (std::vector<OpenDriveLaneSection>::const_iterator j = laneSections.begin(); j != laneSections.end() && sorted; ++j) {
971  if ((*j).s <= lastS) {
972  sorted = false;
973  }
974  lastS = (*j).s;
975  }
976  if (!sorted) {
977  WRITE_WARNING("The sections of edge '" + e.id + "' are not sorted properly.");
978  sort(e.laneSections.begin(), e.laneSections.end(), sections_by_s_sorter());
979  }
980  // check whether no duplicates of s-value occure
981  lastS = -1;
982  laneSections = e.laneSections;
983  for (std::vector<OpenDriveLaneSection>::iterator j = laneSections.begin(); j != laneSections.end();) {
984  bool simlarToLast = fabs((*j).s - lastS) < POSITION_EPS;
985  lastS = (*j).s;
986  if (simlarToLast) {
987  WRITE_WARNING("Almost duplicate s-value '" + toString(lastS) + "' for lane sections occured at edge '" + e.id + "'; second entry was removed.");
988  j = laneSections.erase(j);
989  } else {
990  ++j;
991  }
992  }
993  }
994 }
995 
996 
999  UNUSED_PARAMETER(e);
1000  PositionVector ret;
1001  ret.push_back(Position(g.x, g.y));
1002  ret.push_back(calculateStraightEndPoint(g.hdg, g.length, Position(g.x, g.y)));
1003  return ret;
1004 }
1005 
1006 
1009  UNUSED_PARAMETER(e);
1010  PositionVector ret;
1011  double curveStart = g.params[0];
1012  double curveEnd = g.params[1];
1013  Point2D<double> end;
1014  try {
1015  EulerSpiral s(Point2D<double>(g.x, g.y), g.hdg, curveStart, (curveEnd - curveStart) / g.length, g.length);
1016  std::vector<Point2D<double> > into;
1017  s.computeSpiral(into, resolution);
1018  for (std::vector<Point2D<double> >::iterator i = into.begin(); i != into.end(); ++i) {
1019  ret.push_back(Position((*i).getX(), (*i).getY()));
1020  }
1021  } catch (const std::runtime_error& error) {
1022  WRITE_WARNING("Could not compute spiral geometry for edge '" + e.id + "' (" + error.what() + ").");
1023  ret.push_back(Position(g.x, g.y));
1024  }
1025  return ret.getSubpart2D(0, g.length);
1026 }
1027 
1028 
1031  UNUSED_PARAMETER(e);
1032  PositionVector ret;
1033  double dist = 0.0;
1034  double centerX = g.x;
1035  double centerY = g.y;
1036  // left: positive value
1037  double curvature = g.params[0];
1038  double radius = 1. / curvature;
1039  // center point
1040  calculateCurveCenter(&centerX, &centerY, radius, g.hdg);
1041  double endX = g.x;
1042  double endY = g.y;
1043  double startX = g.x;
1044  double startY = g.y;
1045  double geo_posS = g.s;
1046  double geo_posE = g.s;
1047  bool end = false;
1048  do {
1049  geo_posE += resolution;
1050  if (geo_posE - g.s > g.length) {
1051  geo_posE = g.s + g.length;
1052  }
1053  if (geo_posE - g.s > g.length) {
1054  geo_posE = g.s + g.length;
1055  }
1056  calcPointOnCurve(&endX, &endY, centerX, centerY, radius, geo_posE - geo_posS);
1057 
1058  dist += (geo_posE - geo_posS);
1059  //
1060  ret.push_back(Position(startX, startY));
1061  //
1062  startX = endX;
1063  startY = endY;
1064  geo_posS = geo_posE;
1065 
1066  if (geo_posE - (g.s + g.length) < 0.001 && geo_posE - (g.s + g.length) > -0.001) {
1067  end = true;
1068  }
1069  } while (!end);
1070  return ret.getSubpart2D(0, g.length);
1071 }
1072 
1073 
1076  UNUSED_PARAMETER(e);
1077  const double s = sin(g.hdg);
1078  const double c = cos(g.hdg);
1079  PositionVector ret;
1080  for (double off = 0; off < g.length + 2.; off += resolution) {
1081  double x = off;
1082  double y = g.params[0] + g.params[1] * off + g.params[2] * pow(off, 2.) + g.params[3] * pow(off, 3.);
1083  double xnew = x * c - y * s;
1084  double ynew = x * s + y * c;
1085  ret.push_back(Position(g.x + xnew, g.y + ynew));
1086  }
1087  return ret.getSubpart2D(0, g.length);
1088 }
1089 
1090 
1093  UNUSED_PARAMETER(e);
1094  const double s = sin(g.hdg);
1095  const double c = cos(g.hdg);
1096  const double pMax = g.params[8];
1097  const double pStep = pMax / ceil(g.length / resolution);
1098  PositionVector ret;
1099  for (double p = 0; p <= pMax + pStep; p += pStep) {
1100  double x = g.params[0] + g.params[1] * p + g.params[2] * pow(p, 2.) + g.params[3] * pow(p, 3.);
1101  double y = g.params[4] + g.params[5] * p + g.params[6] * pow(p, 2.) + g.params[7] * pow(p, 3.);
1102  double xnew = x * c - y * s;
1103  double ynew = x * s + y * c;
1104  ret.push_back(Position(g.x + xnew, g.y + ynew));
1105  }
1106  return ret.getSubpart2D(0, g.length);
1107 }
1108 
1109 
1110 Position
1111 NIImporter_OpenDrive::calculateStraightEndPoint(double hdg, double length, const Position& start) {
1112  double normx = 1.0f;
1113  double normy = 0.0f;
1114  double x2 = normx * cos(hdg) - normy * sin(hdg);
1115  double y2 = normx * sin(hdg) + normy * cos(hdg);
1116  normx = x2 * length;
1117  normy = y2 * length;
1118  return Position(start.x() + normx, start.y() + normy);
1119 }
1120 
1121 
1122 void
1123 NIImporter_OpenDrive::calculateCurveCenter(double* ad_x, double* ad_y, double ad_radius, double ad_hdg) {
1124  double normX = 1.0;
1125  double normY = 0.0;
1126  double tmpX;
1127  double turn;
1128  if (ad_radius > 0) {
1129  turn = -1.0;
1130  } else {
1131  turn = 1.0;
1132  }
1133 
1134  tmpX = normX;
1135  normX = normX * cos(ad_hdg) + normY * sin(ad_hdg);
1136  normY = tmpX * sin(ad_hdg) + normY * cos(ad_hdg);
1137 
1138  tmpX = normX;
1139  normX = turn * normY;
1140  normY = -turn * tmpX;
1141 
1142  normX = fabs(ad_radius) * normX;
1143  normY = fabs(ad_radius) * normY;
1144 
1145  *ad_x += normX;
1146  *ad_y += normY;
1147 }
1148 
1149 
1150 void
1151 NIImporter_OpenDrive::calcPointOnCurve(double* ad_x, double* ad_y, double ad_centerX, double ad_centerY,
1152  double ad_r, double ad_length) {
1153  double rotAngle = ad_length / fabs(ad_r);
1154  double vx = *ad_x - ad_centerX;
1155  double vy = *ad_y - ad_centerY;
1156  double tmpx;
1157 
1158  double turn;
1159  if (ad_r > 0) {
1160  turn = -1; //left
1161  } else {
1162  turn = 1; //right
1163  }
1164  tmpx = vx;
1165  vx = vx * cos(rotAngle) + turn * vy * sin(rotAngle);
1166  vy = -1 * turn * tmpx * sin(rotAngle) + vy * cos(rotAngle);
1167  *ad_x = vx + ad_centerX;
1168  *ad_y = vy + ad_centerY;
1169 }
1170 
1171 
1172 // ---------------------------------------------------------------------------
1173 // section
1174 // ---------------------------------------------------------------------------
1176  lanesByDir[OPENDRIVE_TAG_LEFT] = std::vector<OpenDriveLane>();
1177  lanesByDir[OPENDRIVE_TAG_RIGHT] = std::vector<OpenDriveLane>();
1178  lanesByDir[OPENDRIVE_TAG_CENTER] = std::vector<OpenDriveLane>();
1179 }
1180 
1181 
1182 void
1184  int sumoLane = 0;
1185  bool singleType = true;
1186  std::vector<std::string> types;
1187  const std::vector<OpenDriveLane>& dirLanesR = lanesByDir.find(OPENDRIVE_TAG_RIGHT)->second;
1188  for (std::vector<OpenDriveLane>::const_reverse_iterator i = dirLanesR.rbegin(); i != dirLanesR.rend(); ++i) {
1189  if (myImportAllTypes || (tc.knows((*i).type) && !tc.getShallBeDiscarded((*i).type))) {
1190  laneMap[(*i).id] = sumoLane++;
1191  types.push_back((*i).type);
1192  if (types.front() != types.back()) {
1193  singleType = false;
1194  }
1195  }
1196  }
1197  rightLaneNumber = sumoLane;
1198  rightType = sumoLane > 0 ? (singleType ? types.front() : joinToString(types, "|")) : "";
1199  sumoLane = 0;
1200  singleType = true;
1201  types.clear();
1202  const std::vector<OpenDriveLane>& dirLanesL = lanesByDir.find(OPENDRIVE_TAG_LEFT)->second;
1203  for (std::vector<OpenDriveLane>::const_iterator i = dirLanesL.begin(); i != dirLanesL.end(); ++i) {
1204  if (myImportAllTypes || (tc.knows((*i).type) && !tc.getShallBeDiscarded((*i).type))) {
1205  laneMap[(*i).id] = sumoLane++;
1206  types.push_back((*i).type);
1207  if (types.front() != types.back()) {
1208  singleType = false;
1209  }
1210  }
1211  }
1212  leftLaneNumber = sumoLane;
1213  leftType = sumoLane > 0 ? (singleType ? types.front() : joinToString(types, "|")) : "";
1214 }
1215 
1216 
1217 std::map<int, int>
1219  std::map<int, int> ret;
1220  const std::vector<OpenDriveLane>& dirLanes = lanesByDir.find(dir)->second;
1221  for (std::vector<OpenDriveLane>::const_reverse_iterator i = dirLanes.rbegin(); i != dirLanes.rend(); ++i) {
1222  std::map<int, int>::const_iterator toP = laneMap.find((*i).id);
1223  if (toP == laneMap.end()) {
1224  // the current lane is not available in SUMO
1225  continue;
1226  }
1227  int to = (*toP).second;
1228  int from = UNSET_CONNECTION;
1229  if ((*i).predecessor != UNSET_CONNECTION) {
1230  from = (*i).predecessor;
1231  }
1232  if (from != UNSET_CONNECTION) {
1233  std::map<int, int>::const_iterator fromP = prev.laneMap.find(from);
1234  if (fromP != prev.laneMap.end()) {
1235  from = (*fromP).second;
1236  } else {
1237  from = UNSET_CONNECTION;
1238  }
1239  }
1240  if (from != UNSET_CONNECTION && to != UNSET_CONNECTION) {
1241  if (ret.find(from) != ret.end()) {
1242 // WRITE_WARNING("double connection");
1243  }
1244  if (dir == OPENDRIVE_TAG_LEFT) {
1245  std::swap(from, to);
1246  }
1247  ret[from] = to;
1248  } else {
1249 // WRITE_WARNING("missing connection");
1250  }
1251  }
1252  return ret;
1253 }
1254 
1255 
1258  OpenDriveLaneSection ret(*this);
1259  ret.s += startPos;
1260  for (int k = 0; k < (int)ret.lanesByDir[OPENDRIVE_TAG_RIGHT].size(); ++k) {
1262  l.speed = 0;
1263  std::vector<std::pair<double, double> >::const_iterator i = std::find_if(l.speeds.begin(), l.speeds.end(), same_position_finder(startPos));
1264  if (i != l.speeds.end()) {
1265  l.speed = (*i).second;
1266  }
1267  }
1268  for (int k = 0; k < (int)ret.lanesByDir[OPENDRIVE_TAG_LEFT].size(); ++k) {
1270  std::vector<std::pair<double, double> >::const_iterator i = std::find_if(l.speeds.begin(), l.speeds.end(), same_position_finder(startPos));
1271  l.speed = 0;
1272  if (i != l.speeds.end()) {
1273  l.speed = (*i).second;
1274  }
1275  }
1276  return ret;
1277 }
1278 
1279 
1280 bool
1281 NIImporter_OpenDrive::OpenDriveLaneSection::buildSpeedChanges(const NBTypeCont& tc, std::vector<OpenDriveLaneSection>& newSections) {
1282  std::set<double> speedChangePositions;
1283  // collect speed change positions and apply initial speed to the begin
1284  for (std::vector<OpenDriveLane>::iterator k = lanesByDir[OPENDRIVE_TAG_RIGHT].begin(); k != lanesByDir[OPENDRIVE_TAG_RIGHT].end(); ++k) {
1285  for (std::vector<std::pair<double, double> >::const_iterator l = (*k).speeds.begin(); l != (*k).speeds.end(); ++l) {
1286  speedChangePositions.insert((*l).first);
1287  if ((*l).first == 0) {
1288  (*k).speed = (*l).second;
1289  }
1290  }
1291  }
1292  for (std::vector<OpenDriveLane>::iterator k = lanesByDir[OPENDRIVE_TAG_LEFT].begin(); k != lanesByDir[OPENDRIVE_TAG_LEFT].end(); ++k) {
1293  for (std::vector<std::pair<double, double> >::const_iterator l = (*k).speeds.begin(); l != (*k).speeds.end(); ++l) {
1294  speedChangePositions.insert((*l).first);
1295  if ((*l).first == 0) {
1296  (*k).speed = (*l).second;
1297  }
1298  }
1299  }
1300  // do nothing if there is none
1301  if (speedChangePositions.size() == 0) {
1302  return false;
1303  }
1304  if (*speedChangePositions.begin() > 0) {
1305  speedChangePositions.insert(0);
1306  }
1307  //
1308  for (std::set<double>::iterator i = speedChangePositions.begin(); i != speedChangePositions.end(); ++i) {
1309  if (i == speedChangePositions.begin()) {
1310  newSections.push_back(*this);
1311  } else {
1312  newSections.push_back(buildLaneSection(*i));
1313  }
1314  }
1315  // propagate speeds
1316  for (int i = 0; i != (int)newSections.size(); ++i) {
1317  OpenDriveLaneSection& ls = newSections[i];
1318  std::map<OpenDriveXMLTag, std::vector<OpenDriveLane> >& lanesByDir = ls.lanesByDir;
1319  for (std::map<OpenDriveXMLTag, std::vector<OpenDriveLane> >::iterator k = lanesByDir.begin(); k != lanesByDir.end(); ++k) {
1320  std::vector<OpenDriveLane>& lanes = (*k).second;
1321  for (int j = 0; j != (int)lanes.size(); ++j) {
1322  OpenDriveLane& l = lanes[j];
1323  if (l.speed != 0) {
1324  continue;
1325  }
1326  if (i > 0) {
1327  l.speed = newSections[i - 1].lanesByDir[(*k).first][j].speed;
1328  } else {
1329  tc.getSpeed(l.type);
1330  }
1331  }
1332  }
1333  }
1334  return true;
1335 }
1336 
1337 
1338 
1339 // ---------------------------------------------------------------------------
1340 // edge
1341 // ---------------------------------------------------------------------------
1342 int
1344  // for signal interpretations see https://de.wikipedia.org/wiki/Bildtafel_der_Verkehrszeichen_in_der_Bundesrepublik_Deutschland_seit_2013
1345  int prio = 1;
1346  for (std::vector<OpenDriveSignal>::const_iterator i = signals.begin(); i != signals.end(); ++i) {
1347  int tmp = 1;
1348  if ((*i).type == "301" || (*i).type == "306") { // priority road or local priority
1349  tmp = 2;
1350  }
1351  if ((*i).type == "205" /*|| (*i).type == "206"*/) { // yield or stop
1352  tmp = 0;
1353  }
1354  if (tmp != 1 && dir == OPENDRIVE_TAG_RIGHT && (*i).orientation > 0) {
1355  prio = tmp;
1356  }
1357  if (tmp != 1 && dir == OPENDRIVE_TAG_LEFT && (*i).orientation < 0) {
1358  prio = tmp;
1359  }
1360 
1361  }
1362  return prio;
1363 }
1364 
1365 
1366 
1367 // ---------------------------------------------------------------------------
1368 // loader methods
1369 // ---------------------------------------------------------------------------
1370 NIImporter_OpenDrive::NIImporter_OpenDrive(const NBTypeCont& tc, std::map<std::string, OpenDriveEdge*>& edges)
1372  myTypeContainer(tc), myCurrentEdge("", "", "", -1), myEdges(edges) {
1373 }
1374 
1375 
1377 }
1378 
1379 
1380 void
1382  const SUMOSAXAttributes& attrs) {
1383  bool ok = true;
1384  switch (element) {
1385  case OPENDRIVE_TAG_HEADER: {
1386  int majorVersion = attrs.get<int>(OPENDRIVE_ATTR_REVMAJOR, 0, ok);
1387  int minorVersion = attrs.get<int>(OPENDRIVE_ATTR_REVMINOR, 0, ok);
1388  if (majorVersion != 1 || minorVersion != 2) {
1389  WRITE_WARNING("Given openDrive file '" + getFileName() + "' uses version " + toString(majorVersion) + "." + toString(minorVersion) + ";\n Version 1.2 is supported.");
1390  }
1391  }
1392  break;
1393  case OPENDRIVE_TAG_ROAD: {
1394  std::string id = attrs.get<std::string>(OPENDRIVE_ATTR_ID, 0, ok);
1395  std::string streetName = attrs.getOpt<std::string>(OPENDRIVE_ATTR_NAME, 0, ok, "", false);
1396  std::string junction = attrs.get<std::string>(OPENDRIVE_ATTR_JUNCTION, id.c_str(), ok);
1397  double length = attrs.get<double>(OPENDRIVE_ATTR_LENGTH, id.c_str(), ok);
1398  myCurrentEdge = OpenDriveEdge(id, streetName, junction, length);
1399  }
1400  break;
1402  if (myElementStack.size() >= 2 && myElementStack[myElementStack.size() - 2] == OPENDRIVE_TAG_ROAD) {
1403  std::string elementType = attrs.get<std::string>(OPENDRIVE_ATTR_ELEMENTTYPE, myCurrentEdge.id.c_str(), ok);
1404  std::string elementID = attrs.get<std::string>(OPENDRIVE_ATTR_ELEMENTID, myCurrentEdge.id.c_str(), ok);
1405  std::string contactPoint = attrs.hasAttribute(OPENDRIVE_ATTR_CONTACTPOINT)
1406  ? attrs.get<std::string>(OPENDRIVE_ATTR_CONTACTPOINT, myCurrentEdge.id.c_str(), ok)
1407  : "end";
1408  addLink(OPENDRIVE_LT_PREDECESSOR, elementType, elementID, contactPoint);
1409  }
1410  if (myElementStack.size() >= 2 && myElementStack[myElementStack.size() - 2] == OPENDRIVE_TAG_LANE) {
1411  int no = attrs.get<int>(OPENDRIVE_ATTR_ID, myCurrentEdge.id.c_str(), ok);
1412  OpenDriveLane& l = myCurrentEdge.laneSections.back().lanesByDir[myCurrentLaneDirection].back();
1413  l.predecessor = no;
1414  }
1415  }
1416  break;
1417  case OPENDRIVE_TAG_SUCCESSOR: {
1418  if (myElementStack.size() >= 2 && myElementStack[myElementStack.size() - 2] == OPENDRIVE_TAG_ROAD) {
1419  std::string elementType = attrs.get<std::string>(OPENDRIVE_ATTR_ELEMENTTYPE, myCurrentEdge.id.c_str(), ok);
1420  std::string elementID = attrs.get<std::string>(OPENDRIVE_ATTR_ELEMENTID, myCurrentEdge.id.c_str(), ok);
1421  std::string contactPoint = attrs.hasAttribute(OPENDRIVE_ATTR_CONTACTPOINT)
1422  ? attrs.get<std::string>(OPENDRIVE_ATTR_CONTACTPOINT, myCurrentEdge.id.c_str(), ok)
1423  : "start";
1424  addLink(OPENDRIVE_LT_SUCCESSOR, elementType, elementID, contactPoint);
1425  }
1426  if (myElementStack.size() >= 2 && myElementStack[myElementStack.size() - 2] == OPENDRIVE_TAG_LANE) {
1427  int no = attrs.get<int>(OPENDRIVE_ATTR_ID, myCurrentEdge.id.c_str(), ok);
1428  OpenDriveLane& l = myCurrentEdge.laneSections.back().lanesByDir[myCurrentLaneDirection].back();
1429  l.successor = no;
1430  }
1431  }
1432  break;
1433  case OPENDRIVE_TAG_GEOMETRY: {
1434  double length = attrs.get<double>(OPENDRIVE_ATTR_LENGTH, myCurrentEdge.id.c_str(), ok);
1435  double s = attrs.get<double>(OPENDRIVE_ATTR_S, myCurrentEdge.id.c_str(), ok);
1436  double x = attrs.get<double>(OPENDRIVE_ATTR_X, myCurrentEdge.id.c_str(), ok);
1437  double y = attrs.get<double>(OPENDRIVE_ATTR_Y, myCurrentEdge.id.c_str(), ok);
1438  double hdg = attrs.get<double>(OPENDRIVE_ATTR_HDG, myCurrentEdge.id.c_str(), ok);
1439  myCurrentEdge.geometries.push_back(OpenDriveGeometry(length, s, x, y, hdg));
1440  }
1441  break;
1442  case OPENDRIVE_TAG_ELEVATION: {
1443  double s = attrs.get<double>(OPENDRIVE_ATTR_S, myCurrentEdge.id.c_str(), ok);
1444  double a = attrs.get<double>(OPENDRIVE_ATTR_A, myCurrentEdge.id.c_str(), ok);
1445  double b = attrs.get<double>(OPENDRIVE_ATTR_B, myCurrentEdge.id.c_str(), ok);
1446  double c = attrs.get<double>(OPENDRIVE_ATTR_C, myCurrentEdge.id.c_str(), ok);
1447  double d = attrs.get<double>(OPENDRIVE_ATTR_D, myCurrentEdge.id.c_str(), ok);
1448  myCurrentEdge.elevations.push_back(OpenDriveElevation(s, a, b, c, d));
1449  }
1450  break;
1451  case OPENDRIVE_TAG_LINE: {
1452  std::vector<double> vals;
1454  }
1455  break;
1456  case OPENDRIVE_TAG_SPIRAL: {
1457  std::vector<double> vals;
1458  vals.push_back(attrs.get<double>(OPENDRIVE_ATTR_CURVSTART, myCurrentEdge.id.c_str(), ok));
1459  vals.push_back(attrs.get<double>(OPENDRIVE_ATTR_CURVEND, myCurrentEdge.id.c_str(), ok));
1461  }
1462  break;
1463  case OPENDRIVE_TAG_ARC: {
1464  std::vector<double> vals;
1465  vals.push_back(attrs.get<double>(OPENDRIVE_ATTR_CURVATURE, myCurrentEdge.id.c_str(), ok));
1467  }
1468  break;
1469  case OPENDRIVE_TAG_POLY3: {
1470  std::vector<double> vals;
1471  vals.push_back(attrs.get<double>(OPENDRIVE_ATTR_A, myCurrentEdge.id.c_str(), ok));
1472  vals.push_back(attrs.get<double>(OPENDRIVE_ATTR_B, myCurrentEdge.id.c_str(), ok));
1473  vals.push_back(attrs.get<double>(OPENDRIVE_ATTR_C, myCurrentEdge.id.c_str(), ok));
1474  vals.push_back(attrs.get<double>(OPENDRIVE_ATTR_D, myCurrentEdge.id.c_str(), ok));
1476  }
1477  break;
1478  case OPENDRIVE_TAG_PARAMPOLY3: {
1479  std::vector<double> vals;
1480  vals.push_back(attrs.get<double>(OPENDRIVE_ATTR_AU, myCurrentEdge.id.c_str(), ok));
1481  vals.push_back(attrs.get<double>(OPENDRIVE_ATTR_BU, myCurrentEdge.id.c_str(), ok));
1482  vals.push_back(attrs.get<double>(OPENDRIVE_ATTR_CU, myCurrentEdge.id.c_str(), ok));
1483  vals.push_back(attrs.get<double>(OPENDRIVE_ATTR_DU, myCurrentEdge.id.c_str(), ok));
1484  vals.push_back(attrs.get<double>(OPENDRIVE_ATTR_AV, myCurrentEdge.id.c_str(), ok));
1485  vals.push_back(attrs.get<double>(OPENDRIVE_ATTR_BV, myCurrentEdge.id.c_str(), ok));
1486  vals.push_back(attrs.get<double>(OPENDRIVE_ATTR_CV, myCurrentEdge.id.c_str(), ok));
1487  vals.push_back(attrs.get<double>(OPENDRIVE_ATTR_DV, myCurrentEdge.id.c_str(), ok));
1488  vals.push_back(attrs.getOpt<double>(OPENDRIVE_ATTR_PRANGE, myCurrentEdge.id.c_str(), ok, 1.0, false));
1490  }
1491  break;
1493  double s = attrs.get<double>(OPENDRIVE_ATTR_S, myCurrentEdge.id.c_str(), ok);
1495  }
1496  break;
1497  case OPENDRIVE_TAG_LANEOFFSET: {
1498  double s = attrs.get<double>(OPENDRIVE_ATTR_S, myCurrentEdge.id.c_str(), ok);
1499  double a = attrs.get<double>(OPENDRIVE_ATTR_A, myCurrentEdge.id.c_str(), ok);
1500  double b = attrs.get<double>(OPENDRIVE_ATTR_B, myCurrentEdge.id.c_str(), ok);
1501  double c = attrs.get<double>(OPENDRIVE_ATTR_C, myCurrentEdge.id.c_str(), ok);
1502  double d = attrs.get<double>(OPENDRIVE_ATTR_D, myCurrentEdge.id.c_str(), ok);
1503  myCurrentEdge.offsets.push_back(OpenDriveLaneOffset(s, a, b, c, d));
1504  }
1505  break;
1506  case OPENDRIVE_TAG_LEFT:
1508  break;
1509  case OPENDRIVE_TAG_CENTER:
1511  break;
1512  case OPENDRIVE_TAG_RIGHT:
1514  break;
1515  case OPENDRIVE_TAG_LANE: {
1516  std::string type = attrs.get<std::string>(OPENDRIVE_ATTR_TYPE, myCurrentEdge.id.c_str(), ok);
1517  int id = attrs.get<int>(OPENDRIVE_ATTR_ID, myCurrentEdge.id.c_str(), ok);
1518  std::string level = attrs.hasAttribute(OPENDRIVE_ATTR_LEVEL)
1519  ? attrs.get<std::string>(OPENDRIVE_ATTR_LEVEL, myCurrentEdge.id.c_str(), ok)
1520  : "";
1522  ls.lanesByDir[myCurrentLaneDirection].push_back(OpenDriveLane(id, level, type));
1523  }
1524  break;
1525  case OPENDRIVE_TAG_SIGNAL: {
1526  int id = attrs.get<int>(OPENDRIVE_ATTR_ID, myCurrentEdge.id.c_str(), ok);
1527  std::string type = attrs.get<std::string>(OPENDRIVE_ATTR_TYPE, myCurrentEdge.id.c_str(), ok);
1528  std::string name = attrs.getOpt<std::string>(OPENDRIVE_ATTR_NAME, myCurrentEdge.id.c_str(), ok, "", false);
1529  int orientation = attrs.get<std::string>(OPENDRIVE_ATTR_ORIENTATION, myCurrentEdge.id.c_str(), ok) == "-" ? -1 : 1;
1530  double s = attrs.get<double>(OPENDRIVE_ATTR_S, myCurrentEdge.id.c_str(), ok);
1531  bool dynamic = attrs.get<std::string>(OPENDRIVE_ATTR_DYNAMIC, myCurrentEdge.id.c_str(), ok) == "no" ? false : true;
1532  myCurrentEdge.signals.push_back(OpenDriveSignal(id, type, name, orientation, dynamic, s));
1533  }
1534  break;
1536  myCurrentJunctionID = attrs.get<std::string>(OPENDRIVE_ATTR_ID, myCurrentJunctionID.c_str(), ok);
1537  break;
1538  case OPENDRIVE_TAG_CONNECTION: {
1539  std::string id = attrs.get<std::string>(OPENDRIVE_ATTR_ID, myCurrentJunctionID.c_str(), ok);
1540  myCurrentIncomingRoad = attrs.get<std::string>(OPENDRIVE_ATTR_INCOMINGROAD, myCurrentJunctionID.c_str(), ok);
1542  std::string cp = attrs.get<std::string>(OPENDRIVE_ATTR_CONTACTPOINT, myCurrentJunctionID.c_str(), ok);
1544  myConnectionWasEmpty = true;
1545  }
1546  break;
1547  case OPENDRIVE_TAG_LANELINK: {
1548  int from = attrs.get<int>(OPENDRIVE_ATTR_FROM, myCurrentJunctionID.c_str(), ok);
1549  int to = attrs.get<int>(OPENDRIVE_ATTR_TO, myCurrentJunctionID.c_str(), ok);
1550  Connection c;
1552  c.toEdge = myCurrentConnectingRoad;
1553  c.fromLane = from;
1554  c.toLane = to;
1555  c.fromCP = OPENDRIVE_CP_END;
1556  c.toCP = myCurrentContactPoint;
1557  c.all = false;
1558  if (myEdges.find(c.fromEdge) == myEdges.end()) {
1559  WRITE_ERROR("In laneLink-element: incoming road '" + c.fromEdge + "' is not known.");
1560  } else {
1561  OpenDriveEdge* e = myEdges.find(c.fromEdge)->second;
1562  e->connections.insert(c);
1563  myConnectionWasEmpty = false;
1564  }
1565  }
1566  break;
1567  case OPENDRIVE_TAG_WIDTH: {
1568  if (myElementStack.size() >= 2 && myElementStack[myElementStack.size() - 1] == OPENDRIVE_TAG_LANE) {
1569  double width = attrs.get<double>(OPENDRIVE_ATTR_A, myCurrentEdge.id.c_str(), ok);
1570  OpenDriveLane& l = myCurrentEdge.laneSections.back().lanesByDir[myCurrentLaneDirection].back();
1571  l.width = MAX2(l.width, width);
1572  }
1573  }
1574  break;
1575  case OPENDRIVE_TAG_SPEED: {
1576  if (myElementStack.size() >= 2 && myElementStack[myElementStack.size() - 1] == OPENDRIVE_TAG_LANE) {
1577  double speed = attrs.get<double>(OPENDRIVE_ATTR_MAX, myCurrentEdge.id.c_str(), ok);
1578  double pos = attrs.get<double>(OPENDRIVE_ATTR_SOFFSET, myCurrentEdge.id.c_str(), ok);
1579  myCurrentEdge.laneSections.back().lanesByDir[myCurrentLaneDirection].back().speeds.push_back(std::make_pair(pos, speed));
1580  }
1581  }
1582  break;
1583  default:
1584  break;
1585  }
1586  myElementStack.push_back(element);
1587 }
1588 
1589 
1590 void
1592  myElementStack.pop_back();
1593  switch (element) {
1594  case OPENDRIVE_TAG_ROAD:
1596  break;
1598  if (myConnectionWasEmpty) {
1599  Connection c;
1602  c.fromLane = 0;
1603  c.toLane = 0;
1606  c.all = true;
1607  if (myEdges.find(c.fromEdge) == myEdges.end()) {
1608  WRITE_ERROR("In laneLink-element: incoming road '" + c.fromEdge + "' is not known.");
1609  } else {
1610  OpenDriveEdge* e = myEdges.find(c.fromEdge)->second;
1611  e->connections.insert(c);
1612  }
1613  }
1614  break;
1616  myCurrentEdge.laneSections.back().buildLaneMapping(myTypeContainer);
1617  }
1618  break;
1619  default:
1620  break;
1621  }
1622 }
1623 
1624 
1625 
1626 void
1627 NIImporter_OpenDrive::addLink(LinkType lt, const std::string& elementType,
1628  const std::string& elementID,
1629  const std::string& contactPoint) {
1630  OpenDriveLink l(lt, elementID);
1631  // elementType
1632  if (elementType == "road") {
1634  } else if (elementType == "junction") {
1636  }
1637  // contact point
1638  if (contactPoint == "start") {
1640  } else if (contactPoint == "end") {
1642  }
1643  // add
1644  myCurrentEdge.links.push_back(l);
1645 }
1646 
1647 
1648 void
1649 NIImporter_OpenDrive::addGeometryShape(GeometryType type, const std::vector<double>& vals) {
1650  // checks
1651  if (myCurrentEdge.geometries.size() == 0) {
1652  throw ProcessError("Mismatching paranthesis in geometry definition for road '" + myCurrentEdge.id + "'");
1653  }
1655  if (last.type != OPENDRIVE_GT_UNKNOWN) {
1656  throw ProcessError("Double geometry information for road '" + myCurrentEdge.id + "'");
1657  }
1658  // set
1659  last.type = type;
1660  last.params = vals;
1661 }
1662 
1663 
1664 bool
1666  if (c1.fromEdge != c2.fromEdge) {
1667  return c1.fromEdge < c2.fromEdge;
1668  }
1669  if (c1.toEdge != c2.toEdge) {
1670  return c1.toEdge < c2.toEdge;
1671  }
1672  if (c1.fromLane != c2.fromLane) {
1673  return c1.fromLane < c2.fromLane;
1674  }
1675  return c1.toLane < c2.toLane;
1676 }
1677 
1678 
1679 
1680 /****************************************************************************/
1681 
std::map< std::string, OpenDriveEdge * > & myEdges
NBNode * retrieve(const std::string &id) const
Returns the node with the given name.
Definition: NBNodeCont.cpp:106
std::vector< int > myElementStack
std::string rightType
the composite type built from all used lane types
double length2D() const
Returns the length.
double getSpeed(const std::string &type) const
Returns the maximal velocity for the given type [m/s].
Definition: NBTypeCont.cpp:181
static PositionVector geomFromLine(const OpenDriveEdge &e, const OpenDriveGeometry &g)
static StringBijection< int >::Entry openDriveAttrs[]
The names of openDrive-XML attributes (for passing to GenericSAXHandler)
NBTypeCont & getTypeCont()
Returns a reference to the type container.
Definition: NBNetBuilder.h:165
OpenDriveLaneSection buildLaneSection(double startPos)
void addLink(LinkType lt, const std::string &elementType, const std::string &elementID, const std::string &contactPoint)
static PositionVector geomFromArc(const OpenDriveEdge &e, const OpenDriveGeometry &g, double resolution)
static bool isReadable(std::string path)
Checks whether the given file is readable.
Definition: FileHelpers.cpp:54
std::string junction
The id of the junction the edge belongs to.
std::vector< OpenDriveElevation > elevations
static void calculateCurveCenter(double *ad_x, double *ad_y, double ad_radius, double ad_hdg)
int indexOfClosest(const Position &p) const
index of the closest position to p
GeometryType
OpenDrive geometry type enumeration.
int rightLaneNumber
The number of lanes on the right and on the left side, respectively.
static void computeShapes(std::map< std::string, OpenDriveEdge *> &edges)
Computes a polygon representation of each edge&#39;s geometry.
double distanceTo2D(const Position &p2) const
returns the euclidean distance in the x-y-plane
Definition: Position.h:250
Position positionAtOffset2D(double pos, double lateralOffset=0) const
Returns the position at the given length.
Representation of a lane section.
const std::string & getFileName() const
returns the current file name
double y() const
Returns the y-position.
Definition: Position.h:68
The representation of a single edge during network building.
Definition: NBEdge.h:71
NIImporter_OpenDrive(const NBTypeCont &tc, std::map< std::string, OpenDriveEdge *> &edges)
Constructor.
Representation of an openDrive "link".
double x() const
Returns the x-position.
Definition: Position.h:63
The base class for traffic light logic definitions.
static const double UNSPECIFIED_OFFSET
unspecified lane offset
Definition: NBEdge.h:258
ContactPoint myCurrentContactPoint
T MAX2(T a, T b)
Definition: StdDefs.h:70
PositionVector reverse() const
reverse position vector
std::map< OpenDriveXMLTag, std::vector< OpenDriveLane > > lanesByDir
The lanes, sorted by their direction.
static bool transformCoordinates(PositionVector &from, bool includeInBoundary=true, GeoConvHelper *from_srs=0)
static PositionVector geomFromPoly(const OpenDriveEdge &e, const OpenDriveGeometry &g, double resolution)
static void calcPointOnCurve(double *ad_x, double *ad_y, double ad_centerX, double ad_centerY, double ad_r, double ad_length)
double length
The length of the edge.
bool getShallBeDiscarded(const std::string &type) const
Returns the information whether edges of this type shall be discarded.
Definition: NBTypeCont.cpp:199
bool getBool(const std::string &name) const
Returns the boolean-value of the named option (only for Option_Bool)
const std::string & getID() const
Returns the id.
Definition: Named.h:66
bool isDefault(const std::string &name) const
Returns the information whether the named option has still the default value.
Lane & getLaneStruct(int lane)
Definition: NBEdge.h:1154
static bool runParser(GenericSAXHandler &handler, const std::string &file, const bool isNet=false)
Runs the given handler on the given file; returns if everything&#39;s ok.
Definition: XMLSubSys.cpp:110
friend bool operator<(const Connection &c1, const Connection &c2)
virtual bool hasAttribute(int id) const =0
Returns the information whether the named (by its enum-value) attribute is within the current list...
#define UNUSED_PARAMETER(x)
Definition: StdDefs.h:38
static const double UNSPECIFIED_WIDTH
unspecified lane width
Definition: NBEdge.h:255
A class that stores a 2D geometrical boundary.
Definition: Boundary.h:48
static NBNode * getOrBuildNode(const std::string &id, const Position &pos, NBNodeCont &nc)
Builds a node or returns the already built.
void error(const XERCES_CPP_NAMESPACE::SAXParseException &exception)
Handler for XML-errors.
#define WRITE_WARNING(msg)
Definition: MsgHandler.h:200
The connection was computed and validated.
Definition: NBEdge.h:117
static OptionsCont & getOptions()
Retrieves the options.
Definition: OptionsCont.cpp:65
double getWidth(const std::string &type) const
Returns the lane width for the given type [m].
Definition: NBTypeCont.cpp:217
static std::string revertID(const std::string &id)
OpenDriveXMLTag myCurrentLaneDirection
An (internal) definition of a single lane of an edge.
Definition: NBEdge.h:124
SVCPermissions permissions
List of vehicle types that are allowed on this lane.
Definition: NBEdge.h:135
bool isTLControlled() const
Returns whether this node is controlled by any tls.
Definition: NBNode.h:288
std::vector< OpenDriveLink > links
A handler which converts occuring elements and attributes into enums.
int getPriority(OpenDriveXMLTag dir) const
Returns the edge&#39;s priority, regarding the direction.
void removeDoublePoints(double minDist=POSITION_EPS, bool assertLength=false)
Removes positions if too near.
OpenDriveElevation OpenDriveLaneOffset
LaneOffset has the same fields as Elevation.
bool knows(const std::string &type) const
Returns whether the named type is in the container.
Definition: NBTypeCont.cpp:75
static PositionVector geomFromSpiral(const OpenDriveEdge &e, const OpenDriveGeometry &g, double resolution)
#define max(a, b)
Definition: polyfonts.c:65
bool insert(NBEdge *edge, bool ignorePrunning=false)
Adds an edge to the dictionary.
Definition: NBEdgeCont.cpp:158
std::string toString(const T &t, std::streamsize accuracy=gPrecision)
Definition: ToString.h:56
std::string type
The lane&#39;s type.
bool isUsableFileList(const std::string &name) const
Checks whether the named option is usable as a file list (with at least a single file) ...
Encapsulated SAX-Attributes.
double width
The lane&#39;s width;.
static StringBijection< TrafficLightType > TrafficLightTypes
traffic light types
T get(int attr, const char *objectid, bool &ok, bool report=true) const
Tries to read given attribute assuming it is an int.
static Position calculateStraightEndPoint(double hdg, double length, const Position &start)
A point in 2D or 3D with translation and scaling methods.
Definition: Position.h:46
NBEdgeCont & getEdgeCont()
Definition: NBNetBuilder.h:155
void computeSpiral(std::vector< Point2D< double > > &spiral, double ds=0, int NPts=0)
Definition: euler.cpp:262
std::string id
The id of the edge.
A list of positions.
static void loadNetwork(const OptionsCont &oc, NBNetBuilder &nb)
Loads content of the optionally given SUMO file.
bool buildSpeedChanges(const NBTypeCont &tc, std::vector< OpenDriveLaneSection > &newSections)
const NBTypeCont & myTypeContainer
T get(const std::string &str) const
std::vector< OpenDriveLaneOffset > offsets
double speed
The lane&#39;s speed (set in post-processing)
bool exists(const std::string &name) const
Returns the information whether the named option is known.
std::vector< OpenDriveLaneSection > laneSections
std::map< int, int > laneMap
A mapping from OpenDrive to SUMO-index (the first is signed, the second unsigned) ...
std::vector< std::string > getStringVector(const std::string &name) const
Returns the list of string-vector-value of the named option (only for Option_String) ...
#define PROGRESS_BEGIN_MESSAGE(msg)
Definition: MsgHandler.h:202
#define POSITION_EPS
Definition: config.h:175
void myStartElement(int element, const SUMOSAXAttributes &attrs)
Called on the opening of a tag;.
int insertAtClosest(const Position &p)
inserts p between the two closest positions and returns the insertion index
OpenDriveXMLTag
Numbers representing openDrive-XML - element names.
The connection was given by the user.
Definition: NBEdge.h:115
double speed
The speed allowed on this lane.
Definition: NBEdge.h:132
double getFloat(const std::string &name) const
Returns the double-value of the named option (only for Option_Float)
double width
This lane&#39;s width.
Definition: NBEdge.h:144
void move2side(double amount)
move position vector to side using certain ammount
static PositionVector geomFromParamPoly(const OpenDriveEdge &e, const OpenDriveGeometry &g, double resolution)
static StringBijection< int >::Entry openDriveTags[]
The names of openDrive-XML elements (for passing to GenericSAXHandler)
std::string origID
An original ID, if given.
Definition: NBEdge.h:147
void buildLaneMapping(const NBTypeCont &tc)
Build the mapping from OpenDrive to SUMO lanes.
std::vector< OpenDriveSignal > signals
#define WRITE_ERROR(msg)
Definition: MsgHandler.h:206
LinkType
OpenDrive link type enumeration.
void addParameter(const std::string &key, const std::string &value)
Adds a parameter.
PositionVector getSubpart2D(double beginOffset, double endOffset) const
get subpart of a position vector in two dimensions (Z is ignored)
static void revisitLaneSections(const NBTypeCont &tc, std::map< std::string, OpenDriveEdge *> &edges)
Rechecks lane sections of the given edges.
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.
void addTrafficLight(NBTrafficLightDefinition *tlDef)
Adds a traffic light to the list of traffic lights that control this node.
Definition: NBNode.cpp:310
std::map< int, int > getInnerConnections(OpenDriveXMLTag dir, const OpenDriveLaneSection &prev)
Returns the links from the previous to this lane section.
const std::vector< Connection > & getConnections() const
Returns the connections.
Definition: NBEdge.h:834
NBNodeCont & getNodeCont()
Returns a reference to the node container.
Definition: NBNetBuilder.h:160
Instance responsible for building networks.
Definition: NBNetBuilder.h:114
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:298
Representation of an OpenDrive geometry part.
NBEdge * retrieve(const std::string &id, bool retrieveExtracted=false) const
Returns the edge that has the given id.
Definition: NBEdgeCont.cpp:251
A storage for options typed value containers)
Definition: OptionsCont.h:99
bool insert(const std::string &id, const Position &position, NBDistrict *district=0)
Inserts a node into the map.
Definition: NBNodeCont.cpp:77
NBTrafficLightLogicCont & getTLLogicCont()
Returns a reference to the traffic light logics container.
Definition: NBNetBuilder.h:170
bool addLane2LaneConnection(int fromLane, NBEdge *dest, int toLane, Lane2LaneInfoType type, bool mayUseSameDestination=false, bool mayDefinitelyPass=false, bool keepClear=true, double contPos=UNSPECIFIED_CONTPOS, double visibility=UNSPECIFIED_VISIBILITY_DISTANCE)
Adds a connection between the specified this edge&#39;s lane and an approached one.
Definition: NBEdge.cpp:900
static void setEdgeLinks2(OpenDriveEdge &e, const std::map< std::string, OpenDriveEdge *> &edges)
std::vector< OpenDriveGeometry > geometries
Represents a single node (junction) during network building.
Definition: NBNode.h:75
void addGeometryShape(GeometryType type, const std::vector< double > &vals)
A class for sorting lane sections by their s-value.
bool insert(NBTrafficLightDefinition *logic, bool forceInsert=false)
Adds a logic definition to the dictionary.
static void setNodeSecure(NBNodeCont &nc, OpenDriveEdge &e, const std::string &nodeID, NIImporter_OpenDrive::LinkType lt)
Boundary getBoxBoundary() const
Returns a boundary enclosing this list of lines.
void push_back_noDoublePos(const Position &p)
insert in back a non double position
A connection between two roads.
bool wasIgnored(std::string id) const
Returns whether the edge with the id was ignored during parsing.
Definition: NBEdgeCont.h:474
Container for nodes during the netbuilding process.
Definition: NBNodeCont.h:63
std::vector< std::pair< double, double > > speeds
List of positions/speeds of speed changes.
#define PROGRESS_DONE_MESSAGE()
Definition: MsgHandler.h:203
A traffic light logics which must be computed (only nodes/edges are given)
Definition: NBOwnTLDef.h:54
void add(double xoff, double yoff, double zoff)
Position positionAtOffset(double pos, double lateralOffset=0) const
Returns the position at the given length.
Importer for networks stored in openDrive format.
double s
The starting offset of this lane section.
NBNode * getToNode() const
Returns the destination node of the edge.
Definition: NBEdge.h:434
std::string joinToString(const std::vector< T > &v, const T_BETWEEN &between, std::streamsize accuracy=gPrecision)
Definition: ToString.h:228
#define UNSET_CONNECTION
TrafficLightType
SVCPermissions getPermissions(const std::string &type) const
Returns allowed vehicle classes for the given type.
Definition: NBTypeCont.cpp:211
A storage for available types of edges.
Definition: NBTypeCont.h:62
std::string streetName
The road name of the edge.
static void buildConnectionsToOuter(const Connection &c, const std::map< std::string, OpenDriveEdge *> &innerEdges, std::vector< Connection > &into, std::set< Connection > &seen)
void myEndElement(int element)
Called when a closing tag occurs.
Coefficients of an elevation profile (3rd degree polynomial)