SUMO - Simulation of Urban MObility
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
NIImporter_OpenDrive.cpp
Go to the documentation of this file.
1 /****************************************************************************/
10 // Importer for networks stored in openDrive format
11 /****************************************************************************/
12 // SUMO, Simulation of Urban MObility; see http://sumo.sourceforge.net/
13 // Copyright (C) 2001-2012 DLR (http://www.dlr.de/) and contributors
14 /****************************************************************************/
15 //
16 // This file is part of SUMO.
17 // SUMO is free software: you can redistribute it and/or modify
18 // it under the terms of the GNU General Public License as published by
19 // the Free Software Foundation, either version 3 of the License, or
20 // (at your option) any later version.
21 //
22 /****************************************************************************/
23 
24 
25 // ===========================================================================
26 // included modules
27 // ===========================================================================
28 #ifdef _MSC_VER
29 #include <windows_config.h>
30 #else
31 #include <config.h>
32 #endif
33 #include <string>
34 #include <cmath>
38 #include <utils/common/ToString.h>
41 #include <netbuild/NBEdge.h>
42 #include <netbuild/NBEdgeCont.h>
43 #include <netbuild/NBNode.h>
44 #include <netbuild/NBNodeCont.h>
45 #include <netbuild/NBNetBuilder.h>
51 #include <utils/xml/XMLSubSys.h>
52 #include <utils/geom/Boundary.h>
53 #include "NILoader.h"
54 #include "NIImporter_OpenDrive.h"
55 
56 #ifdef CHECK_MEMORY_LEAKS
57 #include <foreign/nvwa/debug_new.h>
58 #endif // CHECK_MEMORY_LEAKS
59 
60 
61 // ===========================================================================
62 // definitions
63 // ===========================================================================
64 #define C_LENGTH 10.
65 
66 
67 // ===========================================================================
68 // static variables
69 // ===========================================================================
86 
88 };
89 
90 
115 
117 };
118 
119 
120 std::set<std::string> NIImporter_OpenDrive::myLaneTypes2Import;
121 
122 // ===========================================================================
123 // method definitions
124 // ===========================================================================
125 // ---------------------------------------------------------------------------
126 // static methods (interface in this case)
127 // ---------------------------------------------------------------------------
128 void
130  // check whether the option is set (properly)
131  if (!oc.isUsableFileList("opendrive-files")) {
132  return;
133  }
134  myLaneTypes2Import.insert("driving");
135  myLaneTypes2Import.insert("stop");
136  //myLaneTypes2Import.insert("mwyEntry");
137  //myLaneTypes2Import.insert("mwyExit");
138  myLaneTypes2Import.insert("special1");
139  myLaneTypes2Import.insert("parking");
140  // build the handler
141  std::vector<OpenDriveEdge> innerEdges, outerEdges;
142  NIImporter_OpenDrive handler(innerEdges, outerEdges);
143  // parse file(s)
144  std::vector<std::string> files = oc.getStringVector("opendrive-files");
145  for (std::vector<std::string>::const_iterator file = files.begin(); file != files.end(); ++file) {
146  if (!FileHelpers::exists(*file)) {
147  WRITE_ERROR("Could not open opendrive file '" + *file + "'.");
148  return;
149  }
150  handler.setFileName(*file);
151  PROGRESS_BEGIN_MESSAGE("Parsing opendrive from '" + *file + "'");
152  XMLSubSys::runParser(handler, *file);
154  }
155  // convert geometries into a discretised representation
156  computeShapes(innerEdges);
157  computeShapes(outerEdges);
158 
159  // -------------------------
160  // node building
161  // -------------------------
162  // build nodes#1
163  // look at all links which belong to a node, collect their bounding boxes
164  // and place the node in the middle of this bounding box
165  std::map<std::string, Boundary> posMap;
166  std::map<std::string, std::string> edge2junction;
167  // compute node positions
168  for (std::vector<OpenDriveEdge>::iterator i = innerEdges.begin(); i != innerEdges.end(); ++i) {
169  OpenDriveEdge& e = *i;
170  assert(e.junction != "-1" && e.junction != "");
171  edge2junction[e.id] = e.junction;
172  if (posMap.find(e.junction) == posMap.end()) {
173  posMap[e.junction] = Boundary();
174  }
175  posMap[e.junction].add(e.geom.getBoxBoundary());
176  }
177  // build nodes
178  for (std::map<std::string, Boundary>::iterator i = posMap.begin(); i != posMap.end(); ++i) {
179  if (!nb.getNodeCont().insert((*i).first, (*i).second.getCenter())) {
180  throw ProcessError("Could not add node '" + (*i).first + "'.");
181  }
182  }
183  // assign built nodes
184  for (std::vector<OpenDriveEdge>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
185  OpenDriveEdge& e = *i;
186  for (std::vector<OpenDriveLink>::iterator j = e.links.begin(); j != e.links.end(); ++j) {
187  OpenDriveLink& l = *j;
188  if (l.elementType != OPENDRIVE_ET_ROAD) {
189  // set node information
191  continue;
192  }
193  if (edge2junction.find(l.elementID) != edge2junction.end()) {
194  // set node information of an internal road
195  setNodeSecure(nb.getNodeCont(), e, edge2junction[l.elementID], l.linkType);
196  continue;
197  }
198  }
199  }
200  // we should now have all nodes set for links which are not outer edge-to-outer edge links
201 
202 
203  // build nodes#2
204  // build nodes for all outer edge-to-outer edge connections
205  for (std::vector<OpenDriveEdge>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
206  OpenDriveEdge& e = *i;
207  for (std::vector<OpenDriveLink>::iterator j = e.links.begin(); j != e.links.end(); ++j) {
208  OpenDriveLink& l = *j;
209  if (l.elementType != OPENDRIVE_ET_ROAD || edge2junction.find(l.elementID) != edge2junction.end()) {
210  // is a connection to an internal edge, or a node, skip
211  continue;
212  }
213  // we have a direct connection between to external edges
214  std::string id1 = e.id;
215  std::string id2 = l.elementID;
216  if (id1 < id2) {
217  std::swap(id1, id2);
218  }
219  std::string nid = id1 + "." + id2;
220  if (nb.getNodeCont().retrieve(nid) == 0) {
221  // not yet seen, build
222  Position pos = l.linkType == OPENDRIVE_LT_SUCCESSOR ? e.geom[(int)e.geom.size() - 1] : e.geom[0];
223  if (!nb.getNodeCont().insert(nid, pos)) {
224  throw ProcessError("Could not build node '" + nid + "'.");
225  }
226  }
227  /* debug-stuff
228  else {
229  Position pos = l.linkType==OPENDRIVE_LT_SUCCESSOR ? e.geom[e.geom.size()-1] : e.geom[0];
230  cout << nid << " " << pos << " " << nb.getNodeCont().retrieve(nid)->getPosition() << endl;
231  }
232  */
233  setNodeSecure(nb.getNodeCont(), e, nid, l.linkType);
234  }
235  }
236  // we should now have start/end nodes for all outer edge-to-outer edge connections
237 
238 
239  // build nodes#3
240  // assign further nodes generated from inner-edges
241  // these nodes have not been assigned earlier, because the connectiosn are referenced in inner-edges
242  for (std::vector<OpenDriveEdge>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
243  OpenDriveEdge& e = *i;
244  if (e.to != 0 && e.from != 0) {
245  continue;
246  }
247  for (std::vector<OpenDriveEdge>::iterator j = innerEdges.begin(); j != innerEdges.end(); ++j) {
248  OpenDriveEdge& ie = *j;
249  for (std::vector<OpenDriveLink>::iterator k = ie.links.begin(); k != ie.links.end(); ++k) {
250  OpenDriveLink& il = *k;
251  if (il.elementType != OPENDRIVE_ET_ROAD || il.elementID != e.id) {
252  // not conneted to the currently investigated outer edge
253  continue;
254  }
255  std::string nid = edge2junction[ie.id];
256  if (il.contactPoint == OPENDRIVE_CP_START) {
258  } else {
260  }
261  }
262  }
263 
264  }
265 
266 
267  // build start/end nodes which were not defined previously
268  for (std::vector<OpenDriveEdge>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
269  OpenDriveEdge& e = *i;
270  if (e.from == 0) {
271  std::string nid = e.id + ".begin";
272  Position pos(e.geometries[0].x, e.geometries[0].y);
273  e.from = getOrBuildNode(nid, e.geom[0], nb.getNodeCont());
274  }
275  if (e.to == 0) {
276  std::string nid = e.id + ".end";
277  Position pos(e.geometries[e.geometries.size() - 1].x, e.geometries[e.geometries.size() - 1].y);
278  e.to = getOrBuildNode(nid, e.geom[(int)e.geom.size() - 1], nb.getNodeCont());
279  }
280  }
281 
282 
283  // -------------------------
284  // edge building
285  // -------------------------
286  std::map<NBEdge*, std::map<int, int> > fromLaneMap;
287  std::map<NBEdge*, std::map<int, int> > toLaneMap;
288  //bool useLoadedLengths = oc.getBool("opendrive.use-given-lengths");
289  // build edges
290  for (std::vector<OpenDriveEdge>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
291  OpenDriveEdge& e = *i;
292  SUMOReal speed = nb.getTypeCont().getSpeed("");
294  unsigned int noLanesRight = e.getMaxLaneNumber(OPENDRIVE_TAG_RIGHT);
295  unsigned int noLanesLeft = e.getMaxLaneNumber(OPENDRIVE_TAG_LEFT);
296  if (noLanesRight != 0 || noLanesLeft != 0) {
297  lsf = LANESPREAD_RIGHT;
298  } else {
299  WRITE_WARNING("Edge '" + e.id + "' has no lanes.");
300  }
301  if (noLanesRight > 0) {
302  int priority = e.getPriority(OPENDRIVE_TAG_RIGHT);
303  NBEdge* nbe = new NBEdge("-" + e.id, e.from, e.to, "", speed, noLanesRight, priority,
305  if (!nb.getEdgeCont().insert(nbe)) {
306  throw ProcessError("Could not add edge '" + std::string("-") + e.id + "'.");
307  }
308  for (unsigned int j = 0; j < noLanesRight; ++j) {
309  nbe->getLaneStruct(j).origID = e.id + " -" + toString(j + 1);
310  }
311  //if(useLoadedLengths) { nbe->setLoadedLength(e.length); }
312  fromLaneMap[nbe] = e.laneSections.back().buildLaneMapping(OPENDRIVE_TAG_RIGHT);
313  toLaneMap[nbe] = e.laneSections[0].buildLaneMapping(OPENDRIVE_TAG_RIGHT);
314  }
315  if (noLanesLeft > 0) {
316  int priority = e.getPriority(OPENDRIVE_TAG_LEFT);
317  NBEdge* nbe = new NBEdge(e.id, e.to, e.from, "", speed, noLanesLeft, priority,
319  if (!nb.getEdgeCont().insert(nbe)) {
320  throw ProcessError("Could not add edge '" + e.id + "'.");
321  }
322  //if(useLoadedLengths) { nbe->setLoadedLength(e.length); }
323  for (unsigned int j = 0; j < noLanesLeft; ++j) {
324  nbe->getLaneStruct(j).origID = e.id + " " + toString(j + 1);
325  }
326  fromLaneMap[nbe] = e.laneSections[0].buildLaneMapping(OPENDRIVE_TAG_LEFT);
327  toLaneMap[nbe] = e.laneSections.back().buildLaneMapping(OPENDRIVE_TAG_LEFT);
328  }
329  }
330 
331 
332  // -------------------------
333  // connections building
334  // -------------------------
335  std::vector<Connection> connections;
336  // connections#1
337  // build connections between outer-edges
338  for (std::vector<OpenDriveEdge>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
339  OpenDriveEdge& e = *i;
340  for (std::vector<OpenDriveLink>::iterator j = e.links.begin(); j != e.links.end(); ++j) {
341  OpenDriveLink& l = *j;
342  if (l.elementType != OPENDRIVE_ET_ROAD) {
343  // we are not interested in connections to nodes
344  continue;
345  }
346  if (edge2junction.find(l.elementID) != edge2junction.end()) {
347  // connection via an inner-road
349  nb.getNodeCont().retrieve(edge2junction[l.elementID]),
350  e, l.linkType, l.elementID, connections);
351  } else {
352  // connection between two outer-edges; can be used directly
353  std::vector<OpenDriveEdge>::iterator p = std::find_if(outerEdges.begin(), outerEdges.end(), edge_by_id_finder(l.elementID));
354  if (p == outerEdges.end()) {
355  throw ProcessError("Could not find connection edge.");
356  }
357  std::string id1 = e.id;
358  std::string id2 = (*p).id;
359  if (id1 < id2) {
360  std::swap(id1, id2);
361  }
362  std::string nid = id1 + "." + id2;
364  addE2EConnectionsSecure(nb.getEdgeCont(), nb.getNodeCont().retrieve(nid), *p, e, connections);
365  } else {
366  addE2EConnectionsSecure(nb.getEdgeCont(), nb.getNodeCont().retrieve(nid), e, *p, connections);
367  }
368  }
369  }
370  }
371 
372  // build inner-edge connections
373  for (std::vector<OpenDriveEdge>::iterator i = innerEdges.begin(); i != innerEdges.end(); ++i) {
374  OpenDriveEdge& e = *i;
375  std::string pred, succ;
377  for (std::vector<OpenDriveLink>::iterator j = e.links.begin(); j != e.links.end(); ++j) {
378  OpenDriveLink& l = *j;
379  if (l.elementType != OPENDRIVE_ET_ROAD) {
380  // we are not interested in connections to nodes
381  std::cout << "unsupported" << std::endl;
382  continue;
383  }
384  if (edge2junction.find(l.elementID) != edge2junction.end()) {
385  // not supported
386  std::cout << "unsupported" << std::endl;
387  continue;
388  }
389  if (l.linkType == OPENDRIVE_LT_SUCCESSOR) {
390  if (succ != "") {
391  std::cout << "double succ" << std::endl;
392  }
393  succ = l.elementID;
394  succC = l.contactPoint;
395  } else {
396  if (pred != "") {
397  std::cout << "double pred" << std::endl;
398  }
399  pred = l.elementID;
400  predC = l.contactPoint;
401  }
402  }
403 
405  std::cout << "Both dirs given!" << std::endl;
406  }
407 
408  bool isReversed = false;
409  if (e.getMaxLaneNumber(OPENDRIVE_TAG_LEFT) != 0) {
410  // std::swap(pred, succ);
411  //std::swap(predC, succC);
412  isReversed = true;
413  }
414 
415  if (succ == "" || pred == "") {
416  std::cout << "Missing edge." << std::endl;
417  continue; // yes, occurs
418  }
419  std::vector<OpenDriveEdge>::iterator predEdge = std::find_if(outerEdges.begin(), outerEdges.end(), edge_by_id_finder(pred));
420  if (predEdge == outerEdges.end()) {
421  throw ProcessError("Could not find connection edge.");
422  }
423  std::vector<OpenDriveEdge>::iterator succEdge = std::find_if(outerEdges.begin(), outerEdges.end(), edge_by_id_finder(succ));
424  if (succEdge == outerEdges.end()) {
425  throw ProcessError("Could not find connection edge.");
426  }
427  NBEdge* fromEdge, *toEdge;
428  if (!isReversed) {
429  fromEdge = predC == OPENDRIVE_CP_END ? nb.getEdgeCont().retrieve("-" + pred) : nb.getEdgeCont().retrieve(pred);
430  toEdge = succC == OPENDRIVE_CP_START ? nb.getEdgeCont().retrieve("-" + succ) : nb.getEdgeCont().retrieve(succ);
431  } else {
432  fromEdge = predC != OPENDRIVE_CP_END ? nb.getEdgeCont().retrieve("-" + pred) : nb.getEdgeCont().retrieve(pred);
433  toEdge = succC != OPENDRIVE_CP_START ? nb.getEdgeCont().retrieve("-" + succ) : nb.getEdgeCont().retrieve(succ);
434  }
435  /*
436  Connection c(
437  n->hasIncoming(nb.getEdgeCont().retrieve("-" + pred)) ? nb.getEdgeCont().retrieve("-" + pred) : nb.getEdgeCont().retrieve(pred),
438  e.id,
439  n->hasOutgoing(nb.getEdgeCont().retrieve("-" + succ)) ? nb.getEdgeCont().retrieve("-" + succ) : nb.getEdgeCont().retrieve(succ));
440  */
441  Connection c(fromEdge, e.id, toEdge);
442  if (c.from == 0 || c.to == 0 || c.from == c.to) {
443  throw ProcessError("Something's false");
444  }
446  *predEdge, c.from->getID()[0] != '-', c.from->getID()[0] == '-' ? OPENDRIVE_TAG_RIGHT : OPENDRIVE_TAG_LEFT,
447  e, isReversed, !isReversed ? OPENDRIVE_TAG_RIGHT : OPENDRIVE_TAG_LEFT,
448  *succEdge, c.to->getID()[0] != '-', c.to->getID()[0] == '-' ? OPENDRIVE_TAG_RIGHT : OPENDRIVE_TAG_LEFT);
449  c.id = e.id;
450  connections.push_back(c);
451  }
452 
453  for (std::vector<Connection>::const_iterator i = connections.begin(); i != connections.end(); ++i) {
454  if ((*i).from == 0 || (*i).to == 0) {
455  std::cout << "Nope." << std::endl;
456  continue;
457  }
458  (*i).from->addEdge2EdgeConnection((*i).to);
459  std::map<int, int> fromMap = fromLaneMap[(*i).from];
460  std::map<int, int> toMap = fromLaneMap[(*i).to];
461  int index = 0;
462  for (std::vector<std::pair<int, int> >::const_iterator j = (*i).lanes.begin(); j != (*i).lanes.end(); ++j, --index) {
463  int fromLane = fromMap[(*j).first];
464  int toLane = toMap[(*j).second];
465  if (static_cast<unsigned int>(fromLane) >= (*i).from->getNumLanes() || fromLane < 0) {
466  std::cout << "False " << std::endl;
467  }
468  if (static_cast<unsigned int>(toLane) >= (*i).to->getNumLanes() || toLane < 0) {
469  std::cout << "False " << std::endl;
470  }
471 
472  (*i).from->addLane2LaneConnection(fromLane, (*i).to, toLane, NBEdge::L2L_VALIDATED, true, false);
473  if ((*i).id != "") {
474  std::vector<NBEdge::Connection>& cons = (*i).from->getConnections();
475  for (std::vector<NBEdge::Connection>::iterator k = cons.begin(); k != cons.end(); ++k) {
476  if ((*k).fromLane == fromLane && (*k).toEdge == (*i).to && (*k).toLane == toLane) {
477  (*k).origID = (*i).id + " " + toString(index);
478  break;
479  }
480  }
481  }
482  }
483  }
484 }
485 
486 
487 void
489  const NBNode* const node, const OpenDriveEdge& e,
490  LinkType lt, const std::string& via,
491  std::vector<NIImporter_OpenDrive::Connection>& connections) {
492  NBEdge* from = 0;
493  NBEdge* to = 0;
494  if (node == e.to) {
495  // the connection is at the end of the "positive" direction
496  if (lt == OPENDRIVE_LT_PREDECESSOR) {
497  // via -> edge
498  to = ec.retrieve(e.id);
499  } else {
500  // -edge -> via
501  // "ambigous?"
502  from = ec.retrieve("-" + e.id);
503  }
504  } else {
505  // the connection is at the begin of the "positive" direction
506  if (lt == OPENDRIVE_LT_PREDECESSOR) {
507  // via -> -edge
508  to = ec.retrieve("-" + e.id);
509  } else {
510  // edge -> via
511  // "ambigous?"
512  from = ec.retrieve(e.id);
513  }
514  }
515  if (from == 0 && to == 0) {
516  throw ProcessError("Missing edge");
517  }
518  Connection c(from, via, to);
519  connections.push_back(c);
520 }
521 
522 
523 void
526  std::vector<NIImporter_OpenDrive::Connection>& connections) {
527  // positive direction (from is incoming, to is outgoing)
528  NBEdge* fromEdge = ec.retrieve("-" + from.id);
529  if (fromEdge == 0 || !node->hasIncoming(fromEdge)) {
530  fromEdge = ec.retrieve(from.id);
531  }
532  NBEdge* toEdge = ec.retrieve("-" + to.id);
533  if (toEdge == 0 || !node->hasOutgoing(toEdge)) {
534  toEdge = ec.retrieve(to.id);
535  }
536  if (fromEdge != 0 && toEdge != 0) {
537  Connection c(fromEdge, "", toEdge);
539  from, c.from->getID()[0] != '-', c.from->getID()[0] == '-' ? OPENDRIVE_TAG_RIGHT : OPENDRIVE_TAG_LEFT,
540  to, c.to->getID()[0] != '-', c.to->getID()[0] == '-' ? OPENDRIVE_TAG_RIGHT : OPENDRIVE_TAG_LEFT);
541  connections.push_back(c);
542  }
543  // negative direction (to is incoming, from is outgoing)
544  fromEdge = ec.retrieve("-" + from.id);
545  if (fromEdge == 0 || !node->hasOutgoing(fromEdge)) {
546  fromEdge = ec.retrieve(from.id);
547  }
548  toEdge = ec.retrieve("-" + to.id);
549  if (toEdge == 0 || !node->hasIncoming(toEdge)) {
550  toEdge = ec.retrieve(to.id);
551  }
552  if (fromEdge != 0 && toEdge != 0) {
553  Connection c(toEdge, "", fromEdge);
555  to, c.to->getID()[0] != '-', c.to->getID()[0] == '-' ? OPENDRIVE_TAG_RIGHT : OPENDRIVE_TAG_LEFT,
556  from, c.from->getID()[0] != '-', c.from->getID()[0] == '-' ? OPENDRIVE_TAG_RIGHT : OPENDRIVE_TAG_LEFT);
557  connections.push_back(c);
558  }
559 }
560 
561 
562 void
564  const OpenDriveEdge& from, bool fromAtBegin, OpenDriveXMLTag fromLaneDir,
565  const OpenDriveEdge& to, bool toAtEnd, OpenDriveXMLTag toLaneDir) {
566  const OpenDriveLaneSection& fromLS = fromAtBegin ? from.laneSections[0] : from.laneSections.back();
567  const std::vector<OpenDriveLane>& fromLanes = fromLS.lanesByDir.find(fromLaneDir)->second;
568  const OpenDriveLaneSection& toLS = toAtEnd ? to.laneSections.back() : to.laneSections[0];
569  const std::vector<OpenDriveLane>& toLanes = toLS.lanesByDir.find(toLaneDir)->second;
570  // in the following, we are probably using the same information twice, stored once
571  // in the from-edge's successor field and in the to-edge's precessor field.
572  // though, we have no proof or information that this is always redundant
573  for (std::vector<OpenDriveLane>::const_iterator i = fromLanes.begin(); i != fromLanes.end(); ++i) {
574  if (myLaneTypes2Import.find((*i).type) == myLaneTypes2Import.end()) {
575  continue;
576  }
577  if (!fromAtBegin && (*i).successor != UNSET_CONNECTION) {
578  c.lanes.push_back(std::make_pair((*i).id, (*i).successor));
579  }
580  if (fromAtBegin && (*i).predecessor != UNSET_CONNECTION) {
581  c.lanes.push_back(std::make_pair((*i).id, (*i).predecessor));
582  }
583  }
584  for (std::vector<OpenDriveLane>::const_iterator i = toLanes.begin(); i != toLanes.end(); ++i) {
585  if (myLaneTypes2Import.find((*i).type) == myLaneTypes2Import.end()) {
586  continue;
587  }
588  if (!toAtEnd && (*i).predecessor != UNSET_CONNECTION) {
589  c.lanes.push_back(std::make_pair((*i).predecessor, (*i).id));
590  }
591  if (toAtEnd && (*i).successor != UNSET_CONNECTION) {
592  c.lanes.push_back(std::make_pair((*i).successor, (*i).id));
593  }
594  }
595 }
596 
597 
598 void
600  const OpenDriveEdge& from, bool fromAtBegin, OpenDriveXMLTag fromLaneDir,
601  const OpenDriveEdge& via, bool viaIsReversed, OpenDriveXMLTag viaLaneDir,
602  const OpenDriveEdge& to, bool toAtEnd, OpenDriveXMLTag toLaneDir) {
603  Connection from2via(0, "", 0);
604  setLaneConnections(from2via, from, fromAtBegin, fromLaneDir, via, viaIsReversed, viaLaneDir);
605  Connection via2to(0, "", 0);
606  setLaneConnections(via2to, via, viaIsReversed, viaLaneDir, to, toAtEnd, toLaneDir);
607  for (std::vector<std::pair<int, int> >::const_iterator i = from2via.lanes.begin(); i != from2via.lanes.end(); ++i) {
608  int fromLane = (*i).first;
609  int viaLane = (*i).second;
610  for (std::vector<std::pair<int, int> >::const_iterator j = via2to.lanes.begin(); j != via2to.lanes.end(); ++j) {
611  if ((*j).first == viaLane) {
612  c.lanes.push_back(std::make_pair(fromLane, (*j).second));
613  break;
614  }
615  }
616  }
617 }
618 
619 
620 NBNode*
621 NIImporter_OpenDrive::getOrBuildNode(const std::string& id, Position& pos,
622  NBNodeCont& nc) {
623  if (nc.retrieve(id) == 0) {
624  // not yet built; build now
625  if (!nc.insert(id, pos)) {
626  // !!! clean up
627  throw ProcessError("Could not add node '" + id + "'.");
628  }
629  }
630  return nc.retrieve(id);
631 }
632 
633 
634 void
636  const std::string& nodeID, NIImporter_OpenDrive::LinkType lt) {
637  NBNode* n = nc.retrieve(nodeID);
638  if (n == 0) {
639  throw ProcessError("Could not find node '" + nodeID + "'.");
640  }
641  if (lt == OPENDRIVE_LT_SUCCESSOR) {
642  if (e.to != 0 && e.to != n) {
643  throw ProcessError("Edge '" + e.id + "' has two end nodes.");
644  }
645  e.to = n;
646  } else {
647  if (e.from != 0 && e.from != n) {
648  throw ProcessError("Edge '" + e.id + "' has two start nodes.");
649  }
650  e.from = n;
651  }
652 }
653 
654 
655 
656 
657 
658 NBEdge*
660  const NBNodeCont& nc,
661  const std::string& edgeID,
662  const std::string& nodeID) {
663  NBNode* node = nc.retrieve(nodeID);
664  NBEdge* e = ec.retrieve(edgeID);
665  if (e != 0 && node->hasOutgoing(e)) {
666  return e;
667  }
668  e = ec.retrieve("-" + edgeID);
669  if (e != 0 && node->hasOutgoing(e)) {
670  return e;
671  }
672  return 0;
673 }
674 
675 
676 NBEdge*
678  const NBNodeCont& nc,
679  const std::string& edgeID,
680  const std::string& nodeID) {
681  NBNode* node = nc.retrieve(nodeID);
682  NBEdge* e = ec.retrieve(edgeID);
683  if (e != 0 && node->hasIncoming(e)) {
684  return e;
685  }
686  e = ec.retrieve("-" + edgeID);
687  if (e != 0 && node->hasIncoming(e)) {
688  return e;
689  }
690  return 0;
691 }
692 
693 
694 void
695 NIImporter_OpenDrive::computeShapes(std::vector<OpenDriveEdge>& edges) {
696  for (std::vector<OpenDriveEdge>::iterator i = edges.begin(); i != edges.end(); ++i) {
697  OpenDriveEdge& e = *i;
698  for (std::vector<OpenDriveGeometry>::iterator j = e.geometries.begin(); j != e.geometries.end(); ++j) {
699  OpenDriveGeometry& g = *j;
700  std::vector<Position> geom;
701  switch (g.type) {
703  break;
704  case OPENDRIVE_GT_LINE:
705  geom = geomFromLine(e, g);
706  break;
707  case OPENDRIVE_GT_ARC:
708  geom = geomFromArc(e, g);
709  break;
710  case OPENDRIVE_GT_POLY3:
711  geom = geomFromPoly(e, g);
712  break;
713  default:
714  break;
715  }
716  for (std::vector<Position>::iterator k = geom.begin(); k != geom.end(); ++k) {
718  }
719  }
720  for (unsigned int j = 0; j < e.geom.size(); ++j) {
722  WRITE_ERROR("Unable to project coordinates for.");
723  }
724  }
725  }
726 }
727 
728 std::vector<Position>
730  UNUSED_PARAMETER(e);
731  std::vector<Position> ret;
732  ret.push_back(Position(g.x, g.y));
733  ret.push_back(calculateStraightEndPoint(g.hdg, g.length, Position(g.x, g.y)));
734  return ret;
735 }
736 
737 
738 std::vector<Position>
740  UNUSED_PARAMETER(e);
741  std::vector<Position> ret;
742  SUMOReal dist = 0.0;
743  SUMOReal centerX = g.x;
744  SUMOReal centerY = g.y;
745  // left: positive value
746  SUMOReal curvature = g.params[0];
747  SUMOReal radius = 1. / curvature;
748  // center point
749  calculateCurveCenter(&centerX, &centerY, radius, g.hdg);
750  SUMOReal endX = g.x;
751  SUMOReal endY = g.y;
752  SUMOReal startX = g.x;
753  SUMOReal startY = g.y;
754  SUMOReal hdgS = g.hdg;
755  SUMOReal hdgE;
756  SUMOReal geo_posS = g.s;
757  SUMOReal geo_posE = g.s;
758  bool end = false;
759  do {
760  geo_posE += C_LENGTH;
761  if (geo_posE - g.s > g.length) {
762  geo_posE = g.s + g.length;
763  }
764  if (geo_posE - g.s > g.length) {
765  geo_posE = g.s + g.length;
766  }
767  calcPointOnCurve(&endX, &endY, centerX, centerY, radius, geo_posE - geo_posS);
768 
769  dist += (geo_posE - geo_posS);
770  if (curvature > 0.0) {
771  hdgE = g.hdg + dist / fabs(radius);
772  } else {
773  hdgE = g.hdg - dist / fabs(radius);
774  }
775  //
776  ret.push_back(Position(startX, startY));
777  //
778  startX = endX;
779  startY = endY;
780  geo_posS = geo_posE;
781  hdgS = hdgE;
782 
783  if (geo_posE - (g.s + g.length) < 0.001 && geo_posE - (g.s + g.length) > -0.001) {
784  end = true;
785  }
786  } while (!end);
787  return ret;
788 }
789 
790 
791 std::vector<Position>
793  UNUSED_PARAMETER(g);
794  UNUSED_PARAMETER(e);
795  std::vector<Position> ret;
796  return ret;
797 }
798 
799 
800 Position
801 NIImporter_OpenDrive::calculateStraightEndPoint(double hdg, double length, const Position& start) {
802  double normx = 1.0f;
803  double normy = 0.0f;
804  double x2 = normx * cos(hdg) - normy * sin(hdg);
805  double y2 = normx * sin(hdg) + normy * cos(hdg);
806  normx = x2 * length;
807  normy = y2 * length;
808  return Position(start.x() + normx, start.y() + normy);
809 }
810 
811 
812 void
814  SUMOReal normX = 1.0;
815  SUMOReal normY = 0.0;
816  SUMOReal tmpX;
817  SUMOReal turn;
818  if (ad_radius > 0) {
819  turn = -1.0;
820  } else {
821  turn = 1.0;
822  }
823 
824  tmpX = normX;
825  normX = normX * cos(ad_hdg) + normY * sin(ad_hdg);
826  normY = tmpX * sin(ad_hdg) + normY * cos(ad_hdg);
827 
828  tmpX = normX;
829  normX = normX * cos(90 * PI / 180) + turn * normY * sin(90 * PI / 180);
830  normY = -1 * turn * tmpX * sin(90 * PI / 180) + normY * cos(90 * PI / 180);
831 
832  normX = fabs(ad_radius) * normX;
833  normY = fabs(ad_radius) * normY;
834 
835  *ad_x += normX;
836  *ad_y += normY;
837 }
838 
839 
840 void
842  SUMOReal ad_r, SUMOReal ad_length) {
843  double rotAngle = ad_length / fabs(ad_r);
844  double vx = *ad_x - ad_centerX;
845  double vy = *ad_y - ad_centerY;
846  double tmpx;
847 
848  double turn;
849  if (ad_r > 0) {
850  turn = -1; //left
851  } else {
852  turn = 1; //right
853  }
854  tmpx = vx;
855  vx = vx * cos(rotAngle) + turn * vy * sin(rotAngle);
856  vy = -1 * turn * tmpx * sin(rotAngle) + vy * cos(rotAngle);
857  *ad_x = vx + ad_centerX;
858  *ad_y = vy + ad_centerY;
859 }
860 
861 
862 // ---------------------------------------------------------------------------
863 // section
864 // ---------------------------------------------------------------------------
866  lanesByDir[OPENDRIVE_TAG_LEFT] = std::vector<OpenDriveLane>();
867  lanesByDir[OPENDRIVE_TAG_RIGHT] = std::vector<OpenDriveLane>();
868  lanesByDir[OPENDRIVE_TAG_CENTER] = std::vector<OpenDriveLane>();
869 }
870 
871 
872 unsigned int
874  unsigned int laneNum = 0;
875  const std::vector<OpenDriveLane>& dirLanes = lanesByDir.find(dir)->second;
876  for (std::vector<OpenDriveLane>::const_iterator i = dirLanes.begin(); i != dirLanes.end(); ++i) {
877  if (myLaneTypes2Import.find((*i).type) != myLaneTypes2Import.end()) {
878  ++laneNum;
879  }
880  }
881  return laneNum;
882 }
883 
884 
885 std::map<int, int>
887  std::map<int, int> ret;
888  unsigned int sumoLane = 0;
889  const std::vector<OpenDriveLane>& dirLanes = lanesByDir.find(dir)->second;
890  if (dir == OPENDRIVE_TAG_RIGHT) {
891  for (std::vector<OpenDriveLane>::const_reverse_iterator i = dirLanes.rbegin(); i != dirLanes.rend(); ++i) {
892  if (myLaneTypes2Import.find((*i).type) != myLaneTypes2Import.end()) {
893  ret[(*i).id] = sumoLane++;
894  }
895  }
896  } else {
897  for (std::vector<OpenDriveLane>::const_iterator i = dirLanes.begin(); i != dirLanes.end(); ++i) {
898  if (myLaneTypes2Import.find((*i).type) != myLaneTypes2Import.end()) {
899  ret[(*i).id] = sumoLane++;
900  }
901  }
902  }
903  return ret;
904 }
905 
906 
907 // ---------------------------------------------------------------------------
908 // edge
909 // ---------------------------------------------------------------------------
910 unsigned int
912  unsigned int maxLaneNum = 0;
913  for (std::vector<OpenDriveLaneSection>::const_iterator i = laneSections.begin(); i != laneSections.end(); ++i) {
914  maxLaneNum = MAX2(maxLaneNum, (*i).getLaneNumber(dir));
915  }
916  return maxLaneNum;
917 }
918 
919 
920 int
922  int prio = 1;
923  SUMOReal lastPos = -1;
924  for (std::vector<OpenDriveSignal>::const_iterator i = signals.begin(); i != signals.end(); ++i) {
925  int tmp = 1;
926  if ((*i).type == "301" || (*i).type == "306") {
927  tmp = 2;
928  }
929  if ((*i).type == "205") {
930  tmp = 0;
931  }
932  if (tmp != 1 && dir == OPENDRIVE_TAG_RIGHT && (*i).orientation < 0) {
933  prio = tmp;
934  }
935  if (tmp != 1 && dir == OPENDRIVE_TAG_LEFT && (*i).orientation > 0) {
936  prio = tmp;
937  }
938 
939  }
940  return prio;
941 }
942 
943 
944 
945 // ---------------------------------------------------------------------------
946 // loader methods
947 // ---------------------------------------------------------------------------
949  std::vector<OpenDriveEdge>& innerEdges,
950  std::vector<OpenDriveEdge>& outerEdges)
952  myCurrentEdge("", "", -1), myInnerEdges(innerEdges), myOuterEdges(outerEdges) {
953 }
954 
955 
957 }
958 
959 
960 void
962  const SUMOSAXAttributes& attrs) {
963  bool ok = true;
964  switch (element) {
965  case OPENDRIVE_TAG_HEADER: {
966  int majorVersion = attrs.getIntReporting(OPENDRIVE_ATTR_REVMAJOR, 0, ok);
967  int minorVersion = attrs.getIntReporting(OPENDRIVE_ATTR_REVMINOR, 0, ok);
968  if (majorVersion != 1 || minorVersion != 2) {
969  WRITE_WARNING("Given openDrive file '" + getFileName() + "' uses version " + toString(majorVersion) + "." + toString(minorVersion) + ";\n Version 1.2 is supported.");
970  }
971  }
972  break;
973  case OPENDRIVE_TAG_ROAD: {
974  std::string id = attrs.getStringReporting(OPENDRIVE_ATTR_ID, 0, ok);
975  std::string junction = attrs.getStringReporting(OPENDRIVE_ATTR_JUNCTION, id.c_str(), ok);
976  SUMOReal length = attrs.getSUMORealReporting(OPENDRIVE_ATTR_LENGTH, id.c_str(), ok);
977  myCurrentEdge = OpenDriveEdge(id, junction, length);
978  }
979  break;
981  if (myElementStack.size() >= 2 && myElementStack[myElementStack.size() - 2] == OPENDRIVE_TAG_ROAD) {
982  std::string elementType = attrs.getStringReporting(OPENDRIVE_ATTR_ELEMENTTYPE, myCurrentEdge.id.c_str(), ok);
983  std::string elementID = attrs.getStringReporting(OPENDRIVE_ATTR_ELEMENTID, myCurrentEdge.id.c_str(), ok);
984  std::string contactPoint = attrs.hasAttribute(OPENDRIVE_ATTR_CONTACTPOINT)
986  : "end";
987  addLink(OPENDRIVE_LT_PREDECESSOR, elementType, elementID, contactPoint);
988  }
989  if (myElementStack.size() >= 2 && myElementStack[myElementStack.size() - 2] == OPENDRIVE_TAG_LANE) {
990  int no = attrs.getIntReporting(OPENDRIVE_ATTR_ID, myCurrentEdge.id.c_str(), ok);
992  l.predecessor = no;
993  }
994  }
995  break;
997  if (myElementStack.size() >= 2 && myElementStack[myElementStack.size() - 2] == OPENDRIVE_TAG_ROAD) {
998  std::string elementType = attrs.getStringReporting(OPENDRIVE_ATTR_ELEMENTTYPE, myCurrentEdge.id.c_str(), ok);
999  std::string elementID = attrs.getStringReporting(OPENDRIVE_ATTR_ELEMENTID, myCurrentEdge.id.c_str(), ok);
1000  std::string contactPoint = attrs.hasAttribute(OPENDRIVE_ATTR_CONTACTPOINT)
1002  : "start";
1003  addLink(OPENDRIVE_LT_SUCCESSOR, elementType, elementID, contactPoint);
1004  }
1005  if (myElementStack.size() >= 2 && myElementStack[myElementStack.size() - 2] == OPENDRIVE_TAG_LANE) {
1006  int no = attrs.getIntReporting(OPENDRIVE_ATTR_ID, myCurrentEdge.id.c_str(), ok);
1008  l.successor = no;
1009  }
1010  }
1011  break;
1012  case OPENDRIVE_TAG_GEOMETRY: {
1018  myCurrentEdge.geometries.push_back(OpenDriveGeometry(length, s, x, y, hdg));
1019  }
1020  break;
1021  case OPENDRIVE_TAG_LINE: {
1022  std::vector<SUMOReal> vals;
1024  }
1025  break;
1026  case OPENDRIVE_TAG_SPIRAL: {
1027  std::vector<SUMOReal> vals;
1028  vals.push_back(attrs.getSUMORealReporting(OPENDRIVE_ATTR_CURVSTART, myCurrentEdge.id.c_str(), ok));
1029  vals.push_back(attrs.getSUMORealReporting(OPENDRIVE_ATTR_CURVEND, myCurrentEdge.id.c_str(), ok));
1031  }
1032  break;
1033  case OPENDRIVE_TAG_ARC: {
1034  std::vector<SUMOReal> vals;
1035  vals.push_back(attrs.getSUMORealReporting(OPENDRIVE_ATTR_CURVATURE, myCurrentEdge.id.c_str(), ok));
1037  }
1038  break;
1039  case OPENDRIVE_TAG_POLY3: {
1040  std::vector<SUMOReal> vals;
1041  vals.push_back(attrs.getSUMORealReporting(OPENDRIVE_ATTR_A, myCurrentEdge.id.c_str(), ok));
1042  vals.push_back(attrs.getSUMORealReporting(OPENDRIVE_ATTR_B, myCurrentEdge.id.c_str(), ok));
1043  vals.push_back(attrs.getSUMORealReporting(OPENDRIVE_ATTR_C, myCurrentEdge.id.c_str(), ok));
1044  vals.push_back(attrs.getSUMORealReporting(OPENDRIVE_ATTR_D, myCurrentEdge.id.c_str(), ok));
1046  }
1047  break;
1051  }
1052  break;
1053  case OPENDRIVE_TAG_LEFT:
1055  break;
1056  case OPENDRIVE_TAG_CENTER:
1058  break;
1059  case OPENDRIVE_TAG_RIGHT:
1061  break;
1062  case OPENDRIVE_TAG_LANE: {
1063  std::string type = attrs.getStringReporting(OPENDRIVE_ATTR_TYPE, myCurrentEdge.id.c_str(), ok);
1064  int id = attrs.getIntReporting(OPENDRIVE_ATTR_ID, myCurrentEdge.id.c_str(), ok);
1065  std::string level = attrs.hasAttribute(OPENDRIVE_ATTR_LEVEL)
1067  : "";
1069  ls.lanesByDir[myCurrentLaneDirection].push_back(OpenDriveLane(id, level, type));
1070  }
1071  break;
1072  case OPENDRIVE_TAG_SIGNAL: {
1073  int id = attrs.getIntReporting(OPENDRIVE_ATTR_ID, myCurrentEdge.id.c_str(), ok);
1074  std::string type = attrs.getStringReporting(OPENDRIVE_ATTR_TYPE, myCurrentEdge.id.c_str(), ok);
1075  int orientation = attrs.getStringReporting(OPENDRIVE_ATTR_ORIENTATION, myCurrentEdge.id.c_str(), ok) == "-" ? -1 : 1;
1077  bool dynamic = attrs.getStringReporting(OPENDRIVE_ATTR_DYNAMIC, myCurrentEdge.id.c_str(), ok) == "no" ? false : true;
1078  myCurrentEdge.signals.push_back(OpenDriveSignal(id, type, orientation, dynamic, s));
1079  }
1080  break;
1081  default:
1082  break;
1083  }
1084  myElementStack.push_back(element);
1085 }
1086 
1087 
1088 void
1090  myElementStack.pop_back();
1091  switch (element) {
1092  case OPENDRIVE_TAG_ROAD:
1093  if (myCurrentEdge.junction == "" || myCurrentEdge.junction == "-1") {
1094  myOuterEdges.push_back(myCurrentEdge);
1095  } else {
1096  myInnerEdges.push_back(myCurrentEdge);
1097  }
1098  break;
1099  default:
1100  break;
1101  }
1102 }
1103 
1104 
1105 
1106 void
1107 NIImporter_OpenDrive::addLink(LinkType lt, const std::string& elementType,
1108  const std::string& elementID,
1109  const std::string& contactPoint) {
1110  OpenDriveLink l(lt, elementID);
1111  // elementType
1112  if (elementType == "road") {
1114  } else if (elementType == "junction") {
1116  }
1117  // contact point
1118  if (contactPoint == "start") {
1120  } else if (contactPoint == "end") {
1122  }
1123  // add
1124  myCurrentEdge.links.push_back(l);
1125 }
1126 
1127 
1128 void
1129 NIImporter_OpenDrive::addGeometryShape(GeometryType type, const std::vector<SUMOReal>& vals) {
1130  // checks
1131  if (myCurrentEdge.geometries.size() == 0) {
1132  throw ProcessError("Mismatching paranthesis in geometry definition for road '" + myCurrentEdge.id + "'");
1133  }
1135  if (last.type != OPENDRIVE_GT_UNKNOWN) {
1136  throw ProcessError("Double geometry information for road '" + myCurrentEdge.id + "'");
1137  }
1138  // set
1139  last.type = type;
1140  last.params = vals;
1141 }
1142 
1143 
1144 /****************************************************************************/
1145