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-2017 German Aerospace Center (DLR) and others.
4 /****************************************************************************/
5 //
6 // This program and the accompanying materials
7 // are made available under the terms of the Eclipse Public License v2.0
8 // which accompanies this distribution, and is available at
9 // http://www.eclipse.org/legal/epl-v20.html
10 //
11 /****************************************************************************/
20 // Importer for networks stored in openDrive format
21 /****************************************************************************/
22 
23 
24 // ===========================================================================
25 // included modules
26 // ===========================================================================
27 #ifdef _MSC_VER
28 #include <windows_config.h>
29 #else
30 #include <config.h>
31 #endif
32 #include <string>
33 #include <cmath>
34 #include <iterator>
38 #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 //#define DEBUG_VARIABLE_WIDTHS
61 //#define DEBUG_VARIABLE_SPEED
62 //#define DEBUG_CONNECTIONS
63 //#define DEBUG_SPIRAL
64 
65 //#define DEBUG_COND(road) ((road)->id == "42")
66 //#define DEBUG_COND2(edgeID) (StringUtils::startsWith((edgeID), "12"))
67 
68 // ===========================================================================
69 // definitions
70 // ===========================================================================
71 
72 // ===========================================================================
73 // static variables
74 // ===========================================================================
99 
101 };
102 
103 
144  // towards xodr v1.4 speed:unit
146 
148 };
149 
150 
154 
155 // ===========================================================================
156 // method definitions
157 // ===========================================================================
158 // ---------------------------------------------------------------------------
159 // static methods (interface in this case)
160 // ---------------------------------------------------------------------------
161 void
163  // check whether the option is set (properly)
164  if (!oc.isUsableFileList("opendrive-files")) {
165  return;
166  }
167  // prepare types
168  myImportAllTypes = oc.getBool("opendrive.import-all-lanes");
169  myImportWidths = !oc.getBool("opendrive.ignore-widths");
170  myMinWidth = oc.getFloat("opendrive.min-width");
171  NBTypeCont& tc = nb.getTypeCont();
172  // build the handler
173  std::map<std::string, OpenDriveEdge*> edges;
174  NIImporter_OpenDrive handler(nb.getTypeCont(), edges);
175  // parse file(s)
176  std::vector<std::string> files = oc.getStringVector("opendrive-files");
177  for (std::vector<std::string>::const_iterator file = files.begin(); file != files.end(); ++file) {
178  if (!FileHelpers::isReadable(*file)) {
179  WRITE_ERROR("Could not open opendrive file '" + *file + "'.");
180  return;
181  }
182  handler.setFileName(*file);
183  PROGRESS_BEGIN_MESSAGE("Parsing opendrive from '" + *file + "'");
184  XMLSubSys::runParser(handler, *file);
186  }
187  // split inner/outer edges
188  std::map<std::string, OpenDriveEdge*> innerEdges, outerEdges;
189  for (std::map<std::string, OpenDriveEdge*>::iterator i = edges.begin(); i != edges.end(); ++i) {
190  if ((*i).second->isInner) {
191  innerEdges[(*i).first] = (*i).second;
192  } else {
193  outerEdges[(*i).first] = (*i).second;
194  }
195  }
196 
197  // convert geometries into a discretised representation
198  computeShapes(edges);
199  // check whether lane sections are valid and whether further must be introduced
200  revisitLaneSections(tc, edges);
201 
202  // -------------------------
203  // node building
204  // -------------------------
205  // build nodes#1
206  // look at all links which belong to a node, collect their bounding boxes
207  // and place the node in the middle of this bounding box
208  std::map<std::string, Boundary> posMap;
209  std::map<std::string, std::string> edge2junction;
210  // compute node positions
211  for (std::map<std::string, OpenDriveEdge*>::iterator i = innerEdges.begin(); i != innerEdges.end(); ++i) {
212  OpenDriveEdge* e = (*i).second;
213  assert(e->junction != "-1" && e->junction != "");
214  edge2junction[e->id] = e->junction;
215  if (posMap.find(e->junction) == posMap.end()) {
216  posMap[e->junction] = Boundary();
217  }
218  posMap[e->junction].add(e->geom.getBoxBoundary());
219  }
220  // build nodes
221  for (std::map<std::string, Boundary>::iterator i = posMap.begin(); i != posMap.end(); ++i) {
222  //std::cout << " import node=" << (*i).first << " z=" << (*i).second.getCenter() << " boundary=" << (*i).second << "\n";
223  if (!nb.getNodeCont().insert((*i).first, (*i).second.getCenter())) {
224  throw ProcessError("Could not add node '" + (*i).first + "'.");
225  }
226  }
227  // assign built nodes
228  for (std::map<std::string, OpenDriveEdge*>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
229  OpenDriveEdge* e = (*i).second;
230  for (std::vector<OpenDriveLink>::iterator j = e->links.begin(); j != e->links.end(); ++j) {
231  OpenDriveLink& l = *j;
232  const std::string& nid = l.elementID;
233  if (l.elementType != OPENDRIVE_ET_ROAD) {
234  if (nb.getNodeCont().retrieve(nid) == 0) {
235  // not yet seen, build (possibly a junction without connections)
236  Position pos = l.linkType == OPENDRIVE_LT_SUCCESSOR ? e->geom[-1] : e->geom[0];
237  if (!nb.getNodeCont().insert(nid, pos)) {
238  throw ProcessError("Could not build node '" + nid + "'.");
239  }
240  }
241  // set node information
243  continue;
244  }
245  if (edge2junction.find(l.elementID) != edge2junction.end()) {
246  // set node information of an internal road
247  setNodeSecure(nb.getNodeCont(), *e, edge2junction[l.elementID], l.linkType);
248  continue;
249  }
250  }
251  }
252  // we should now have all nodes set for links which are not outer edge-to-outer edge links
253 
254 
255  // build nodes#2
256  // build nodes for all outer edge-to-outer edge connections
257  for (std::map<std::string, OpenDriveEdge*>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
258  OpenDriveEdge* e = (*i).second;
259  for (std::vector<OpenDriveLink>::iterator j = e->links.begin(); j != e->links.end(); ++j) {
260  OpenDriveLink& l = *j;
261  if (l.elementType != OPENDRIVE_ET_ROAD || edge2junction.find(l.elementID) != edge2junction.end()) {
262  // is a connection to an internal edge, or a node, skip
263  continue;
264  }
265  // we have a direct connection between to external edges
266  std::string id1 = e->id;
267  std::string id2 = l.elementID;
268  if (id1 < id2) {
269  std::swap(id1, id2);
270  }
271  std::string nid = id1 + "." + id2;
272  if (nb.getNodeCont().retrieve(nid) == 0) {
273  // not yet seen, build
274  Position pos = l.linkType == OPENDRIVE_LT_SUCCESSOR ? e->geom[-1] : e->geom[0];
275  if (!nb.getNodeCont().insert(nid, pos)) {
276  throw ProcessError("Could not build node '" + nid + "'.");
277  }
278  }
279  /* debug-stuff
280  else {
281  Position pos = l.linkType==OPENDRIVE_LT_SUCCESSOR ? e.geom[e.geom.size()-1] : e.geom[0];
282  cout << nid << " " << pos << " " << nb.getNodeCont().retrieve(nid)->getPosition() << endl;
283  }
284  */
285  setNodeSecure(nb.getNodeCont(), *e, nid, l.linkType);
286  }
287  }
288  // we should now have start/end nodes for all outer edge-to-outer edge connections
289 
290 
291  // build nodes#3
292  // assign further nodes generated from inner-edges
293  // these nodes have not been assigned earlier, because the connections are referenced in inner-edges
294  for (std::map<std::string, OpenDriveEdge*>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
295  OpenDriveEdge* e = (*i).second;
296  if (e->to != 0 && e->from != 0) {
297  continue;
298  }
299  for (std::map<std::string, OpenDriveEdge*>::iterator j = innerEdges.begin(); j != innerEdges.end(); ++j) {
300  OpenDriveEdge* ie = (*j).second;
301  for (std::vector<OpenDriveLink>::iterator k = ie->links.begin(); k != ie->links.end(); ++k) {
302  OpenDriveLink& il = *k;
303  if (il.elementType != OPENDRIVE_ET_ROAD || il.elementID != e->id) {
304  // not conneted to the currently investigated outer edge
305  continue;
306  }
307  std::string nid = edge2junction[ie->id];
308  if (il.contactPoint == OPENDRIVE_CP_START) {
310  } else {
312  }
313  }
314  }
315 
316  }
317 
318  // build start/end nodes which were not defined previously
319  for (std::map<std::string, OpenDriveEdge*>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
320  OpenDriveEdge* e = (*i).second;
321  if ((e->from == 0 || e->to == 0) && e->geom.size() == 0) {
322  continue;
323  }
324  if (e->from == 0) {
325  const std::string nid = e->id + ".begin";
326  e->from = getOrBuildNode(nid, e->geom.front(), nb.getNodeCont());
327  }
328  if (e->to == 0) {
329  const std::string nid = e->id + ".end";
330  e->to = getOrBuildNode(nid, e->geom.back(), nb.getNodeCont());
331  }
332  }
333 
334 
335  // -------------------------
336  // edge building
337  // -------------------------
338  double defaultSpeed = tc.getSpeed("");
339  const bool saveOrigIDs = OptionsCont::getOptions().getBool("output.original-names");
340  // build edges
341  for (std::map<std::string, OpenDriveEdge*>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
342  OpenDriveEdge* e = (*i).second;
343  if (e->geom.size() == 0) {
344  WRITE_WARNING("Ignoring road '" + e->id + "' without geometry.");
345  continue;
346  }
347  bool lanesBuilt = false;
348 
349  // go along the lane sections, build a node in between of each pair
350 
353 
355  NBNode* sFrom = e->from;
356  NBNode* sTo = e->to;
357  int priorityR = e->getPriority(OPENDRIVE_TAG_RIGHT);
358  int priorityL = e->getPriority(OPENDRIVE_TAG_LEFT);
359  double sB = 0;
360  double sE = e->length;
361  // 0-length geometries are possible if only the inner points are represented
362  const double length2D = e->geom.length2D();
363  double cF = length2D == 0 ? 1 : e->length / length2D;
364  NBEdge* prevRight = 0;
365  NBEdge* prevLeft = 0;
366 
367  // starting at the same node as ending, and no lane sections?
368  if (sFrom == sTo && e->laneSections.size() == 1) {
369  // --> loop, split!
371  ls.s = e->length / 2.;
372  e->laneSections.push_back(ls);
373  WRITE_WARNING("Edge '" + e->id + "' has to be split as it connects same junctions.")
374  }
375  if (myMinWidth > 0) {
376  const double minDist = oc.getFloat("opendrive.curve-resolution");
377  splitMinWidths(e, tc, minDist);
378  }
379 
380  // build along lane sections
381  for (std::vector<OpenDriveLaneSection>::iterator j = e->laneSections.begin(); j != e->laneSections.end(); ++j) {
382  // add internal node if needed
383  if (j == e->laneSections.end() - 1) {
384  sTo = e->to;
385  sE = e->length / cF;
386  } else {
387  double nextS = (j + 1)->s;
388  sTo = new NBNode(e->id + "." + toString(nextS), e->geom.positionAtOffset(nextS));
389  if (!nb.getNodeCont().insert(sTo)) {
390  throw ProcessError("Could not add node '" + sTo->getID() + "'.");
391  }
392  sE = nextS / cF;
393  }
394  PositionVector geom = e->geom.getSubpart2D(sB, sE);
395  std::string id = e->id;
396  if (sFrom != e->from || sTo != e->to) {
397  id = id + "." + toString((*j).s);
398  } else if (e->laneSections.size() == 1) {
399  id = id + ".0.00";
400  }
401 #ifdef DEBUG_VARIABLE_WIDTHS
402  if (DEBUG_COND(e)) {
403  std::cout << " id=" << id << " sB=" << sB << " sE=" << sE << " geom=" << geom << "\n";
404  }
405 #endif
406 
407  // build lanes to right
408  NBEdge* currRight = 0;
409  if ((*j).rightLaneNumber > 0) {
410  currRight = new NBEdge("-" + id, sFrom, sTo, (*j).rightType, defaultSpeed, (*j).rightLaneNumber, priorityR,
412  lanesBuilt = true;
413  const std::vector<OpenDriveLane>& lanes = (*j).lanesByDir[OPENDRIVE_TAG_RIGHT];
414  for (std::vector<OpenDriveLane>::const_iterator k = lanes.begin(); k != lanes.end(); ++k) {
415  std::map<int, int>::const_iterator lp = (*j).laneMap.find((*k).id);
416  if (lp != (*j).laneMap.end()) {
417  int sumoLaneIndex = lp->second;
418  NBEdge::Lane& sumoLane = currRight->getLaneStruct(sumoLaneIndex);
419  const OpenDriveLane& odLane = *k;
420  if (saveOrigIDs) {
421  sumoLane.setParameter(SUMO_PARAM_ORIGID, e->id + "_" + toString((*k).id));
422  }
423  sumoLane.speed = odLane.speed != 0 ? odLane.speed : tc.getSpeed(odLane.type);
424  sumoLane.permissions = tc.getPermissions(odLane.type);
425  sumoLane.width = myImportWidths && odLane.width != NBEdge::UNSPECIFIED_WIDTH ? odLane.width : tc.getWidth(odLane.type);
426  if (sumoLane.width < myMinWidth
427  && sumoLane.permissions != SVC_BICYCLE
428  && sumoLane.permissions != SVC_PEDESTRIAN
429  && sumoLane.width < tc.getWidth(odLane.type)) {
431  }
432  }
433  }
434  if (!nb.getEdgeCont().insert(currRight, myImportAllTypes)) {
435  throw ProcessError("Could not add edge '" + currRight->getID() + "'.");
436  }
437  if (nb.getEdgeCont().wasIgnored(id)) {
438  prevRight = 0;
439  } else {
440  // connect lane sections
441  if (prevRight != 0) {
442  std::map<int, int> connections = (*j).getInnerConnections(OPENDRIVE_TAG_RIGHT, *(j - 1));
443  for (std::map<int, int>::const_iterator k = connections.begin(); k != connections.end(); ++k) {
444 #ifdef DEBUG_CONNECTIONS
445  if (DEBUG_COND(e)) {
446  std::cout << "addCon1 from=" << prevRight->getID() << "_" << (*k).first << " to=" << currRight->getID() << "_" << (*k).second << "\n";
447  }
448 #endif
449  prevRight->addLane2LaneConnection((*k).first, currRight, (*k).second, NBEdge::L2L_VALIDATED);
450  }
451  }
452  prevRight = currRight;
453  }
454  }
455 
456  // build lanes to left
457  NBEdge* currLeft = 0;
458  if ((*j).leftLaneNumber > 0) {
459  currLeft = new NBEdge(id, sTo, sFrom, (*j).leftType, defaultSpeed, (*j).leftLaneNumber, priorityL,
461  lanesBuilt = true;
462  const std::vector<OpenDriveLane>& lanes = (*j).lanesByDir[OPENDRIVE_TAG_LEFT];
463  for (std::vector<OpenDriveLane>::const_iterator k = lanes.begin(); k != lanes.end(); ++k) {
464  std::map<int, int>::const_iterator lp = (*j).laneMap.find((*k).id);
465  if (lp != (*j).laneMap.end()) {
466  int sumoLaneIndex = lp->second;
467  NBEdge::Lane& sumoLane = currLeft->getLaneStruct(sumoLaneIndex);
468  const OpenDriveLane& odLane = *k;
469  if (saveOrigIDs) {
470  sumoLane.setParameter(SUMO_PARAM_ORIGID, e->id + "_" + toString((*k).id));
471  }
472  sumoLane.speed = odLane.speed != 0 ? odLane.speed : tc.getSpeed(odLane.type);
473  sumoLane.permissions = tc.getPermissions(odLane.type);
474  sumoLane.width = myImportWidths && odLane.width != NBEdge::UNSPECIFIED_WIDTH ? odLane.width : tc.getWidth(odLane.type);
475  if (sumoLane.width < myMinWidth
476  && sumoLane.permissions != SVC_BICYCLE
477  && sumoLane.permissions != SVC_PEDESTRIAN
478  && sumoLane.width < tc.getWidth(odLane.type)) {
480  }
481  }
482  }
483  if (!nb.getEdgeCont().insert(currLeft, myImportAllTypes)) {
484  throw ProcessError("Could not add edge '" + currLeft->getID() + "'.");
485  }
486  if (nb.getEdgeCont().wasIgnored(id)) {
487  prevLeft = 0;
488  } else {
489  // connect lane sections
490  if (prevLeft != 0) {
491  std::map<int, int> connections = (*j).getInnerConnections(OPENDRIVE_TAG_LEFT, *(j - 1));
492  for (std::map<int, int>::const_iterator k = connections.begin(); k != connections.end(); ++k) {
493 #ifdef DEBUG_CONNECTIONS
494  if (DEBUG_COND(e)) {
495  std::cout << "addCon2 from=" << currLeft->getID() << "_" << (*k).first << " to=" << prevLeft->getID() << "_" << (*k).second << "\n";
496  }
497 #endif
498  currLeft->addLane2LaneConnection((*k).first, prevLeft, (*k).second, NBEdge::L2L_VALIDATED);
499  }
500  }
501  prevLeft = currLeft;
502  }
503  }
504  (*j).sumoID = id;
505 
506 
507  sB = sE;
508  sFrom = sTo;
509  }
510  if (!lanesBuilt) {
511  WRITE_WARNING("Edge '" + e->id + "' has no lanes.");
512  }
513  }
514 
515  // -------------------------
516  // connections building
517  // -------------------------
518  // generate explicit lane-to-lane connections
519  for (std::map<std::string, OpenDriveEdge*>::iterator i = edges.begin(); i != edges.end(); ++i) {
520  setEdgeLinks2(*(*i).second, edges);
521  }
522  // compute connections across intersections, if any
523  std::vector<Connection> connections2;
524  for (std::map<std::string, OpenDriveEdge*>::iterator j = edges.begin(); j != edges.end(); ++j) {
525  const std::set<Connection>& conns = (*j).second->connections;
526 
527  for (std::set<Connection>::const_iterator i = conns.begin(); i != conns.end(); ++i) {
528  if (innerEdges.find((*i).fromEdge) != innerEdges.end()) {
529  // connections starting at inner edges are processed by starting from outer edges
530  continue;
531  }
532  if (innerEdges.find((*i).toEdge) != innerEdges.end()) {
533  std::set<Connection> seen;
534  buildConnectionsToOuter(*i, innerEdges, connections2, seen);
535  } else {
536  connections2.push_back(*i);
537  }
538  }
539  }
540  // set connections
541  for (std::vector<Connection>::const_iterator i = connections2.begin(); i != connections2.end(); ++i) {
542  std::string fromEdge = (*i).fromEdge;
543  if (edges.find(fromEdge) == edges.end()) {
544  WRITE_WARNING("While setting connections: from-edge '" + fromEdge + "' is not known.");
545  continue;
546  }
547  OpenDriveEdge* odFrom = edges[fromEdge];
548  int fromLane = (*i).fromLane;
549  bool fromLast = ((*i).fromCP == OPENDRIVE_CP_END) ^ ((*i).fromLane > 0 && !(*i).all);
550  fromEdge = fromLast ? odFrom->laneSections.back().sumoID : odFrom->laneSections[0].sumoID;
551 
552  std::string toEdge = (*i).toEdge;
553  if (edges.find(toEdge) == edges.end()) {
554  WRITE_WARNING("While setting connections: to-edge '" + toEdge + "' is not known.");
555  continue;
556  }
557 
558  OpenDriveEdge* odTo = edges[toEdge];
559  int toLane = (*i).toLane;
560  bool toLast = ((*i).toCP == OPENDRIVE_CP_END) || ((*i).toLane > 0);
561  toEdge = toLast ? odTo->laneSections.back().sumoID : odTo->laneSections[0].sumoID;
562 
563  if (fromLane == UNSET_CONNECTION) {
564  continue;
565  }
566  if (fromLane < 0) {
567  fromEdge = revertID(fromEdge);
568  }
569  if (toLane == UNSET_CONNECTION) {
570  continue;
571  }
572  if (toLane < 0) {
573  toEdge = revertID(toEdge);
574  }
575  fromLane = fromLast ? odFrom->laneSections.back().laneMap[fromLane] : odFrom->laneSections[0].laneMap[fromLane];
576  toLane = toLast ? odTo->laneSections.back().laneMap[toLane] : odTo->laneSections[0].laneMap[toLane];
577  NBEdge* from = nb.getEdgeCont().retrieve(fromEdge);
578  NBEdge* to = nb.getEdgeCont().retrieve(toEdge);
579  if (from == 0) {
580  WRITE_WARNING("Could not find fromEdge representation of '" + fromEdge + "' in connection '" + (*i).origID + "'.");
581  }
582  if (to == 0) {
583  WRITE_WARNING("Could not find fromEdge representation of '" + toEdge + "' in connection '" + (*i).origID + "'.");
584  }
585  if (from == 0 || to == 0) {
586  continue;
587  }
588 
589 #ifdef DEBUG_CONNECTIONS
590  if (DEBUG_COND2(from->getID())) {
591  std::cout << "addCon3 from=" << from->getID() << "_" << fromLane << " to=" << to->getID() << "_" << toLane << "\n";
592  }
593 #endif
594  from->addLane2LaneConnection(fromLane, to, toLane, NBEdge::L2L_USER);
595 
596  if ((*i).origID != "" && saveOrigIDs) {
597  // @todo: this is the most silly way to determine the connection
598  std::vector<NBEdge::Connection>& cons = from->getConnections();
599  for (std::vector<NBEdge::Connection>::iterator k = cons.begin(); k != cons.end(); ++k) {
600  if ((*k).fromLane == fromLane && (*k).toEdge == to && (*k).toLane == toLane) {
601  (*k).setParameter(SUMO_PARAM_ORIGID, (*i).origID + "_" + toString((*i).origLane));
602  break;
603  }
604  }
605  }
606  }
607 
608 
609  // -------------------------
610  // traffic lights
611  // -------------------------
612  std::map<std::string, std::string> tlsControlled;
613  for (std::map<std::string, OpenDriveEdge*>::iterator i = edges.begin(); i != edges.end(); ++i) {
614  OpenDriveEdge* e = (*i).second;
615  for (std::vector<OpenDriveSignal>::const_iterator j = e->signals.begin(); j != e->signals.end(); ++j) {
616  if ((*j).type != "1000001") { // traffic_light (Section 6.11)
617  continue;
618  }
619  std::vector<OpenDriveLaneSection>::iterator k = e->laneSections.begin();
620  bool found = false;
621  for (; k != e->laneSections.end() - 1 && !found;) {
622  if ((*j).s > (*k).s && (*j).s <= (*(k + 1)).s) {
623  found = true;
624  } else {
625  ++k;
626  }
627  }
628 
629  // @todo: major problem, currently, still not completely solved:
630  // inner edges may have traffic lights, too. Nice on one hand, as directions can be recognized
631  // but hard to follow backwards
632  std::string id = (*k).sumoID;
633  if (id == "") {
634  if (e->junction != "") {
635  //WRITE_WARNING("Found a traffic light signal on an internal edge; will not build it (original edge id='" + e->id + "').");
636  std::string fromID, toID;
637  for (std::vector<OpenDriveLink>::const_iterator l = e->links.begin(); l != e->links.end(); ++l) {
638  if ((*l).linkType == OPENDRIVE_LT_PREDECESSOR && (*l).elementType == OPENDRIVE_ET_ROAD) {
639  if (fromID != "") {
640  WRITE_WARNING("Ambigous start of connection.");
641  }
642  OpenDriveEdge* e = edges[(*l).elementID];
643  if ((*l).contactPoint == OPENDRIVE_CP_START) {
644  fromID = e->laneSections[0].sumoID;
645  if ((*j).orientation < 0) {
646  fromID = "-" + fromID;
647  }
648  } else {
649  fromID = e->laneSections.back().sumoID;
650  if ((*j).orientation > 0) {
651  fromID = "-" + fromID;
652  }
653  }
654  }
655  if ((*l).linkType == OPENDRIVE_LT_SUCCESSOR && (*l).elementType == OPENDRIVE_ET_ROAD) {
656  if (toID != "") {
657  WRITE_WARNING("Ambigous end of connection.");
658  }
659  OpenDriveEdge* e = edges[(*l).elementID];
660  toID = (*l).contactPoint == OPENDRIVE_CP_START ? e->laneSections[0].sumoID : e->laneSections.back().sumoID;
661  }
662  }
663  id = fromID + "->" + toID;
664  } else {
665  WRITE_WARNING("Found a traffic light signal on an unknown edge (original edge id='" + e->id + "').");
666  continue;
667  }
668  } else {
669  if ((*j).orientation > 0) {
670  id = "-" + id;
671  }
672  }
673  tlsControlled[id] = (*j).name;
674  }
675  }
676 
677  for (std::map<std::string, std::string>::iterator i = tlsControlled.begin(); i != tlsControlled.end(); ++i) {
678  std::string id = (*i).first;
679  if (id.find("->") != std::string::npos) {
680  id = id.substr(0, id.find("->"));
681  }
682  NBEdge* e = nb.getEdgeCont().retrieve(id);
683  if (e == 0) {
684  WRITE_WARNING("Could not find edge '" + id + "' while building its traffic light.");
685  continue;
686  }
687  NBNode* toNode = e->getToNode();
688  if (!toNode->isTLControlled()) {
690  NBOwnTLDef* tlDef = new NBOwnTLDef(toNode->getID(), toNode, 0, type);
691  if (!nb.getTLLogicCont().insert(tlDef)) {
692  // actually, nothing should fail here
693  delete tlDef;
694  throw ProcessError();
695  }
696  toNode->addTrafficLight(tlDef);
697  //tlDef->setSinglePhase();
698  }
699  NBTrafficLightDefinition* tlDef = *toNode->getControllingTLS().begin();
700  tlDef->setParameter("connection:" + id, (*i).second);
701  }
702 
703  // -------------------------
704  // clean up
705  // -------------------------
706  for (std::map<std::string, OpenDriveEdge*>::iterator i = edges.begin(); i != edges.end(); ++i) {
707  delete(*i).second;
708  }
709 }
710 
711 
712 void
713 NIImporter_OpenDrive::buildConnectionsToOuter(const Connection& c, const std::map<std::string, OpenDriveEdge*>& innerEdges, std::vector<Connection>& into, std::set<Connection>& seen) {
714  //std::cout << "buildConnectionsToOuter "
715  // << " seen=" << seen.size()
716  // << " from=" << c.fromEdge
717  // << " to=" << c.toEdge
718  // << " fromLane=" << c.fromLane
719  // << " toLane=" << c.toLane
720  // << " fromCP=" << c.fromCP
721  // << " toCP=" << c.toCP
722  // << " all=" << c.all
723  // << " origID=" << c.origID
724  // << " origLane=" << c.origLane
725  // << " seenlist=";
726  //for (std::set<Connection>::const_iterator i = seen.begin(); i != seen.end(); ++i) {
727  // std::cout << (*i).fromEdge << "," << (*i).toEdge << " ";
728  //}
729  //std::cout << "\n";
730 
731  OpenDriveEdge* dest = innerEdges.find(c.toEdge)->second;
732  if (dest == 0) {
734  return;
735  }
736  seen.insert(c);
737  const std::set<Connection>& conts = dest->connections;
738  for (std::set<Connection>::const_iterator i = conts.begin(); i != conts.end(); ++i) {
739  if (innerEdges.find((*i).toEdge) != innerEdges.end()) {
740  std::vector<Connection> t;
741  if (seen.count(*i) == 0) {
742  buildConnectionsToOuter(*i, innerEdges, t, seen);
743  for (std::vector<Connection>::const_iterator j = t.begin(); j != t.end(); ++j) {
744  // @todo this section is unverified
745  Connection cn = (*j);
746  cn.fromEdge = c.fromEdge;
747  cn.fromLane = c.fromLane;
748  cn.fromCP = c.fromCP;
749  cn.all = c.all; // @todo "all" is a hack trying to avoid the "from is zero" problem;
750  into.push_back(cn);
751  }
752  } else {
753  WRITE_WARNING("Circular connections in junction including roads '" + c.fromEdge + "' and '" + c.toEdge + "', loop size " + toString(seen.size()));
754  }
755  } else {
756  if ((*i).fromLane == c.toLane) {
757  Connection cn = (*i);
758  cn.fromEdge = c.fromEdge;
759  cn.fromLane = c.fromLane;
760  cn.fromCP = c.fromCP;
761  cn.all = c.all;
762  cn.origID = c.toEdge;
763  cn.origLane = c.toLane;
764  into.push_back(cn);
765  }
766  }
767  }
768 }
769 
770 
771 void
772 NIImporter_OpenDrive::setEdgeLinks2(OpenDriveEdge& e, const std::map<std::string, OpenDriveEdge*>& edges) {
773  for (std::vector<OpenDriveLink>::iterator i = e.links.begin(); i != e.links.end(); ++i) {
774  OpenDriveLink& l = *i;
775  if (l.elementType != OPENDRIVE_ET_ROAD) {
776  // we assume that links to nodes are later given as connections to edges
777  continue;
778  }
779  // get the right direction of the connected edge
780  std::string connectedEdge = l.elementID;
781  std::string edgeID = e.id;
782 
783  OpenDriveLaneSection& laneSection = l.linkType == OPENDRIVE_LT_SUCCESSOR ? e.laneSections.back() : e.laneSections[0];
784  const std::map<int, int>& laneMap = laneSection.laneMap;
785 #ifdef DEBUG_CONNECTIONS
786  if (DEBUG_COND(&e)) {
787  std::cout << "edge=" << e.id << " eType=" << l.elementType << " lType=" << l.linkType << " connectedEdge=" << connectedEdge << " laneSection=" << laneSection.s << " map:\n";
788  std::cout << joinToString(laneMap, "\n", ":") << "\n";
789  }
790 #endif
791  if (laneSection.lanesByDir.find(OPENDRIVE_TAG_RIGHT) != laneSection.lanesByDir.end()) {
792  const std::vector<OpenDriveLane>& lanes = laneSection.lanesByDir.find(OPENDRIVE_TAG_RIGHT)->second;
793  for (std::vector<OpenDriveLane>::const_iterator j = lanes.begin(); j != lanes.end(); ++j) {
794  if (!myImportAllTypes && laneMap.find((*j).id) == laneMap.end()) {
795  continue;
796  }
797  Connection c; // @todo: give Connection a new name and a constructor
798  c.fromEdge = e.id;
799  c.fromLane = (*j).id;
801  c.toLane = l.linkType == OPENDRIVE_LT_SUCCESSOR ? (*j).successor : (*j).predecessor;
802  c.toEdge = connectedEdge;
803  c.toCP = l.contactPoint;
804  c.all = false;
805  if (l.linkType != OPENDRIVE_LT_SUCCESSOR) {
806  std::swap(c.fromEdge, c.toEdge);
807  std::swap(c.fromLane, c.toLane);
808  std::swap(c.fromCP, c.toCP);
809  }
810  if (edges.find(c.fromEdge) == edges.end()) {
811  WRITE_ERROR("While setting connections: incoming road '" + c.fromEdge + "' is not known.");
812  } else {
813  OpenDriveEdge* src = edges.find(c.fromEdge)->second;
814  src->connections.insert(c);
815 #ifdef DEBUG_CONNECTIONS
816  if (DEBUG_COND(src) std::cout << "insertConRight from=" << src->id << "_" << c.fromLane << " to=" << c.toEdge << "_" << c.toLane << "\n";
817 #endif
818  }
819  }
820  }
821  if (laneSection.lanesByDir.find(OPENDRIVE_TAG_LEFT) != laneSection.lanesByDir.end()) {
822  const std::vector<OpenDriveLane>& lanes = laneSection.lanesByDir.find(OPENDRIVE_TAG_LEFT)->second;
823  for (std::vector<OpenDriveLane>::const_iterator j = lanes.begin(); j != lanes.end(); ++j) {
824  if (!myImportAllTypes && laneMap.find((*j).id) == laneMap.end()) {
825  continue;
826  }
827  Connection c;
828  c.toEdge = e.id;
829  c.toLane = (*j).id;
831  c.fromLane = l.linkType == OPENDRIVE_LT_SUCCESSOR ? (*j).successor : (*j).predecessor;
832  c.fromEdge = connectedEdge;
833  c.fromCP = l.contactPoint;
834  c.all = false;
835  if (l.linkType != OPENDRIVE_LT_SUCCESSOR) {
836  std::swap(c.fromEdge, c.toEdge);
837  std::swap(c.fromLane, c.toLane);
838  std::swap(c.fromCP, c.toCP);
839  }
840  if (edges.find(c.fromEdge) == edges.end()) {
841  WRITE_ERROR("While setting connections: incoming road '" + c.fromEdge + "' is not known.");
842  } else {
843  OpenDriveEdge* src = edges.find(c.fromEdge)->second;
844  src->connections.insert(c);
845 #ifdef DEBUG_CONNECTIONS
846  if (DEBUG_COND2(src)) {
847  std::cout << "insertConLeft from=" << src->id << "_" << c.fromLane << " to=" << c.toEdge << "_" << c.toLane << "\n";
848  }
849 #endif
850  }
851  }
852  }
853  }
854 }
855 
856 
857 std::string NIImporter_OpenDrive::revertID(const std::string& id) {
858  if (id[0] == '-') {
859  return id.substr(1);
860  }
861  return "-" + id;
862 }
863 
864 
865 NBNode*
866 NIImporter_OpenDrive::getOrBuildNode(const std::string& id, const Position& pos,
867  NBNodeCont& nc) {
868  if (nc.retrieve(id) == 0) {
869  // not yet built; build now
870  if (!nc.insert(id, pos)) {
871  // !!! clean up
872  throw ProcessError("Could not add node '" + id + "'.");
873  }
874  }
875  return nc.retrieve(id);
876 }
877 
878 
879 void
881  const std::string& nodeID, NIImporter_OpenDrive::LinkType lt) {
882  NBNode* n = nc.retrieve(nodeID);
883  if (n == 0) {
884  throw ProcessError("Could not find node '" + nodeID + "'.");
885  }
886  if (lt == OPENDRIVE_LT_SUCCESSOR) {
887  if (e.to != 0 && e.to != n) {
888  throw ProcessError("Edge '" + e.id + "' has two end nodes.");
889  }
890  e.to = n;
891  } else {
892  if (e.from != 0 && e.from != n) {
893  throw ProcessError("Edge '" + e.id + "' has two start nodes.");
894  }
895  e.from = n;
896  }
897 }
898 
899 
900 void
901 NIImporter_OpenDrive::computeShapes(std::map<std::string, OpenDriveEdge*>& edges) {
903  const double res = oc.getFloat("opendrive.curve-resolution");
904  for (std::map<std::string, OpenDriveEdge*>::iterator i = edges.begin(); i != edges.end(); ++i) {
905  OpenDriveEdge& e = *(*i).second;
907  for (std::vector<OpenDriveGeometry>::iterator j = e.geometries.begin(); j != e.geometries.end(); ++j) {
908  OpenDriveGeometry& g = *j;
909  PositionVector geom;
910  switch (g.type) {
912  break;
913  case OPENDRIVE_GT_LINE:
914  geom = geomFromLine(e, g);
915  break;
916  case OPENDRIVE_GT_SPIRAL:
917  geom = geomFromSpiral(e, g, res);
918  break;
919  case OPENDRIVE_GT_ARC:
920  geom = geomFromArc(e, g, res);
921  break;
922  case OPENDRIVE_GT_POLY3:
923  geom = geomFromPoly(e, g, res);
924  break;
926  geom = geomFromParamPoly(e, g, res);
927  break;
928  default:
929  break;
930  }
931  if (e.geom.size() > 0 && prevType == OPENDRIVE_GT_LINE) {
932  // remove redundant end point of the previous geometry segment
933  // (the start point of the current segment should have the same value)
934  // this avoids geometry errors due to imprecision
935  if (!e.geom.back().almostSame(geom.front())) {
936  const int index = (int)(j - e.geometries.begin());
937  WRITE_WARNING("Mismatched geometry for edge '" + e.id + "' between geometry segments " + toString(index - 1) + " and " + toString(index) + ".");
938  }
939  e.geom.pop_back();
940  }
941  //std::cout << " adding geometry to road=" << e.id << " old=" << e.geom << " new=" << geom << "\n";
942  for (PositionVector::iterator k = geom.begin(); k != geom.end(); ++k) {
944  }
945  prevType = g.type;
946  }
947  if (oc.exists("geometry.min-dist") && !oc.isDefault("geometry.min-dist")) {
948  e.geom.removeDoublePoints(oc.getFloat("geometry.min-dist"), true);
949  }
951  WRITE_ERROR("Unable to project coordinates for edge '" + e.id + "'.");
952  }
953  // add z-data
954  int k = 0;
955  double pos = 0;
956  for (std::vector<OpenDriveElevation>::iterator j = e.elevations.begin(); j != e.elevations.end(); ++j) {
957  const OpenDriveElevation& el = *j;
958  const double sNext = (j + 1) == e.elevations.end() ? std::numeric_limits<double>::max() : (*(j + 1)).s;
959  while (k < (int)e.geom.size() && pos < sNext) {
960  const double z = el.computeAt(pos);
961  //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";
962  e.geom[k].add(0, 0, z);
963  k++;
964  if (k < (int)e.geom.size()) {
965  // XXX pos understimates the actual position since the
966  // actual geometry between k-1 and k could be curved
967  pos += e.geom[k - 1].distanceTo2D(e.geom[k]);
968  }
969  }
970  }
971  // add laneoffset
972  if (e.offsets.size() > 0) {
973  // make sure there are intermediate points for each offset-section
974  for (std::vector<OpenDriveLaneOffset>::iterator j = e.offsets.begin(); j != e.offsets.end(); ++j) {
975  const OpenDriveLaneOffset& el = *j;
976  // check wether we need to insert a new point at dist
977  Position pS = e.geom.positionAtOffset2D(el.s);
978  int iS = e.geom.indexOfClosest(pS);
979  // prevent close spacing to reduce impact of rounding errors in z-axis
980  if (pS.distanceTo2D(e.geom[iS]) > POSITION_EPS) {
981  e.geom.insertAtClosest(pS);
982  //std::cout << " edge=" << e.id << " inserting pos=" << pS << " s=" << el.s << " iS=" << iS << " dist=" << pS.distanceTo2D(e.geom[iS]) << "\n";
983  }
984  }
985  // XXX add further points for sections with non-constant offset
986  // shift each point orthogonally by the specified offset
987  int k = 0;
988  double pos = 0;
989  PositionVector geom2;
990  for (std::vector<OpenDriveLaneOffset>::iterator j = e.offsets.begin(); j != e.offsets.end(); ++j) {
991  const OpenDriveLaneOffset& el = *j;
992  const double sNext = (j + 1) == e.offsets.end() ? std::numeric_limits<double>::max() : (*(j + 1)).s;
993  while (k < (int)e.geom.size() && pos < sNext) {
994  const double offset = el.computeAt(pos);
995  //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";
996  if (fabs(offset) > POSITION_EPS) {
997  try {
998  PositionVector tmp = e.geom;
999  // XXX shifting the whole geometry is inefficient. could also use positionAtOffset(lateralOffset=...)
1000  tmp.move2side(-offset);
1001  //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";
1002  geom2.push_back(tmp[k]);
1003  } catch (InvalidArgument&) {
1004  geom2.push_back(e.geom[k]);
1005  }
1006  } else {
1007  geom2.push_back(e.geom[k]);
1008  }
1009  k++;
1010  if (k < (int)e.geom.size()) {
1011  // XXX pos understimates the actual position since the
1012  // actual geometry between k-1 and k could be curved
1013  pos += e.geom[k - 1].distanceTo2D(e.geom[k]);
1014  }
1015  }
1016  }
1017  assert(e.geom.size() == geom2.size());
1018  e.geom = geom2;
1019  }
1020  //std::cout << " loaded geometry " << e.id << "=" << e.geom << "\n";
1021  }
1022 }
1023 
1024 
1025 void
1026 NIImporter_OpenDrive::revisitLaneSections(const NBTypeCont& tc, std::map<std::string, OpenDriveEdge*>& edges) {
1027  for (std::map<std::string, OpenDriveEdge*>::iterator i = edges.begin(); i != edges.end(); ++i) {
1028  OpenDriveEdge& e = *(*i).second;
1029 #ifdef DEBUG_VARIABLE_SPEED
1030  if (DEBUG_COND(&e)) {
1031  gDebugFlag1 = true;
1032  std::cout << "revisitLaneSections e=" << e.id << "\n";
1033  }
1034 #endif
1035  std::vector<OpenDriveLaneSection>& laneSections = e.laneSections;
1036  // split by speed limits
1037  std::vector<OpenDriveLaneSection> newSections;
1038  for (std::vector<OpenDriveLaneSection>::iterator j = laneSections.begin(); j != laneSections.end(); ++j) {
1039  std::vector<OpenDriveLaneSection> splitSections;
1040  bool splitBySpeed = (*j).buildSpeedChanges(tc, splitSections);
1041  if (!splitBySpeed) {
1042  newSections.push_back(*j);
1043  } else {
1044  std::copy(splitSections.begin(), splitSections.end(), back_inserter(newSections));
1045  }
1046  }
1047 
1048  e.laneSections = newSections;
1049  laneSections = e.laneSections;
1050  double lastS = -1;
1051  // check whether the lane sections are in the right order
1052  bool sorted = true;
1053  for (std::vector<OpenDriveLaneSection>::const_iterator j = laneSections.begin(); j != laneSections.end() && sorted; ++j) {
1054  if ((*j).s <= lastS) {
1055  sorted = false;
1056  }
1057  lastS = (*j).s;
1058  }
1059  if (!sorted) {
1060  WRITE_WARNING("The sections of edge '" + e.id + "' are not sorted properly.");
1061  sort(e.laneSections.begin(), e.laneSections.end(), sections_by_s_sorter());
1062  }
1063  // check whether no duplicates of s-value occure
1064  lastS = -1;
1065  laneSections = e.laneSections;
1066  for (std::vector<OpenDriveLaneSection>::iterator j = laneSections.begin(); j != laneSections.end();) {
1067  bool simlarToLast = fabs((*j).s - lastS) < POSITION_EPS;
1068  lastS = (*j).s;
1069  if (simlarToLast) {
1070  WRITE_WARNING("Almost duplicate s-value '" + toString(lastS) + "' for lane sections occured at edge '" + e.id + "'; second entry was removed.");
1071  j = laneSections.erase(j);
1072  } else {
1073  ++j;
1074  }
1075  }
1076 #ifdef DEBUG_VARIABLE_SPEED
1077  gDebugFlag1 = false;
1078 #endif
1079  }
1080 }
1081 
1082 
1085  UNUSED_PARAMETER(e);
1086  PositionVector ret;
1087  ret.push_back(Position(g.x, g.y));
1088  ret.push_back(calculateStraightEndPoint(g.hdg, g.length, Position(g.x, g.y)));
1089  return ret;
1090 }
1091 
1092 
1095  UNUSED_PARAMETER(e);
1096  PositionVector ret;
1097  double curveStart = g.params[0];
1098  double curveEnd = g.params[1];
1099  try {
1100  double cDot = (curveEnd - curveStart) / g.length;
1101  if (cDot == 0 || g.length == 0) {
1102  WRITE_WARNING("Could not compute spiral geometry for edge '" + e.id + "' (cDot=" + toString(cDot) + " length=" + toString(g.length) + ").");
1103  ret.push_back(Position(g.x, g.y));
1104  }
1105  double sStart = curveStart / cDot;
1106  double sEnd = curveEnd / cDot;
1107  double x = 0;
1108  double y = 0;
1109  double t = 0;
1110  double tStart = 0;
1111  double s;
1112  odrSpiral(sStart, cDot, &x, &y, &tStart);
1113  for (s = sStart; s <= sEnd; s += resolution) {
1114  odrSpiral(s, cDot, &x, &y, &t);
1115  ret.push_back(Position(x, y));
1116  }
1117  if (s != sEnd /*&& ret.size() == 1*/) {
1118  odrSpiral(sEnd, cDot, &x, &y, &t);
1119  ret.push_back(Position(x, y));
1120  }
1121  //if (s != sEnd && ret.size() > 2) {
1122  // ret.pop_back();
1123  //}
1124  assert(ret.size() >= 2);
1125  assert(ret[0] != ret[1]);
1126  // shift start to coordinate origin
1127  PositionVector ret1 = ret;
1128  ret.add(ret.front() * -1);
1129  // rotate
1130  PositionVector ret2 = ret;
1131  ret.rotate2D(g.hdg - tStart);
1132 #ifdef DEBUG_SPIRAL
1133  std::cout
1134  << std::setprecision(4)
1135  << "edge=" << e.id << " s=" << g.s
1136  << " cStart=" << curveStart
1137  << " cEnd=" << curveEnd
1138  << " cDot=" << cDot
1139  << " sStart=" << sStart
1140  << " sEnd=" << sEnd
1141  << " g.hdg=" << GeomHelper::naviDegree(g.hdg)
1142  << " tStart=" << GeomHelper::naviDegree(tStart)
1143  << "\n beforeShift=" << ret1
1144  << "\n beforeRot=" << ret2
1145  << "\n";
1146 #endif
1147  // shift to geometry start
1148  ret.add(g.x, g.y, 0);
1149  } catch (const std::runtime_error& error) {
1150  WRITE_WARNING("Could not compute spiral geometry for edge '" + e.id + "' (" + error.what() + ").");
1151  ret.push_back(Position(g.x, g.y));
1152  }
1153  return ret.getSubpart2D(0, g.length);
1154 }
1155 
1156 
1159  UNUSED_PARAMETER(e);
1160  PositionVector ret;
1161  double dist = 0.0;
1162  double centerX = g.x;
1163  double centerY = g.y;
1164  // left: positive value
1165  double curvature = g.params[0];
1166  double radius = 1. / curvature;
1167  // center point
1168  calculateCurveCenter(&centerX, &centerY, radius, g.hdg);
1169  double endX = g.x;
1170  double endY = g.y;
1171  double startX = g.x;
1172  double startY = g.y;
1173  double geo_posS = g.s;
1174  double geo_posE = g.s;
1175  bool end = false;
1176  do {
1177  geo_posE += resolution;
1178  if (geo_posE - g.s > g.length) {
1179  geo_posE = g.s + g.length;
1180  }
1181  if (geo_posE - g.s > g.length) {
1182  geo_posE = g.s + g.length;
1183  }
1184  calcPointOnCurve(&endX, &endY, centerX, centerY, radius, geo_posE - geo_posS);
1185 
1186  dist += (geo_posE - geo_posS);
1187  //
1188  ret.push_back(Position(startX, startY));
1189  //
1190  startX = endX;
1191  startY = endY;
1192  geo_posS = geo_posE;
1193 
1194  if (geo_posE - (g.s + g.length) < 0.001 && geo_posE - (g.s + g.length) > -0.001) {
1195  end = true;
1196  }
1197  } while (!end);
1198  return ret.getSubpart2D(0, g.length);
1199 }
1200 
1201 
1204  UNUSED_PARAMETER(e);
1205  const double s = sin(g.hdg);
1206  const double c = cos(g.hdg);
1207  PositionVector ret;
1208  for (double off = 0; off < g.length + 2.; off += resolution) {
1209  double x = off;
1210  double y = g.params[0] + g.params[1] * off + g.params[2] * pow(off, 2.) + g.params[3] * pow(off, 3.);
1211  double xnew = x * c - y * s;
1212  double ynew = x * s + y * c;
1213  ret.push_back(Position(g.x + xnew, g.y + ynew));
1214  }
1215  return ret.getSubpart2D(0, g.length);
1216 }
1217 
1218 
1221  UNUSED_PARAMETER(e);
1222  const double s = sin(g.hdg);
1223  const double c = cos(g.hdg);
1224  const double pMax = g.params[8] <= 0 ? g.length : g.params[8];
1225  const double pStep = pMax / ceil(g.length / resolution);
1226  PositionVector ret;
1227  for (double p = 0; p <= pMax + pStep; p += pStep) {
1228  double x = g.params[0] + g.params[1] * p + g.params[2] * pow(p, 2.) + g.params[3] * pow(p, 3.);
1229  double y = g.params[4] + g.params[5] * p + g.params[6] * pow(p, 2.) + g.params[7] * pow(p, 3.);
1230  double xnew = x * c - y * s;
1231  double ynew = x * s + y * c;
1232  ret.push_back(Position(g.x + xnew, g.y + ynew));
1233  }
1234  return ret.getSubpart2D(0, g.length);
1235 }
1236 
1237 
1238 Position
1239 NIImporter_OpenDrive::calculateStraightEndPoint(double hdg, double length, const Position& start) {
1240  double normx = 1.0f;
1241  double normy = 0.0f;
1242  double x2 = normx * cos(hdg) - normy * sin(hdg);
1243  double y2 = normx * sin(hdg) + normy * cos(hdg);
1244  normx = x2 * length;
1245  normy = y2 * length;
1246  return Position(start.x() + normx, start.y() + normy);
1247 }
1248 
1249 
1250 void
1251 NIImporter_OpenDrive::calculateCurveCenter(double* ad_x, double* ad_y, double ad_radius, double ad_hdg) {
1252  double normX = 1.0;
1253  double normY = 0.0;
1254  double tmpX;
1255  double turn;
1256  if (ad_radius > 0) {
1257  turn = -1.0;
1258  } else {
1259  turn = 1.0;
1260  }
1261 
1262  tmpX = normX;
1263  normX = normX * cos(ad_hdg) + normY * sin(ad_hdg);
1264  normY = tmpX * sin(ad_hdg) + normY * cos(ad_hdg);
1265 
1266  tmpX = normX;
1267  normX = turn * normY;
1268  normY = -turn * tmpX;
1269 
1270  normX = fabs(ad_radius) * normX;
1271  normY = fabs(ad_radius) * normY;
1272 
1273  *ad_x += normX;
1274  *ad_y += normY;
1275 }
1276 
1277 
1278 void
1279 NIImporter_OpenDrive::calcPointOnCurve(double* ad_x, double* ad_y, double ad_centerX, double ad_centerY,
1280  double ad_r, double ad_length) {
1281  double rotAngle = ad_length / fabs(ad_r);
1282  double vx = *ad_x - ad_centerX;
1283  double vy = *ad_y - ad_centerY;
1284  double tmpx;
1285 
1286  double turn;
1287  if (ad_r > 0) {
1288  turn = -1; //left
1289  } else {
1290  turn = 1; //right
1291  }
1292  tmpx = vx;
1293  vx = vx * cos(rotAngle) + turn * vy * sin(rotAngle);
1294  vy = -1 * turn * tmpx * sin(rotAngle) + vy * cos(rotAngle);
1295  *ad_x = vx + ad_centerX;
1296  *ad_y = vy + ad_centerY;
1297 }
1298 
1299 
1300 // ---------------------------------------------------------------------------
1301 // section
1302 // ---------------------------------------------------------------------------
1304  lanesByDir[OPENDRIVE_TAG_LEFT] = std::vector<OpenDriveLane>();
1305  lanesByDir[OPENDRIVE_TAG_RIGHT] = std::vector<OpenDriveLane>();
1306  lanesByDir[OPENDRIVE_TAG_CENTER] = std::vector<OpenDriveLane>();
1307 }
1308 
1309 
1310 void
1312  int sumoLane = 0;
1313  bool singleType = true;
1314  std::vector<std::string> types;
1315  const std::vector<OpenDriveLane>& dirLanesR = lanesByDir.find(OPENDRIVE_TAG_RIGHT)->second;
1316  for (std::vector<OpenDriveLane>::const_reverse_iterator i = dirLanesR.rbegin(); i != dirLanesR.rend(); ++i) {
1317  if (myImportAllTypes || (tc.knows((*i).type) && !tc.getShallBeDiscarded((*i).type))) {
1318  laneMap[(*i).id] = sumoLane++;
1319  types.push_back((*i).type);
1320  if (types.front() != types.back()) {
1321  singleType = false;
1322  }
1323  }
1324  }
1325  rightLaneNumber = sumoLane;
1326  rightType = sumoLane > 0 ? (singleType ? types.front() : joinToString(types, "|")) : "";
1327  sumoLane = 0;
1328  singleType = true;
1329  types.clear();
1330  const std::vector<OpenDriveLane>& dirLanesL = lanesByDir.find(OPENDRIVE_TAG_LEFT)->second;
1331  for (std::vector<OpenDriveLane>::const_iterator i = dirLanesL.begin(); i != dirLanesL.end(); ++i) {
1332  if (myImportAllTypes || (tc.knows((*i).type) && !tc.getShallBeDiscarded((*i).type))) {
1333  laneMap[(*i).id] = sumoLane++;
1334  types.push_back((*i).type);
1335  if (types.front() != types.back()) {
1336  singleType = false;
1337  }
1338  }
1339  }
1340  leftLaneNumber = sumoLane;
1341  leftType = sumoLane > 0 ? (singleType ? types.front() : joinToString(types, "|")) : "";
1342 }
1343 
1344 
1345 std::map<int, int>
1347  std::map<int, int> ret;
1348  const std::vector<OpenDriveLane>& dirLanes = lanesByDir.find(dir)->second;
1349  for (std::vector<OpenDriveLane>::const_reverse_iterator i = dirLanes.rbegin(); i != dirLanes.rend(); ++i) {
1350  std::map<int, int>::const_iterator toP = laneMap.find((*i).id);
1351  if (toP == laneMap.end()) {
1352  // the current lane is not available in SUMO
1353  continue;
1354  }
1355  int to = (*toP).second;
1356  int from = UNSET_CONNECTION;
1357  if ((*i).predecessor != UNSET_CONNECTION) {
1358  from = (*i).predecessor;
1359  }
1360  if (from != UNSET_CONNECTION) {
1361  std::map<int, int>::const_iterator fromP = prev.laneMap.find(from);
1362  if (fromP != prev.laneMap.end()) {
1363  from = (*fromP).second;
1364  } else {
1365  from = UNSET_CONNECTION;
1366  }
1367  }
1368  if (from != UNSET_CONNECTION && to != UNSET_CONNECTION) {
1369  if (ret.find(from) != ret.end()) {
1370 // WRITE_WARNING("double connection");
1371  }
1372  if (dir == OPENDRIVE_TAG_LEFT) {
1373  std::swap(from, to);
1374  }
1375  ret[from] = to;
1376  } else {
1377 // WRITE_WARNING("missing connection");
1378  }
1379  }
1380  return ret;
1381 }
1382 
1383 
1386  OpenDriveLaneSection ret(*this);
1387  ret.s += startPos;
1388  for (int k = 0; k < (int)ret.lanesByDir[OPENDRIVE_TAG_RIGHT].size(); ++k) {
1390  l.speed = 0;
1391  std::vector<std::pair<double, double> >::const_iterator i = std::find_if(l.speeds.begin(), l.speeds.end(), same_position_finder(startPos));
1392  if (i != l.speeds.end()) {
1393  l.speed = (*i).second;
1394  }
1395  }
1396  for (int k = 0; k < (int)ret.lanesByDir[OPENDRIVE_TAG_LEFT].size(); ++k) {
1398  std::vector<std::pair<double, double> >::const_iterator i = std::find_if(l.speeds.begin(), l.speeds.end(), same_position_finder(startPos));
1399  l.speed = 0;
1400  if (i != l.speeds.end()) {
1401  l.speed = (*i).second;
1402  }
1403  }
1404  return ret;
1405 }
1406 
1407 
1408 bool
1409 NIImporter_OpenDrive::OpenDriveLaneSection::buildSpeedChanges(const NBTypeCont& tc, std::vector<OpenDriveLaneSection>& newSections) {
1410  std::set<double> speedChangePositions;
1411  // collect speed change positions and apply initial speed to the begin
1412  for (std::vector<OpenDriveLane>::iterator k = lanesByDir[OPENDRIVE_TAG_RIGHT].begin(); k != lanesByDir[OPENDRIVE_TAG_RIGHT].end(); ++k) {
1413  for (std::vector<std::pair<double, double> >::const_iterator l = (*k).speeds.begin(); l != (*k).speeds.end(); ++l) {
1414  speedChangePositions.insert((*l).first);
1415  if ((*l).first == 0) {
1416  (*k).speed = (*l).second;
1417  }
1418  }
1419  }
1420  for (std::vector<OpenDriveLane>::iterator k = lanesByDir[OPENDRIVE_TAG_LEFT].begin(); k != lanesByDir[OPENDRIVE_TAG_LEFT].end(); ++k) {
1421  for (std::vector<std::pair<double, double> >::const_iterator l = (*k).speeds.begin(); l != (*k).speeds.end(); ++l) {
1422  speedChangePositions.insert((*l).first);
1423  if ((*l).first == 0) {
1424  (*k).speed = (*l).second;
1425  }
1426  }
1427  }
1428  // do nothing if there is none
1429  if (speedChangePositions.size() == 0) {
1430  return false;
1431  }
1432  if (*speedChangePositions.begin() > 0) {
1433  speedChangePositions.insert(0);
1434  }
1435 #ifdef DEBUG_VARIABLE_SPEED
1436  if (gDebugFlag1) std::cout
1437  << " buildSpeedChanges sectionStart=" << s
1438  << " speedChangePositions=" << joinToString(speedChangePositions, ", ")
1439  << "\n";
1440 #endif
1441  for (std::set<double>::iterator i = speedChangePositions.begin(); i != speedChangePositions.end(); ++i) {
1442  if (i == speedChangePositions.begin()) {
1443  newSections.push_back(*this);
1444  } else {
1445  newSections.push_back(buildLaneSection(*i));
1446  }
1447  }
1448  // propagate speeds
1449  for (int i = 0; i != (int)newSections.size(); ++i) {
1450  OpenDriveLaneSection& ls = newSections[i];
1451  std::map<OpenDriveXMLTag, std::vector<OpenDriveLane> >& lanesByDir = ls.lanesByDir;
1452  for (std::map<OpenDriveXMLTag, std::vector<OpenDriveLane> >::iterator k = lanesByDir.begin(); k != lanesByDir.end(); ++k) {
1453  std::vector<OpenDriveLane>& lanes = (*k).second;
1454  for (int j = 0; j != (int)lanes.size(); ++j) {
1455  OpenDriveLane& l = lanes[j];
1456  if (l.speed != 0) {
1457  continue;
1458  }
1459  if (i > 0) {
1460  l.speed = newSections[i - 1].lanesByDir[(*k).first][j].speed;
1461  } else {
1462  tc.getSpeed(l.type);
1463  }
1464  }
1465  }
1466  }
1467  return true;
1468 }
1469 
1470 
1471 
1472 // ---------------------------------------------------------------------------
1473 // edge
1474 // ---------------------------------------------------------------------------
1475 int
1477  // for signal interpretations see https://de.wikipedia.org/wiki/Bildtafel_der_Verkehrszeichen_in_der_Bundesrepublik_Deutschland_seit_2013
1478  int prio = 1;
1479  for (std::vector<OpenDriveSignal>::const_iterator i = signals.begin(); i != signals.end(); ++i) {
1480  int tmp = 1;
1481  if ((*i).type == "301" || (*i).type == "306") { // priority road or local priority
1482  tmp = 2;
1483  }
1484  if ((*i).type == "205" /*|| (*i).type == "206"*/) { // yield or stop
1485  tmp = 0;
1486  }
1487  if (tmp != 1 && dir == OPENDRIVE_TAG_RIGHT && (*i).orientation > 0) {
1488  prio = tmp;
1489  }
1490  if (tmp != 1 && dir == OPENDRIVE_TAG_LEFT && (*i).orientation < 0) {
1491  prio = tmp;
1492  }
1493 
1494  }
1495  return prio;
1496 }
1497 
1498 
1499 
1500 // ---------------------------------------------------------------------------
1501 // loader methods
1502 // ---------------------------------------------------------------------------
1503 NIImporter_OpenDrive::NIImporter_OpenDrive(const NBTypeCont& tc, std::map<std::string, OpenDriveEdge*>& edges)
1505  myTypeContainer(tc), myCurrentEdge("", "", "", -1), myEdges(edges) {
1506 }
1507 
1508 
1510 }
1511 
1512 
1513 void
1515  const SUMOSAXAttributes& attrs) {
1516  bool ok = true;
1517  switch (element) {
1518  case OPENDRIVE_TAG_HEADER: {
1519  int majorVersion = attrs.get<int>(OPENDRIVE_ATTR_REVMAJOR, 0, ok);
1520  int minorVersion = attrs.get<int>(OPENDRIVE_ATTR_REVMINOR, 0, ok);
1521  if (majorVersion != 1 || minorVersion != 2) {
1522  // TODO: leave note of exceptions
1523  WRITE_WARNING("Given openDrive file '" + getFileName() + "' uses version " + toString(majorVersion) + "." + toString(minorVersion) + ";\n Version 1.2 is supported.");
1524  }
1525  }
1526  break;
1527  case OPENDRIVE_TAG_ROAD: {
1528  std::string id = attrs.get<std::string>(OPENDRIVE_ATTR_ID, 0, ok);
1529  std::string streetName = attrs.getOpt<std::string>(OPENDRIVE_ATTR_NAME, 0, ok, "", false);
1530  std::string junction = attrs.get<std::string>(OPENDRIVE_ATTR_JUNCTION, id.c_str(), ok);
1531  double length = attrs.get<double>(OPENDRIVE_ATTR_LENGTH, id.c_str(), ok);
1532  myCurrentEdge = OpenDriveEdge(id, streetName, junction, length);
1533  }
1534  break;
1536  if (myElementStack.size() >= 2 && myElementStack[myElementStack.size() - 2] == OPENDRIVE_TAG_ROAD) {
1537  std::string elementType = attrs.get<std::string>(OPENDRIVE_ATTR_ELEMENTTYPE, myCurrentEdge.id.c_str(), ok);
1538  std::string elementID = attrs.get<std::string>(OPENDRIVE_ATTR_ELEMENTID, myCurrentEdge.id.c_str(), ok);
1539  std::string contactPoint = attrs.hasAttribute(OPENDRIVE_ATTR_CONTACTPOINT)
1540  ? attrs.get<std::string>(OPENDRIVE_ATTR_CONTACTPOINT, myCurrentEdge.id.c_str(), ok)
1541  : "end";
1542  addLink(OPENDRIVE_LT_PREDECESSOR, elementType, elementID, contactPoint);
1543  }
1544  if (myElementStack.size() >= 2 && myElementStack[myElementStack.size() - 2] == OPENDRIVE_TAG_LANE) {
1545  int no = attrs.get<int>(OPENDRIVE_ATTR_ID, myCurrentEdge.id.c_str(), ok);
1546  OpenDriveLane& l = myCurrentEdge.laneSections.back().lanesByDir[myCurrentLaneDirection].back();
1547  l.predecessor = no;
1548  }
1549  }
1550  break;
1551  case OPENDRIVE_TAG_SUCCESSOR: {
1552  if (myElementStack.size() >= 2 && myElementStack[myElementStack.size() - 2] == OPENDRIVE_TAG_ROAD) {
1553  std::string elementType = attrs.get<std::string>(OPENDRIVE_ATTR_ELEMENTTYPE, myCurrentEdge.id.c_str(), ok);
1554  std::string elementID = attrs.get<std::string>(OPENDRIVE_ATTR_ELEMENTID, myCurrentEdge.id.c_str(), ok);
1555  std::string contactPoint = attrs.hasAttribute(OPENDRIVE_ATTR_CONTACTPOINT)
1556  ? attrs.get<std::string>(OPENDRIVE_ATTR_CONTACTPOINT, myCurrentEdge.id.c_str(), ok)
1557  : "start";
1558  addLink(OPENDRIVE_LT_SUCCESSOR, elementType, elementID, contactPoint);
1559  }
1560  if (myElementStack.size() >= 2 && myElementStack[myElementStack.size() - 2] == OPENDRIVE_TAG_LANE) {
1561  int no = attrs.get<int>(OPENDRIVE_ATTR_ID, myCurrentEdge.id.c_str(), ok);
1562  OpenDriveLane& l = myCurrentEdge.laneSections.back().lanesByDir[myCurrentLaneDirection].back();
1563  l.successor = no;
1564  }
1565  }
1566  break;
1567  case OPENDRIVE_TAG_GEOMETRY: {
1568  double length = attrs.get<double>(OPENDRIVE_ATTR_LENGTH, myCurrentEdge.id.c_str(), ok);
1569  double s = attrs.get<double>(OPENDRIVE_ATTR_S, myCurrentEdge.id.c_str(), ok);
1570  double x = attrs.get<double>(OPENDRIVE_ATTR_X, myCurrentEdge.id.c_str(), ok);
1571  double y = attrs.get<double>(OPENDRIVE_ATTR_Y, myCurrentEdge.id.c_str(), ok);
1572  double hdg = attrs.get<double>(OPENDRIVE_ATTR_HDG, myCurrentEdge.id.c_str(), ok);
1573  myCurrentEdge.geometries.push_back(OpenDriveGeometry(length, s, x, y, hdg));
1574  }
1575  break;
1576  case OPENDRIVE_TAG_ELEVATION: {
1577  double s = attrs.get<double>(OPENDRIVE_ATTR_S, myCurrentEdge.id.c_str(), ok);
1578  double a = attrs.get<double>(OPENDRIVE_ATTR_A, myCurrentEdge.id.c_str(), ok);
1579  double b = attrs.get<double>(OPENDRIVE_ATTR_B, myCurrentEdge.id.c_str(), ok);
1580  double c = attrs.get<double>(OPENDRIVE_ATTR_C, myCurrentEdge.id.c_str(), ok);
1581  double d = attrs.get<double>(OPENDRIVE_ATTR_D, myCurrentEdge.id.c_str(), ok);
1582  myCurrentEdge.elevations.push_back(OpenDriveElevation(s, a, b, c, d));
1583  }
1584  break;
1585  case OPENDRIVE_TAG_LINE: {
1586  if (myElementStack.size() > 0 && myElementStack.back() == OPENDRIVE_TAG_GEOMETRY) {
1587  std::vector<double> vals;
1589  }
1590  }
1591  break;
1592  case OPENDRIVE_TAG_SPIRAL: {
1593  std::vector<double> vals;
1594  vals.push_back(attrs.get<double>(OPENDRIVE_ATTR_CURVSTART, myCurrentEdge.id.c_str(), ok));
1595  vals.push_back(attrs.get<double>(OPENDRIVE_ATTR_CURVEND, myCurrentEdge.id.c_str(), ok));
1597  }
1598  break;
1599  case OPENDRIVE_TAG_ARC: {
1600  std::vector<double> vals;
1601  vals.push_back(attrs.get<double>(OPENDRIVE_ATTR_CURVATURE, myCurrentEdge.id.c_str(), ok));
1603  }
1604  break;
1605  case OPENDRIVE_TAG_POLY3: {
1606  std::vector<double> vals;
1607  vals.push_back(attrs.get<double>(OPENDRIVE_ATTR_A, myCurrentEdge.id.c_str(), ok));
1608  vals.push_back(attrs.get<double>(OPENDRIVE_ATTR_B, myCurrentEdge.id.c_str(), ok));
1609  vals.push_back(attrs.get<double>(OPENDRIVE_ATTR_C, myCurrentEdge.id.c_str(), ok));
1610  vals.push_back(attrs.get<double>(OPENDRIVE_ATTR_D, myCurrentEdge.id.c_str(), ok));
1612  }
1613  break;
1614  case OPENDRIVE_TAG_PARAMPOLY3: {
1615  std::vector<double> vals;
1616  vals.push_back(attrs.get<double>(OPENDRIVE_ATTR_AU, myCurrentEdge.id.c_str(), ok));
1617  vals.push_back(attrs.get<double>(OPENDRIVE_ATTR_BU, myCurrentEdge.id.c_str(), ok));
1618  vals.push_back(attrs.get<double>(OPENDRIVE_ATTR_CU, myCurrentEdge.id.c_str(), ok));
1619  vals.push_back(attrs.get<double>(OPENDRIVE_ATTR_DU, myCurrentEdge.id.c_str(), ok));
1620  vals.push_back(attrs.get<double>(OPENDRIVE_ATTR_AV, myCurrentEdge.id.c_str(), ok));
1621  vals.push_back(attrs.get<double>(OPENDRIVE_ATTR_BV, myCurrentEdge.id.c_str(), ok));
1622  vals.push_back(attrs.get<double>(OPENDRIVE_ATTR_CV, myCurrentEdge.id.c_str(), ok));
1623  vals.push_back(attrs.get<double>(OPENDRIVE_ATTR_DV, myCurrentEdge.id.c_str(), ok));
1624  const std::string pRange = attrs.getOpt<std::string>(OPENDRIVE_ATTR_PRANGE, myCurrentEdge.id.c_str(), ok, "normalized", false);
1625  if (pRange == "normalized") {
1626  vals.push_back(1.0);
1627  } else if (pRange == "arcLength") {
1628  vals.push_back(-1.0);
1629  } else {
1630  WRITE_WARNING("Ignoring invalid pRange value '" + pRange + "' for road '" + myCurrentEdge.id + "'.");
1631  vals.push_back(1.0);
1632  }
1634  }
1635  break;
1637  double s = attrs.get<double>(OPENDRIVE_ATTR_S, myCurrentEdge.id.c_str(), ok);
1639  }
1640  break;
1641  case OPENDRIVE_TAG_LANEOFFSET: {
1642  double s = attrs.get<double>(OPENDRIVE_ATTR_S, myCurrentEdge.id.c_str(), ok);
1643  double a = attrs.get<double>(OPENDRIVE_ATTR_A, myCurrentEdge.id.c_str(), ok);
1644  double b = attrs.get<double>(OPENDRIVE_ATTR_B, myCurrentEdge.id.c_str(), ok);
1645  double c = attrs.get<double>(OPENDRIVE_ATTR_C, myCurrentEdge.id.c_str(), ok);
1646  double d = attrs.get<double>(OPENDRIVE_ATTR_D, myCurrentEdge.id.c_str(), ok);
1647  myCurrentEdge.offsets.push_back(OpenDriveLaneOffset(s, a, b, c, d));
1648  }
1649  break;
1650  case OPENDRIVE_TAG_LEFT:
1652  break;
1653  case OPENDRIVE_TAG_CENTER:
1655  break;
1656  case OPENDRIVE_TAG_RIGHT:
1658  break;
1659  case OPENDRIVE_TAG_LANE: {
1660  std::string type = attrs.get<std::string>(OPENDRIVE_ATTR_TYPE, myCurrentEdge.id.c_str(), ok);
1661  int id = attrs.get<int>(OPENDRIVE_ATTR_ID, myCurrentEdge.id.c_str(), ok);
1662  std::string level = attrs.hasAttribute(OPENDRIVE_ATTR_LEVEL)
1663  ? attrs.get<std::string>(OPENDRIVE_ATTR_LEVEL, myCurrentEdge.id.c_str(), ok)
1664  : "";
1666  ls.lanesByDir[myCurrentLaneDirection].push_back(OpenDriveLane(id, level, type));
1667  }
1668  break;
1669  case OPENDRIVE_TAG_SIGNAL: {
1670  int id = attrs.get<int>(OPENDRIVE_ATTR_ID, myCurrentEdge.id.c_str(), ok);
1671  std::string type = attrs.get<std::string>(OPENDRIVE_ATTR_TYPE, myCurrentEdge.id.c_str(), ok);
1672  std::string name = attrs.getOpt<std::string>(OPENDRIVE_ATTR_NAME, myCurrentEdge.id.c_str(), ok, "", false);
1673  int orientation = attrs.get<std::string>(OPENDRIVE_ATTR_ORIENTATION, myCurrentEdge.id.c_str(), ok) == "-" ? -1 : 1;
1674  double s = attrs.get<double>(OPENDRIVE_ATTR_S, myCurrentEdge.id.c_str(), ok);
1675  bool dynamic = attrs.get<std::string>(OPENDRIVE_ATTR_DYNAMIC, myCurrentEdge.id.c_str(), ok) == "no" ? false : true;
1676  myCurrentEdge.signals.push_back(OpenDriveSignal(id, type, name, orientation, dynamic, s));
1677  }
1678  break;
1680  myCurrentJunctionID = attrs.get<std::string>(OPENDRIVE_ATTR_ID, myCurrentJunctionID.c_str(), ok);
1681  break;
1682  case OPENDRIVE_TAG_CONNECTION: {
1683  std::string id = attrs.get<std::string>(OPENDRIVE_ATTR_ID, myCurrentJunctionID.c_str(), ok);
1684  myCurrentIncomingRoad = attrs.get<std::string>(OPENDRIVE_ATTR_INCOMINGROAD, myCurrentJunctionID.c_str(), ok);
1686  std::string cp = attrs.get<std::string>(OPENDRIVE_ATTR_CONTACTPOINT, myCurrentJunctionID.c_str(), ok);
1688  myConnectionWasEmpty = true;
1689  }
1690  break;
1691  case OPENDRIVE_TAG_LANELINK: {
1692  int from = attrs.get<int>(OPENDRIVE_ATTR_FROM, myCurrentJunctionID.c_str(), ok);
1693  int to = attrs.get<int>(OPENDRIVE_ATTR_TO, myCurrentJunctionID.c_str(), ok);
1694  Connection c;
1696  c.toEdge = myCurrentConnectingRoad;
1697  c.fromLane = from;
1698  c.toLane = to;
1699  c.fromCP = OPENDRIVE_CP_END;
1700  c.toCP = myCurrentContactPoint;
1701  c.all = false;
1702  if (myEdges.find(c.fromEdge) == myEdges.end()) {
1703  WRITE_ERROR("In laneLink-element: incoming road '" + c.fromEdge + "' is not known.");
1704  } else {
1705  OpenDriveEdge* e = myEdges.find(c.fromEdge)->second;
1706  e->connections.insert(c);
1707  myConnectionWasEmpty = false;
1708  }
1709  }
1710  break;
1711  case OPENDRIVE_TAG_WIDTH: {
1712  if (myElementStack.size() >= 2 && myElementStack[myElementStack.size() - 1] == OPENDRIVE_TAG_LANE) {
1713  const double s = attrs.get<double>(OPENDRIVE_ATTR_SOFFSET, myCurrentEdge.id.c_str(), ok);
1714  const double a = attrs.get<double>(OPENDRIVE_ATTR_A, myCurrentEdge.id.c_str(), ok);
1715  const double b = attrs.get<double>(OPENDRIVE_ATTR_B, myCurrentEdge.id.c_str(), ok);
1716  const double c = attrs.get<double>(OPENDRIVE_ATTR_C, myCurrentEdge.id.c_str(), ok);
1717  const double d = attrs.get<double>(OPENDRIVE_ATTR_D, myCurrentEdge.id.c_str(), ok);
1718  OpenDriveLane& l = myCurrentEdge.laneSections.back().lanesByDir[myCurrentLaneDirection].back();
1719  l.width = MAX2(l.width, a);
1720  l.widthData.push_back(OpenDriveWidth(s, a, b, c, d));
1721 #ifdef DEBUG_VARIABLE_WIDTHS
1722  if (DEBUG_COND(&myCurrentEdge)) {
1723  std::cout << " road=" << myCurrentEdge.id
1724  << std::setprecision(gPrecision)
1725  << " junction=" << myCurrentEdge.junction
1726  << " section=" << myCurrentEdge.laneSections.size() - 1
1727  << " dir=" << myCurrentLaneDirection << " lane=" << l.id
1728  << " type=" << l.type
1729  << " width=" << l.width
1730  << " a=" << a
1731  << " b=" << b
1732  << " c=" << c
1733  << " d=" << d
1734  << " s=" << s
1735  << " entries=" << l.widthData.size()
1736  << "\n";
1737  }
1738 #endif
1739  }
1740  }
1741  break;
1742  case OPENDRIVE_TAG_SPEED: {
1743  if (myElementStack.size() >= 2 && myElementStack[myElementStack.size() - 1] == OPENDRIVE_TAG_LANE) {
1744  double speed = attrs.get<double>(OPENDRIVE_ATTR_MAX, myCurrentEdge.id.c_str(), ok);
1745  double pos = attrs.get<double>(OPENDRIVE_ATTR_SOFFSET, myCurrentEdge.id.c_str(), ok);
1746  // required for xodr v1.4
1747  const std::string unit = attrs.getOpt<std::string>(OPENDRIVE_ATTR_UNIT, myCurrentEdge.id.c_str(), ok, "", false);
1748  // now convert the speed to reasonable default SI [m/s]
1749  if (!unit.empty()) {
1750  // something to be done at all ?
1751  if (unit == "km/h") {
1752  speed /= 3.6;
1753  }
1754  if (unit == "mph") {
1755  speed *= 1.609344 / 3.6;
1756  }
1757  // IGNORING unknown units.
1758  }
1759  myCurrentEdge.laneSections.back().lanesByDir[myCurrentLaneDirection].back().speeds.push_back(std::make_pair(pos, speed));
1760  }
1761  }
1762  break;
1763  default:
1764  break;
1765  }
1766  myElementStack.push_back(element);
1767 }
1768 
1769 
1770 void
1772  myElementStack.pop_back();
1773  switch (element) {
1774  case OPENDRIVE_TAG_ROAD:
1776  break;
1778  if (myConnectionWasEmpty) {
1779  Connection c;
1782  c.fromLane = 0;
1783  c.toLane = 0;
1786  c.all = true;
1787  if (myEdges.find(c.fromEdge) == myEdges.end()) {
1788  WRITE_ERROR("In laneLink-element: incoming road '" + c.fromEdge + "' is not known.");
1789  } else {
1790  OpenDriveEdge* e = myEdges.find(c.fromEdge)->second;
1791  e->connections.insert(c);
1792  }
1793  }
1794  break;
1796  myCurrentEdge.laneSections.back().buildLaneMapping(myTypeContainer);
1797  }
1798  break;
1799  default:
1800  break;
1801  }
1802 }
1803 
1804 
1805 
1806 void
1807 NIImporter_OpenDrive::addLink(LinkType lt, const std::string& elementType,
1808  const std::string& elementID,
1809  const std::string& contactPoint) {
1810  OpenDriveLink l(lt, elementID);
1811  // elementType
1812  if (elementType == "road") {
1814  } else if (elementType == "junction") {
1816  }
1817  // contact point
1818  if (contactPoint == "start") {
1820  } else if (contactPoint == "end") {
1822  }
1823  // add
1824  myCurrentEdge.links.push_back(l);
1825 }
1826 
1827 
1828 void
1829 NIImporter_OpenDrive::addGeometryShape(GeometryType type, const std::vector<double>& vals) {
1830  // checks
1831  if (myCurrentEdge.geometries.size() == 0) {
1832  throw ProcessError("Mismatching paranthesis in geometry definition for road '" + myCurrentEdge.id + "'");
1833  }
1835  if (last.type != OPENDRIVE_GT_UNKNOWN) {
1836  throw ProcessError("Double geometry information for road '" + myCurrentEdge.id + "'");
1837  }
1838  // set
1839  last.type = type;
1840  last.params = vals;
1841 }
1842 
1843 
1844 bool
1846  if (c1.fromEdge != c2.fromEdge) {
1847  return c1.fromEdge < c2.fromEdge;
1848  }
1849  if (c1.toEdge != c2.toEdge) {
1850  return c1.toEdge < c2.toEdge;
1851  }
1852  if (c1.fromLane != c2.fromLane) {
1853  return c1.fromLane < c2.fromLane;
1854  }
1855  return c1.toLane < c2.toLane;
1856 }
1857 
1858 
1859 void
1861  std::vector<OpenDriveLaneSection> newSections;
1862 #ifdef DEBUG_VARIABLE_WIDTHS
1863  if (DEBUG_COND(e)) {
1864  gDebugFlag1 = true;
1865  std::cout << "splitMinWidths e=" << e->id << " sections=" << e->laneSections.size() << "\n";
1866  }
1867 #endif
1868  for (std::vector<OpenDriveLaneSection>::iterator j = e->laneSections.begin(); j != e->laneSections.end(); ++j) {
1869  OpenDriveLaneSection& sec = *j;
1870  std::vector<double> splitPositions;
1871  const double sectionEnd = (j + 1) == e->laneSections.end() ? e->length : (*(j + 1)).s;
1872  const int section = (int)(j - e->laneSections.begin());
1873 #ifdef DEBUG_VARIABLE_WIDTHS
1874  if (DEBUG_COND(e)) {
1875  std::cout << " findWidthSplit section=" << section << " sectionStart=" << sec.s << " sectionOrigStart=" << sec.sOrig << " sectionEnd=" << sectionEnd << "\n";
1876  }
1877 #endif
1878  if (sec.rightLaneNumber > 0) {
1879  findWidthSplit(tc, sec.lanesByDir[OPENDRIVE_TAG_RIGHT], section, sec.sOrig, sectionEnd, splitPositions);
1880  }
1881  if (sec.leftLaneNumber > 0) {
1882  findWidthSplit(tc, sec.lanesByDir[OPENDRIVE_TAG_LEFT], section, sec.sOrig, sectionEnd, splitPositions);
1883  }
1884  newSections.push_back(sec);
1885  std::sort(splitPositions.begin(), splitPositions.end());
1886  // filter out tiny splits
1887  double prevSplit = sec.s;
1888  for (std::vector<double>::iterator it = splitPositions.begin(); it != splitPositions.end();) {
1889  if ((*it) - prevSplit < minDist || sectionEnd - (*it) < minDist) {
1890  // avoid tiny (or duplicate) splits
1891 #ifdef DEBUG_VARIABLE_WIDTHS
1892  if (DEBUG_COND(e)) {
1893  std::cout << " skip close split=" << (*it) << " prevSplit=" << prevSplit << "\n";
1894  }
1895 #endif
1896  it = splitPositions.erase(it);
1897  } else if ((*it) < sec.s) {
1898  // avoid splits for another section
1899 #ifdef DEBUG_VARIABLE_WIDTHS
1900  if (DEBUG_COND(e)) {
1901  std::cout << " skip early split=" << (*it) << " s=" << sec.s << "\n";
1902  }
1903 #endif
1904  it = splitPositions.erase(it);
1905  } else {
1906  prevSplit = *it;
1907  it++;
1908  }
1909  }
1910 
1911  if (splitPositions.size() > 0) {
1912 #ifdef DEBUG_VARIABLE_WIDTHS
1913  if (DEBUG_COND(e)) {
1914  std::cout << " road=" << e->id << " splitMinWidths section=" << section
1915  << " start=" << sec.s
1916  << " origStart=" << sec.sOrig
1917  << " end=" << sectionEnd << " minDist=" << minDist
1918  << " splitPositions=" << toString(splitPositions) << "\n";
1919  }
1920 #endif
1921 #ifdef DEBUG_VARIABLE_WIDTHS
1922  if (DEBUG_COND(e)) {
1923  std::cout << "first section...\n";
1924  }
1925 #endif
1926  recomputeWidths(newSections.back(), sec.sOrig, splitPositions.front(), sec.sOrig, sectionEnd);
1927  for (std::vector<double>::iterator it = splitPositions.begin(); it != splitPositions.end(); ++it) {
1928  OpenDriveLaneSection secNew = sec;
1929  secNew.s = *it;
1930 #ifdef DEBUG_VARIABLE_WIDTHS
1931  if (DEBUG_COND(e)) {
1932  std::cout << "splitAt " << secNew.s << "\n";
1933  }
1934 #endif
1935  newSections.push_back(secNew);
1936  if (secNew.rightLaneNumber > 0) {
1937  setStraightConnections(newSections.back().lanesByDir[OPENDRIVE_TAG_RIGHT]);
1938  }
1939  if (secNew.leftLaneNumber > 0) {
1940  setStraightConnections(newSections.back().lanesByDir[OPENDRIVE_TAG_LEFT]);
1941  }
1942  double end = (it + 1) == splitPositions.end() ? sectionEnd : *(it + 1);
1943  recomputeWidths(newSections.back(), secNew.s, end, sec.sOrig, sectionEnd);
1944  }
1945  }
1946  }
1947  gDebugFlag1 = false;
1948  e->laneSections = newSections;
1949 }
1950 
1951 
1952 void
1953 NIImporter_OpenDrive::findWidthSplit(const NBTypeCont& tc, std::vector<OpenDriveLane>& lanes,
1954  int section, double sectionStart, double sectionEnd,
1955  std::vector<double>& splitPositions) {
1956  UNUSED_PARAMETER(section);
1957  for (std::vector<OpenDriveLane>::iterator k = lanes.begin(); k != lanes.end(); ++k) {
1958  OpenDriveLane& l = *k;
1959  SVCPermissions permissions = tc.getPermissions(l.type) & ~(SVC_PEDESTRIAN | SVC_BICYCLE);
1960  if (l.widthData.size() > 0 && tc.knows(l.type) && !tc.getShallBeDiscarded(l.type) && permissions != 0) {
1961  double sPrev = l.widthData.front().s;
1962  double wPrev = l.widthData.front().computeAt(sPrev);
1963  if (gDebugFlag1) std::cout
1964  << "findWidthSplit section=" << section
1965  << " sectionStart=" << sectionStart
1966  << " sectionEnd=" << sectionEnd
1967  << " lane=" << l.id
1968  << " type=" << l.type
1969  << " widthEntries=" << l.widthData.size() << "\n"
1970  << " s=" << sPrev
1971  << " w=" << wPrev
1972  << "\n";
1973  for (std::vector<OpenDriveWidth>::iterator it_w = l.widthData.begin(); it_w != l.widthData.end(); ++it_w) {
1974  double sEnd = (it_w + 1) != l.widthData.end() ? (*(it_w + 1)).s : sectionEnd - sectionStart;
1975  double w = (*it_w).computeAt(sEnd);
1976  if (gDebugFlag1) std::cout
1977  << " sEnd=" << sEnd
1978  << " s=" << (*it_w).s
1979  << " a=" << (*it_w).a << " b=" << (*it_w).b << " c=" << (*it_w).c << " d=" << (*it_w).d
1980  << " w=" << w
1981  << "\n";
1982  const double changeDist = fabs(myMinWidth - wPrev);
1983  if (((wPrev < myMinWidth) && (w > myMinWidth))
1984  || ((wPrev > myMinWidth) && (w < myMinWidth))) {
1985  double splitPos = sPrev + (sEnd - sPrev) / fabs(w - wPrev) * changeDist;
1986  double wSplit = (*it_w).computeAt(splitPos);
1987  if (gDebugFlag1) {
1988  std::cout << " candidate splitPos=" << splitPos << " w=" << wSplit << "\n";
1989  }
1990  // ensure that the thin part is actually thin enough
1991  while (wSplit > myMinWidth) {
1992  if (wPrev < myMinWidth) {
1993  // getting wider
1994  splitPos -= POSITION_EPS;
1995  if (splitPos < sPrev) {
1996  if (gDebugFlag1) {
1997  std::cout << " aborting search splitPos=" << splitPos << " wSplit=" << wSplit << " sPrev=" << sPrev << " wPrev=" << wPrev << "\n";
1998  }
1999  splitPos = sPrev;
2000  break;
2001  }
2002  } else {
2003  // getting thinner
2004  splitPos += POSITION_EPS;
2005  if (splitPos > sEnd) {
2006  if (gDebugFlag1) {
2007  std::cout << " aborting search splitPos=" << splitPos << " wSplit=" << wSplit << " sEnd=" << sEnd << " w=" << w << "\n";
2008  }
2009  splitPos = sEnd;
2010  break;
2011  }
2012  }
2013  wSplit = (*it_w).computeAt(splitPos);
2014  if (gDebugFlag1) {
2015  std::cout << " refined splitPos=" << splitPos << " w=" << wSplit << "\n";
2016  }
2017  }
2018  splitPositions.push_back(sectionStart + splitPos);
2019  }
2020  // //wPrev = wSplit;
2021  //} else if ((fabs(wPrev) < NUMERICAL_EPS && w > POSITION_EPS)
2022  // || (wPrev > POSITION_EPS && fabs(w) < NUMERICAL_EPS)) {
2023  // splitPositions.push_back(sectionStart + sPrev);
2024  // if (gDebugFlag1) std::cout << " laneDisappears candidate splitPos=" << sPrev << " wPrev=" << wPrev << " w=" << w<< "\n";
2025  //}
2026  wPrev = w;
2027  sPrev = sEnd;
2028  }
2029  }
2030  }
2031 }
2032 
2033 
2034 void
2035 NIImporter_OpenDrive::setStraightConnections(std::vector<OpenDriveLane>& lanes) {
2036  for (std::vector<OpenDriveLane>::iterator k = lanes.begin(); k != lanes.end(); ++k) {
2037  (*k).predecessor = (*k).id;
2038  }
2039 }
2040 
2041 
2042 void
2043 NIImporter_OpenDrive::recomputeWidths(OpenDriveLaneSection& sec, double start, double end, double sectionStart, double sectionEnd) {
2044  if (sec.rightLaneNumber > 0) {
2045  recomputeWidths(sec.lanesByDir[OPENDRIVE_TAG_RIGHT], start, end, sectionStart, sectionEnd);
2046  }
2047  if (sec.leftLaneNumber > 0) {
2048  recomputeWidths(sec.lanesByDir[OPENDRIVE_TAG_LEFT], start, end, sectionStart, sectionEnd);
2049  }
2050 }
2051 
2052 
2053 void
2054 NIImporter_OpenDrive::recomputeWidths(std::vector<OpenDriveLane>& lanes, double start, double end, double sectionStart, double sectionEnd) {
2055  for (std::vector<OpenDriveLane>::iterator k = lanes.begin(); k != lanes.end(); ++k) {
2056  OpenDriveLane& l = *k;
2057  if (l.widthData.size() > 0) {
2058 #ifdef DEBUG_VARIABLE_WIDTHS
2059  if (gDebugFlag1) std::cout
2060  << "recomputeWidths lane=" << l.id
2061  << " type=" << l.type
2062  << " start=" << start
2063  << " end=" << end
2064  << " sectionStart=" << sectionStart
2065  << " sectionEnd=" << sectionEnd
2066  << " widthEntries=" << l.widthData.size() << "\n"
2067  << "\n";
2068 #endif
2069  l.width = 0;
2070  double sPrev = l.widthData.front().s;
2071  double sPrevAbs = sPrev + sectionStart;
2072  for (std::vector<OpenDriveWidth>::iterator it_w = l.widthData.begin(); it_w != l.widthData.end(); ++it_w) {
2073  double sEnd = (it_w + 1) != l.widthData.end() ? (*(it_w + 1)).s : sectionEnd - sectionStart;
2074  double sEndAbs = sEnd + sectionStart;
2075 #ifdef DEBUG_VARIABLE_WIDTHS
2076  if (gDebugFlag1) std::cout
2077  << " sPrev=" << sPrev << " sPrevAbs=" << sPrevAbs
2078  << " sEnd=" << sEnd << " sEndAbs=" << sEndAbs
2079  << " widthData s=" << (*it_w).s
2080  << " a=" << (*it_w).a
2081  << " b=" << (*it_w).b
2082  << " c=" << (*it_w).c
2083  << " d=" << (*it_w).d
2084  << "\n";
2085 #endif
2086  if (sPrevAbs <= start && sEndAbs >= start) {
2087 #ifdef DEBUG_VARIABLE_WIDTHS
2088  if (gDebugFlag1) {
2089  std::cout << " atStart=" << start << " pos=" << start - sectionStart << " w=" << (*it_w).computeAt(start - sectionStart) << "\n";
2090  }
2091 #endif
2092  l.width = MAX2(l.width, (*it_w).computeAt(start - sectionStart));
2093  }
2094  if (sPrevAbs <= end && sEndAbs >= end) {
2095 #ifdef DEBUG_VARIABLE_WIDTHS
2096  if (gDebugFlag1) {
2097  std::cout << " atEnd=" << end << " pos=" << end - sectionStart << " w=" << (*it_w).computeAt(end - sectionStart) << "\n";
2098  }
2099 #endif
2100  l.width = MAX2(l.width, (*it_w).computeAt(end - sectionStart));
2101  }
2102  if (start <= sPrevAbs && end >= sPrevAbs) {
2103 #ifdef DEBUG_VARIABLE_WIDTHS
2104  if (gDebugFlag1) {
2105  std::cout << " atSPrev=" << sPrev << " w=" << (*it_w).computeAt(sPrev) << "\n";
2106  }
2107 #endif
2108  l.width = MAX2(l.width, (*it_w).computeAt(sPrev));
2109  }
2110  if (start <= sEndAbs && end >= sEndAbs) {
2111 #ifdef DEBUG_VARIABLE_WIDTHS
2112  if (gDebugFlag1) {
2113  std::cout << " atSEnd=" << sEnd << " w=" << (*it_w).computeAt(sEnd) << "\n";
2114  }
2115 #endif
2116  l.width = MAX2(l.width, (*it_w).computeAt(sEnd));
2117  }
2118 #ifdef DEBUG_VARIABLE_WIDTHS
2119  if (gDebugFlag1) {
2120  std::cout << " sPrev=" << sPrev << " sEnd=" << sEnd << " l.width=" << l.width << "\n";
2121  }
2122 #endif
2123  sPrev = sEnd;
2124  sPrevAbs = sEndAbs;
2125  }
2126  }
2127  }
2128 }
2129 
2130 /****************************************************************************/
2131 
std::map< std::string, OpenDriveEdge * > & myEdges
bool gDebugFlag1
global utility flags for debugging
Definition: StdDefs.cpp:32
NBNode * retrieve(const std::string &id) const
Returns the node with the given name.
Definition: NBNodeCont.cpp:108
std::vector< int > myElementStack
std::string rightType
the composite type built from all used lane types
double length2D() const
Returns the length.
double sOrig
The original starting offset of this lane section (differs from s if the section had to be split) ...
double getSpeed(const std::string &type) const
Returns the maximal velocity for the given type [m/s].
Definition: NBTypeCont.cpp:180
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:166
OpenDriveLaneSection buildLaneSection(double startPos)
is a pedestrian
std::vector< OpenDriveWidth > widthData
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:53
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:249
int gPrecision
the precision for floating point outputs
Definition: StdDefs.cpp:29
Position positionAtOffset2D(double pos, double lateralOffset=0) const
Returns the position at the given length.
Representation of a lane section.
vehicle is a bicycle
const std::string & getFileName() const
returns the current file name
double y() const
Returns the y-position.
Definition: Position.h:67
int SVCPermissions
bitset where each bit declares whether a certain SVC may use this edge/lane
The representation of a single edge during network building.
Definition: NBEdge.h:70
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:62
The base class for traffic light logic definitions.
static const double UNSPECIFIED_OFFSET
unspecified lane offset
Definition: NBEdge.h:257
ContactPoint myCurrentContactPoint
void odrSpiral(double s, double cDot, double *x, double *y, double *t)
Definition: odrSpiral.cpp:231
T MAX2(T a, T b)
Definition: StdDefs.h:73
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:198
bool getBool(const std::string &name) const
Returns the boolean-value of the named option (only for Option_Bool)
Poly3 OpenDriveElevation
LaneOffset has the same fields as Elevation.
const std::string & getID() const
Returns the id.
Definition: Named.h:74
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:1177
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:109
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:39
static const double UNSPECIFIED_WIDTH
unspecified lane width
Definition: NBEdge.h:254
A class that stores a 2D geometrical boundary.
Definition: Boundary.h:47
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:199
The connection was computed and validated.
Definition: NBEdge.h:114
static OptionsCont & getOptions()
Retrieves the options.
Definition: OptionsCont.cpp:64
double getWidth(const std::string &type) const
Returns the lane width for the given type [m].
Definition: NBTypeCont.cpp:216
authorities vehicles
static std::string revertID(const std::string &id)
static void findWidthSplit(const NBTypeCont &tc, std::vector< OpenDriveLane > &lanes, int section, double sectionStart, double sectionEnd, std::vector< double > &splitPositions)
OpenDriveXMLTag myCurrentLaneDirection
An (internal) definition of a single lane of an edge.
Definition: NBEdge.h:121
SVCPermissions permissions
List of vehicle types that are allowed on this lane.
Definition: NBEdge.h:132
bool isTLControlled() const
Returns whether this node is controlled by any tls.
Definition: NBNode.h:297
std::vector< OpenDriveLink > links
static double naviDegree(const double angle)
Definition: GeomHelper.cpp:186
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.
bool knows(const std::string &type) const
Returns whether the named type is in the container.
Definition: NBTypeCont.cpp:74
static PositionVector geomFromSpiral(const OpenDriveEdge &e, const OpenDriveGeometry &g, double resolution)
static void setStraightConnections(std::vector< OpenDriveLane > &lanes)
bool insert(NBEdge *edge, bool ignorePrunning=false)
Adds an edge to the dictionary.
Definition: NBEdgeCont.cpp:157
std::string toString(const T &t, std::streamsize accuracy=gPrecision)
Definition: ToString.h:55
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.
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)
static void recomputeWidths(OpenDriveLaneSection &sec, double start, double end, double sectionStart, double sectionEnd)
A point in 2D or 3D with translation and scaling methods.
Definition: Position.h:45
NBEdgeCont & getEdgeCont()
Definition: NBNetBuilder.h:156
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)
static void splitMinWidths(OpenDriveEdge *e, const NBTypeCont &tc, double minDist)
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:201
#define POSITION_EPS
Definition: config.h:175
void setParameter(const std::string &key, const std::string &value)
Sets a parameter.
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:112
double speed
The speed allowed on this lane.
Definition: NBEdge.h:129
double width
This lane&#39;s width.
Definition: NBEdge.h:141
double getFloat(const std::string &name) const
Returns the double-value of the named option (only for Option_Float)
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)
void buildLaneMapping(const NBTypeCont &tc)
Build the mapping from OpenDrive to SUMO lanes.
std::vector< OpenDriveSignal > signals
#define WRITE_ERROR(msg)
Definition: MsgHandler.h:205
LinkType
OpenDrive link type enumeration.
PositionVector getSubpart2D(double beginOffset, double endOffset) const
get subpart of a position vector in two dimensions (Z is ignored)
void rotate2D(double angle)
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:321
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, double speed=UNSPECIFIED_SPEED, const PositionVector &customShape=PositionVector::EMPTY)
Adds a connection between the specified this edge&#39;s lane and an approached one.
Definition: NBEdge.cpp:921
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:845
NBNodeCont & getNodeCont()
Returns a reference to the node container.
Definition: NBNetBuilder.h:161
Instance responsible for building networks.
Definition: NBNetBuilder.h:115
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:302
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:250
A storage for options typed value containers)
Definition: OptionsCont.h:98
bool insert(const std::string &id, const Position &position, NBDistrict *district=0)
Inserts a node into the map.
Definition: NBNodeCont.cpp:79
NBTrafficLightLogicCont & getTLLogicCont()
Returns a reference to the traffic light logics container.
Definition: NBNetBuilder.h:171
const std::string SUMO_PARAM_ORIGID
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:74
void addGeometryShape(GeometryType type, const std::vector< double > &vals)
A class for sorting lane sections by their s-value.
#define DEBUG_COND
bool insert(NBTrafficLightDefinition *logic, bool forceInsert=false)
Adds a logic definition to the dictionary.
double computeAt(double pos) const
static void setNodeSecure(NBNodeCont &nc, OpenDriveEdge &e, const std::string &nodeID, NIImporter_OpenDrive::LinkType lt)
#define DEBUG_COND2(obj)
Definition: MSLane.cpp:86
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:66
std::vector< std::pair< double, double > > speeds
List of positions/speeds of speed changes.
#define PROGRESS_DONE_MESSAGE()
Definition: MsgHandler.h:202
A traffic light logics which must be computed (only nodes/edges are given)
Definition: NBOwnTLDef.h:53
void add(double xoff, double yoff, double zoff)
public emergency vehicles
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:433
static double cn[6]
Definition: odrSpiral.cpp:63
std::string joinToString(const std::vector< T > &v, const T_BETWEEN &between, std::streamsize accuracy=gPrecision)
Definition: ToString.h:236
#define UNSET_CONNECTION
TrafficLightType
SVCPermissions getPermissions(const std::string &type) const
Returns allowed vehicle classes for the given type.
Definition: NBTypeCont.cpp:210
A storage for available types of edges.
Definition: NBTypeCont.h:61
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.