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