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>
40 #include <netbuild/NBEdge.h>
41 #include <netbuild/NBEdgeCont.h>
42 #include <netbuild/NBNode.h>
43 #include <netbuild/NBNodeCont.h>
44 #include <netbuild/NBNetBuilder.h>
50 #include <utils/xml/XMLSubSys.h>
51 #include <utils/geom/Boundary.h>
52 #include "NILoader.h"
53 #include "NIImporter_OpenDrive.h"
54 
55 #ifdef CHECK_MEMORY_LEAKS
56 #include <foreign/nvwa/debug_new.h>
57 #endif // CHECK_MEMORY_LEAKS
58 
59 
60 // ===========================================================================
61 // definitions
62 // ===========================================================================
63 #define C_LENGTH 10.
64 
65 
66 // ===========================================================================
67 // static variables
68 // ===========================================================================
84 
86 };
87 
88 
111 
113 };
114 
115 
116 // ===========================================================================
117 // method definitions
118 // ===========================================================================
119 // ---------------------------------------------------------------------------
120 // static methods (interface in this case)
121 // ---------------------------------------------------------------------------
122 void
124  // check whether the option is set (properly)
125  if (!oc.isUsableFileList("opendrive-files")) {
126  return;
127  }
128  // build the handler
129  std::vector<OpenDriveEdge> innerEdges, outerEdges;
130  NIImporter_OpenDrive handler(innerEdges, outerEdges);
131  // parse file(s)
132  std::vector<std::string> files = oc.getStringVector("opendrive-files");
133  for (std::vector<std::string>::const_iterator file = files.begin(); file != files.end(); ++file) {
134  if (!FileHelpers::exists(*file)) {
135  WRITE_ERROR("Could not open opendrive file '" + *file + "'.");
136  return;
137  }
138  handler.setFileName(*file);
139  PROGRESS_BEGIN_MESSAGE("Parsing opendrive from '" + *file + "'");
140  XMLSubSys::runParser(handler, *file);
142  }
143  // convert geometries into a discretised representation
144  computeShapes(innerEdges);
145  computeShapes(outerEdges);
146 
147  // -------------------------
148  // node building
149  // -------------------------
150  // build nodes#1
151  // look at all links which belong to a node, collect their bounding boxes
152  // and place the node in the middle of this bounding box
153  std::map<std::string, Boundary> posMap;
154  std::map<std::string, std::string> edge2junction;
155  // compute node positions
156  for (std::vector<OpenDriveEdge>::iterator i = innerEdges.begin(); i != innerEdges.end(); ++i) {
157  OpenDriveEdge& e = *i;
158  assert(e.junction != "-1" && e.junction != "");
159  edge2junction[e.id] = e.junction;
160  if (posMap.find(e.junction) == posMap.end()) {
161  posMap[e.junction] = Boundary();
162  }
163  posMap[e.junction].add(e.geom.getBoxBoundary());
164  }
165  // build nodes
166  for (std::map<std::string, Boundary>::iterator i = posMap.begin(); i != posMap.end(); ++i) {
167  if (!nb.getNodeCont().insert((*i).first, (*i).second.getCenter())) {
168  throw ProcessError("Could not add node '" + (*i).first + "'.");
169  }
170  }
171  // assign built nodes
172  for (std::vector<OpenDriveEdge>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
173  OpenDriveEdge& e = *i;
174  for (std::vector<OpenDriveLink>::iterator j = e.links.begin(); j != e.links.end(); ++j) {
175  OpenDriveLink& l = *j;
176  if (l.elementType != OPENDRIVE_ET_ROAD) {
177  // set node information
179  continue;
180  }
181  if (edge2junction.find(l.elementID) != edge2junction.end()) {
182  // set node information of an internal road
183  setNodeSecure(nb.getNodeCont(), e, edge2junction[l.elementID], l.linkType);
184  continue;
185  }
186  }
187  }
188  // we should now have all nodes set for links which are not outer edge-to-outer edge links
189 
190 
191  // build nodes#2
192  // build nodes for all outer edge-to-outer edge connections
193  for (std::vector<OpenDriveEdge>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
194  OpenDriveEdge& e = *i;
195  for (std::vector<OpenDriveLink>::iterator j = e.links.begin(); j != e.links.end(); ++j) {
196  OpenDriveLink& l = *j;
197  if (l.elementType != OPENDRIVE_ET_ROAD || edge2junction.find(l.elementID) != edge2junction.end()) {
198  // is a connection to an internal edge, or a node, skip
199  continue;
200  }
201  // we have a direct connection between to external edges
202  std::string id1 = e.id;
203  std::string id2 = l.elementID;
204  if (id1 < id2) {
205  std::swap(id1, id2);
206  }
207  std::string nid = id1 + "." + id2;
208  if (nb.getNodeCont().retrieve(nid) == 0) {
209  // not yet seen, build
210  Position pos = l.linkType == OPENDRIVE_LT_SUCCESSOR ? e.geom[(int)e.geom.size() - 1] : e.geom[0];
211  if (!nb.getNodeCont().insert(nid, pos)) {
212  throw ProcessError("Could not build node '" + nid + "'.");
213  }
214  }
215  /* debug-stuff
216  else {
217  Position pos = l.linkType==OPENDRIVE_LT_SUCCESSOR ? e.geom[e.geom.size()-1] : e.geom[0];
218  cout << nid << " " << pos << " " << nb.getNodeCont().retrieve(nid)->getPosition() << endl;
219  }
220  */
221  setNodeSecure(nb.getNodeCont(), e, nid, l.linkType);
222  }
223  }
224  // we should now have start/end nodes for all outer edge-to-outer edge connections
225 
226 
227  // build nodes#3
228  // assign further nodes generated from inner-edges
229  // these nodes have not been assigned earlier, because the connectiosn are referenced in inner-edges
230  for (std::vector<OpenDriveEdge>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
231  OpenDriveEdge& e = *i;
232  if (e.to != 0 && e.from != 0) {
233  continue;
234  }
235  for (std::vector<OpenDriveEdge>::iterator j = innerEdges.begin(); j != innerEdges.end(); ++j) {
236  OpenDriveEdge& ie = *j;
237  for (std::vector<OpenDriveLink>::iterator k = ie.links.begin(); k != ie.links.end(); ++k) {
238  OpenDriveLink& il = *k;
239  if (il.elementType != OPENDRIVE_ET_ROAD || il.elementID != e.id) {
240  // not conneted to the currently investigated outer edge
241  continue;
242  }
243  std::string nid = edge2junction[ie.id];
244  if (il.contactPoint == OPENDRIVE_CP_START) {
246  } else {
248  }
249  }
250  }
251 
252  }
253  //
254 
255 
256  // build start/end nodes which were not defined previously
257  for (std::vector<OpenDriveEdge>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
258  OpenDriveEdge& e = *i;
259  if (e.from == 0) {
260  std::string nid = e.id + ".begin";
261  Position pos(e.geometries[0].x, e.geometries[0].y);
262  e.from = getOrBuildNode(nid, e.geom[0], nb.getNodeCont());
263  }
264  if (e.to == 0) {
265  std::string nid = e.id + ".end";
266  Position pos(e.geometries[e.geometries.size() - 1].x, e.geometries[e.geometries.size() - 1].y);
267  e.to = getOrBuildNode(nid, e.geom[(int)e.geom.size() - 1], nb.getNodeCont());
268  }
269  }
270 
271 
272  // -------------------------
273  // edge building
274  // -------------------------
275  std::map<NBEdge*, std::map<int, int> > fromLaneMap;
276  std::map<NBEdge*, std::map<int, int> > toLaneMap;
277  // build edges
278  for (std::vector<OpenDriveEdge>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
279  OpenDriveEdge& e = *i;
280  SUMOReal speed = nb.getTypeCont().getSpeed("");
281  int priority = nb.getTypeCont().getPriority("");
283  unsigned int noLanesRight = e.getMaxLaneNumber(OPENDRIVE_TAG_RIGHT);
284  unsigned int noLanesLeft = e.getMaxLaneNumber(OPENDRIVE_TAG_LEFT);
285  if (noLanesRight != 0 || noLanesLeft != 0) {
286  lsf = LANESPREAD_RIGHT;
287  } else {
288  WRITE_WARNING("Edge '" + e.id + "' has no lanes.");
289  }
290  if (noLanesRight > 0) {
291  NBEdge* nbe = new NBEdge("-" + e.id, e.from, e.to, "", speed, noLanesRight, priority, -1, -1, e.geom, "", lsf, true);
292  if (!nb.getEdgeCont().insert(nbe)) {
293  throw ProcessError("Could not add edge '" + std::string("-") + e.id + "'.");
294  }
295  fromLaneMap[nbe] = e.laneSections.back().buildLaneMapping(OPENDRIVE_TAG_RIGHT);
296  toLaneMap[nbe] = e.laneSections[0].buildLaneMapping(OPENDRIVE_TAG_RIGHT);
297  }
298  if (noLanesLeft > 0) {
299  NBEdge* nbe = new NBEdge(e.id, e.to, e.from, "", speed, noLanesLeft, priority, -1, -1, e.geom.reverse(), "", lsf, true);
300  if (!nb.getEdgeCont().insert(nbe)) {
301  throw ProcessError("Could not add edge '" + e.id + "'.");
302  }
303  fromLaneMap[nbe] = e.laneSections[0].buildLaneMapping(OPENDRIVE_TAG_LEFT);
304  toLaneMap[nbe] = e.laneSections.back().buildLaneMapping(OPENDRIVE_TAG_LEFT);
305  }
306  }
307 
308 
309  // -------------------------
310  // connections building
311  // -------------------------
312  std::vector<Connection> connections;
313  // connections#1
314  // build connections between outer-edges
315  for (std::vector<OpenDriveEdge>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
316  OpenDriveEdge& e = *i;
317  for (std::vector<OpenDriveLink>::iterator j = e.links.begin(); j != e.links.end(); ++j) {
318  OpenDriveLink& l = *j;
319  if (l.elementType != OPENDRIVE_ET_ROAD) {
320  // we are not interested in connections to nodes
321  continue;
322  }
323  if (edge2junction.find(l.elementID) != edge2junction.end()) {
324  // connection via an inner-road
326  nb.getNodeCont().retrieve(edge2junction[l.elementID]),
327  e, l.linkType, l.elementID, connections);
328  } else {
329  // connection between two outer-edges; can be used directly
330  std::vector<OpenDriveEdge>::iterator p = std::find_if(outerEdges.begin(), outerEdges.end(), edge_by_id_finder(l.elementID));
331  if (p == outerEdges.end()) {
332  throw ProcessError("Could not find connection edge.");
333  }
334  std::string id1 = e.id;
335  std::string id2 = (*p).id;
336  if (id1 < id2) {
337  std::swap(id1, id2);
338  }
339  std::string nid = id1 + "." + id2;
341  addE2EConnectionsSecure(nb.getEdgeCont(), nb.getNodeCont().retrieve(nid), *p, e, connections);
342  } else {
343  addE2EConnectionsSecure(nb.getEdgeCont(), nb.getNodeCont().retrieve(nid), e, *p, connections);
344  }
345  }
346  }
347  }
348 
349  // build inner-edge connections
350  for (std::vector<OpenDriveEdge>::iterator i = innerEdges.begin(); i != innerEdges.end(); ++i) {
351  OpenDriveEdge& e = *i;
352  std::string pred, succ;
353  ContactPoint predC, succC;
354  for (std::vector<OpenDriveLink>::iterator j = e.links.begin(); j != e.links.end(); ++j) {
355  OpenDriveLink& l = *j;
356  if (l.elementType != OPENDRIVE_ET_ROAD) {
357  // we are not interested in connections to nodes
358  std::cout << "unsupported" << std::endl;
359  continue;
360  }
361  if (edge2junction.find(l.elementID) != edge2junction.end()) {
362  // not supported
363  std::cout << "unsupported" << std::endl;
364  continue;
365  }
366  if (l.linkType == OPENDRIVE_LT_SUCCESSOR) {
367  if (succ != "") {
368  std::cout << "double succ" << std::endl;
369  }
370  succ = l.elementID;
371  succC = l.contactPoint;
372  } else {
373  if (pred != "") {
374  std::cout << "double pred" << std::endl;
375  }
376  pred = l.elementID;
377  predC = l.contactPoint;
378  }
379  }
380 
382  std::cout << "Both dirs given!" << std::endl;
383  }
384 
385  bool isReversed = false;
386  if (e.getMaxLaneNumber(OPENDRIVE_TAG_LEFT) != 0) {
387  // std::swap(pred, succ);
388  //std::swap(predC, succC);
389  isReversed = true;
390  }
391 
392  if (succ == "" || pred == "") {
393  std::cout << "Missing edge." << std::endl;
394  continue; // yes, occurs
395  }
396  // !!!why edge2junction; e already has the junction...
397  NBNode* n = nb.getNodeCont().retrieve(edge2junction[e.id]);
398  std::vector<OpenDriveEdge>::iterator predEdge = std::find_if(outerEdges.begin(), outerEdges.end(), edge_by_id_finder(pred));
399  if (predEdge == outerEdges.end()) {
400  throw ProcessError("Could not find connection edge.");
401  }
402  std::vector<OpenDriveEdge>::iterator succEdge = std::find_if(outerEdges.begin(), outerEdges.end(), edge_by_id_finder(succ));
403  if (succEdge == outerEdges.end()) {
404  throw ProcessError("Could not find connection edge.");
405  }
406  NBEdge* fromEdge, *toEdge;
407  if (!isReversed) {
408  fromEdge = predC == OPENDRIVE_CP_END ? nb.getEdgeCont().retrieve("-" + pred) : nb.getEdgeCont().retrieve(pred);
409  toEdge = succC == OPENDRIVE_CP_START ? nb.getEdgeCont().retrieve("-" + succ) : nb.getEdgeCont().retrieve(succ);
410  } else {
411  fromEdge = predC != OPENDRIVE_CP_END ? nb.getEdgeCont().retrieve("-" + pred) : nb.getEdgeCont().retrieve(pred);
412  toEdge = succC != OPENDRIVE_CP_START ? nb.getEdgeCont().retrieve("-" + succ) : nb.getEdgeCont().retrieve(succ);
413  }
414  /*
415  Connection c(
416  n->hasIncoming(nb.getEdgeCont().retrieve("-" + pred)) ? nb.getEdgeCont().retrieve("-" + pred) : nb.getEdgeCont().retrieve(pred),
417  e.id,
418  n->hasOutgoing(nb.getEdgeCont().retrieve("-" + succ)) ? nb.getEdgeCont().retrieve("-" + succ) : nb.getEdgeCont().retrieve(succ));
419  */
420  Connection c(fromEdge, e.id, toEdge);
421  if (c.from == 0 || c.to == 0 || c.from == c.to) {
422  throw ProcessError("Something's false");
423  }
425  *predEdge, c.from->getID()[0] != '-', c.from->getID()[0] == '-' ? OPENDRIVE_TAG_RIGHT : OPENDRIVE_TAG_LEFT,
426  e, isReversed, !isReversed ? OPENDRIVE_TAG_RIGHT : OPENDRIVE_TAG_LEFT,
427  *succEdge, c.to->getID()[0] != '-', c.to->getID()[0] == '-' ? OPENDRIVE_TAG_RIGHT : OPENDRIVE_TAG_LEFT);
428  connections.push_back(c);
429  }
430  for (std::vector<Connection>::const_iterator i = connections.begin(); i != connections.end(); ++i) {
431  if ((*i).from == 0 || (*i).to == 0) {
432  std::cout << "Nope." << std::endl;
433  continue;
434  }
435  (*i).from->addEdge2EdgeConnection((*i).to);
436  std::map<int, int> fromMap = fromLaneMap[(*i).from];
437  std::map<int, int> toMap = fromLaneMap[(*i).to];
438  for (std::vector<std::pair<int, int> >::const_iterator j = (*i).lanes.begin(); j != (*i).lanes.end(); ++j) {
439  int fromLane = fromMap[(*j).first];
440  int toLane = toMap[(*j).second];
441  if (static_cast<unsigned int>(fromLane) >= (*i).from->getNumLanes() || fromLane < 0) {
442  std::cout << "False " << std::endl;
443  }
444  if (static_cast<unsigned int>(toLane) >= (*i).to->getNumLanes() || toLane < 0) {
445  std::cout << "False " << std::endl;
446  }
447 
448  (*i).from->addLane2LaneConnection(fromLane, (*i).to, toLane, NBEdge::L2L_VALIDATED, true);
449  }
450  }
451 }
452 
453 
454 void
456  const NBNode* const node, const OpenDriveEdge& e,
457  LinkType lt, const std::string& via,
458  std::vector<NIImporter_OpenDrive::Connection> &connections) {
459  NBEdge* from = 0;
460  NBEdge* to = 0;
461  if (node == e.to) {
462  // the connection is at the end of the "positive" direction
463  if (lt == OPENDRIVE_LT_PREDECESSOR) {
464  // via -> edge
465  to = ec.retrieve(e.id);
466  } else {
467  // -edge -> via
468  // "ambigous?"
469  from = ec.retrieve("-" + e.id);
470  }
471  } else {
472  // the connection is at the begin of the "positive" direction
473  if (lt == OPENDRIVE_LT_PREDECESSOR) {
474  // via -> -edge
475  to = ec.retrieve("-" + e.id);
476  } else {
477  // edge -> via
478  // "ambigous?"
479  from = ec.retrieve(e.id);
480  }
481  }
482  if (from == 0 && to == 0) {
483  throw ProcessError("Missing edge");
484  }
485  Connection c(from, via, to);
486  connections.push_back(c);
487 }
488 
489 
490 void
493  std::vector<NIImporter_OpenDrive::Connection> &connections) {
494  // positive direction (from is incoming, to is outgoing)
495  NBEdge* fromEdge = ec.retrieve("-" + from.id);
496  if (fromEdge == 0 || !node->hasIncoming(fromEdge)) {
497  fromEdge = ec.retrieve(from.id);
498  }
499  NBEdge* toEdge = ec.retrieve("-" + to.id);
500  if (toEdge == 0 || !node->hasOutgoing(toEdge)) {
501  toEdge = ec.retrieve(to.id);
502  }
503  if (fromEdge != 0 && toEdge != 0) {
504  Connection c(fromEdge, "", toEdge);
506  from, c.from->getID()[0] != '-', c.from->getID()[0] == '-' ? OPENDRIVE_TAG_RIGHT : OPENDRIVE_TAG_LEFT,
507  to, c.to->getID()[0] != '-', c.to->getID()[0] == '-' ? OPENDRIVE_TAG_RIGHT : OPENDRIVE_TAG_LEFT);
508  connections.push_back(c);
509  }
510  // negative direction (to is incoming, from is outgoing)
511  fromEdge = ec.retrieve("-" + from.id);
512  if (fromEdge == 0 || !node->hasOutgoing(fromEdge)) {
513  fromEdge = ec.retrieve(from.id);
514  }
515  toEdge = ec.retrieve("-" + to.id);
516  if (toEdge == 0 || !node->hasIncoming(toEdge)) {
517  toEdge = ec.retrieve(to.id);
518  }
519  if (fromEdge != 0 && toEdge != 0) {
520  Connection c(toEdge, "", fromEdge);
522  to, c.to->getID()[0] != '-', c.to->getID()[0] == '-' ? OPENDRIVE_TAG_RIGHT : OPENDRIVE_TAG_LEFT,
523  from, c.from->getID()[0] != '-', c.from->getID()[0] == '-' ? OPENDRIVE_TAG_RIGHT : OPENDRIVE_TAG_LEFT);
524  connections.push_back(c);
525  }
526 }
527 
528 
529 void
531  const OpenDriveEdge& from, bool fromAtBegin, OpenDriveXMLTag fromLaneDir,
532  const OpenDriveEdge& to, bool toAtEnd, OpenDriveXMLTag toLaneDir) {
533  const OpenDriveLaneSection& fromLS = fromAtBegin ? from.laneSections[0] : from.laneSections.back();
534  const std::vector<OpenDriveLane> &fromLanes = fromLS.lanesByDir.find(fromLaneDir)->second;
535  const OpenDriveLaneSection& toLS = toAtEnd ? to.laneSections.back() : to.laneSections[0];
536  const std::vector<OpenDriveLane> &toLanes = toLS.lanesByDir.find(toLaneDir)->second;
537  // in the following, we are probably using the same information twice, stored once
538  // in the from-edge's successor field and in the to-edge's precessor field.
539  // though, we have no proof or information that this is always redundant
540  for (std::vector<OpenDriveLane>::const_iterator i = fromLanes.begin(); i != fromLanes.end(); ++i) {
541  if ((*i).type != "driving") {
542  continue;
543  }
544  if (!fromAtBegin && (*i).successor != UNSET_CONNECTION) {
545  c.lanes.push_back(std::make_pair((*i).id, (*i).successor));
546  }
547  if (fromAtBegin && (*i).predecessor != UNSET_CONNECTION) {
548  c.lanes.push_back(std::make_pair((*i).id, (*i).predecessor));
549  }
550  }
551  for (std::vector<OpenDriveLane>::const_iterator i = toLanes.begin(); i != toLanes.end(); ++i) {
552  if ((*i).type != "driving") {
553  continue;
554  }
555  if (!toAtEnd && (*i).predecessor != UNSET_CONNECTION) {
556  c.lanes.push_back(std::make_pair((*i).predecessor, (*i).id));
557  }
558  if (toAtEnd && (*i).successor != UNSET_CONNECTION) {
559  c.lanes.push_back(std::make_pair((*i).successor, (*i).id));
560  }
561  }
562 }
563 
564 
565 void
567  const OpenDriveEdge& from, bool fromAtBegin, OpenDriveXMLTag fromLaneDir,
568  const OpenDriveEdge& via, bool viaIsReversed, OpenDriveXMLTag viaLaneDir,
569  const OpenDriveEdge& to, bool toAtEnd, OpenDriveXMLTag toLaneDir) {
570  Connection from2via(0, "", 0);
571  setLaneConnections(from2via, from, fromAtBegin, fromLaneDir, via, viaIsReversed, viaLaneDir);
572  Connection via2to(0, "", 0);
573  setLaneConnections(via2to, via, viaIsReversed, viaLaneDir, to, toAtEnd, toLaneDir);
574  for (std::vector<std::pair<int, int> >::const_iterator i = from2via.lanes.begin(); i != from2via.lanes.end(); ++i) {
575  int fromLane = (*i).first;
576  int viaLane = (*i).second;
577  for (std::vector<std::pair<int, int> >::const_iterator j = via2to.lanes.begin(); j != via2to.lanes.end(); ++j) {
578  if ((*j).first == viaLane) {
579  c.lanes.push_back(std::make_pair(fromLane, (*j).second));
580  break;
581  }
582  }
583  }
584 }
585 
586 
587 NBNode*
588 NIImporter_OpenDrive::getOrBuildNode(const std::string& id, Position& pos,
589  NBNodeCont& nc) {
590  if (nc.retrieve(id) == 0) {
591  // not yet built; build now
592  if (!nc.insert(id, pos)) {
593  // !!! clean up
594  throw ProcessError("Could not add node '" + id + "'.");
595  }
596  }
597  return nc.retrieve(id);
598 }
599 
600 
601 void
603  const std::string& nodeID, NIImporter_OpenDrive::LinkType lt) {
604  NBNode* n = nc.retrieve(nodeID);
605  if (n == 0) {
606  throw ProcessError("Could not find node '" + nodeID + "'.");
607  }
608  if (lt == OPENDRIVE_LT_SUCCESSOR) {
609  if (e.to != 0 && e.to != n) {
610  throw ProcessError("Edge '" + e.id + "' has two end nodes.");
611  }
612  e.to = n;
613  } else {
614  if (e.from != 0 && e.from != n) {
615  throw ProcessError("Edge '" + e.id + "' has two start nodes.");
616  }
617  e.from = n;
618  }
619 }
620 
621 
622 
623 
624 
625 NBEdge*
627  const NBNodeCont& nc,
628  const std::string& edgeID,
629  const std::string& nodeID) {
630  NBNode* node = nc.retrieve(nodeID);
631  NBEdge* e = ec.retrieve(edgeID);
632  if (e != 0 && node->hasOutgoing(e)) {
633  return e;
634  }
635  e = ec.retrieve("-" + edgeID);
636  if (e != 0 && node->hasOutgoing(e)) {
637  return e;
638  }
639  return 0;
640 }
641 
642 
643 NBEdge*
645  const NBNodeCont& nc,
646  const std::string& edgeID,
647  const std::string& nodeID) {
648  NBNode* node = nc.retrieve(nodeID);
649  NBEdge* e = ec.retrieve(edgeID);
650  if (e != 0 && node->hasIncoming(e)) {
651  return e;
652  }
653  e = ec.retrieve("-" + edgeID);
654  if (e != 0 && node->hasIncoming(e)) {
655  return e;
656  }
657  return 0;
658 }
659 
660 
661 void
662 NIImporter_OpenDrive::computeShapes(std::vector<OpenDriveEdge> &edges) {
663  for (std::vector<OpenDriveEdge>::iterator i = edges.begin(); i != edges.end(); ++i) {
664  OpenDriveEdge& e = *i;
665  for (std::vector<OpenDriveGeometry>::iterator j = e.geometries.begin(); j != e.geometries.end(); ++j) {
666  OpenDriveGeometry& g = *j;
667  std::vector<Position> geom;
668  switch (g.type) {
670  break;
671  case OPENDRIVE_GT_LINE:
672  geom = geomFromLine(e, g);
673  break;
674  case OPENDRIVE_GT_ARC:
675  geom = geomFromArc(e, g);
676  break;
677  case OPENDRIVE_GT_POLY3:
678  geom = geomFromPoly(e, g);
679  break;
680  default:
681  break;
682  }
683  for (std::vector<Position>::iterator k = geom.begin(); k != geom.end(); ++k) {
685  }
686  }
687  for (unsigned int j = 0; j < e.geom.size(); ++j) {
689  WRITE_ERROR("Unable to project coordinates for.");
690  }
691  }
692  }
693 }
694 
695 std::vector<Position>
697  UNUSED_PARAMETER(e);
698  std::vector<Position> ret;
699  ret.push_back(Position(g.x, g.y));
700  ret.push_back(calculateStraightEndPoint(g.hdg, g.length, Position(g.x, g.y)));
701  return ret;
702 }
703 
704 
705 std::vector<Position>
707  UNUSED_PARAMETER(e);
708  std::vector<Position> ret;
709  SUMOReal dist = 0.0;
710  SUMOReal centerX = g.x;
711  SUMOReal centerY = g.y;
712  // left: positive value
713  SUMOReal curvature = g.params[0];
714  SUMOReal radius = 1. / curvature;
715  // center point
716  calculateCurveCenter(&centerX, &centerY, radius, g.hdg);
717  SUMOReal endX = g.x;
718  SUMOReal endY = g.y;
719  SUMOReal startX = g.x;
720  SUMOReal startY = g.y;
721  SUMOReal hdgS = g.hdg;
722  SUMOReal hdgE;
723  SUMOReal geo_posS = g.s;
724  SUMOReal geo_posE = g.s;
725  bool end = false;
726  do {
727  geo_posE += C_LENGTH;
728  if (geo_posE - g.s > g.length) {
729  geo_posE = g.s + g.length;
730  }
731  if (geo_posE - g.s > g.length) {
732  geo_posE = g.s + g.length;
733  }
734  calcPointOnCurve(&endX, &endY, centerX, centerY, radius, geo_posE - geo_posS);
735 
736  dist += (geo_posE - geo_posS);
737  if (curvature > 0.0) {
738  hdgE = g.hdg + dist / fabs(radius);
739  } else {
740  hdgE = g.hdg - dist / fabs(radius);
741  }
742  //
743  ret.push_back(Position(startX, startY));
744  //
745  startX = endX;
746  startY = endY;
747  geo_posS = geo_posE;
748  hdgS = hdgE;
749 
750  if (geo_posE - (g.s + g.length) < 0.001 && geo_posE - (g.s + g.length) > -0.001) {
751  end = true;
752  }
753  } while (!end);
754  return ret;
755 }
756 
757 
758 std::vector<Position>
760  UNUSED_PARAMETER(g);
761  UNUSED_PARAMETER(e);
762  std::vector<Position> ret;
763  return ret;
764 }
765 
766 
767 Position
768 NIImporter_OpenDrive::calculateStraightEndPoint(double hdg, double length, const Position& start) {
769  double normx = 1.0f;
770  double normy = 0.0f;
771  double x2 = normx * cos(hdg) - normy * sin(hdg);
772  double y2 = normx * sin(hdg) + normy * cos(hdg);
773  normx = x2 * length;
774  normy = y2 * length;
775  return Position(start.x() + normx, start.y() + normy);
776 }
777 
778 
779 void
781  SUMOReal normX = 1.0;
782  SUMOReal normY = 0.0;
783  SUMOReal tmpX;
784  SUMOReal turn;
785  if (ad_radius > 0) {
786  turn = -1.0;
787  } else {
788  turn = 1.0;
789  }
790 
791  tmpX = normX;
792  normX = normX * cos(ad_hdg) + normY * sin(ad_hdg);
793  normY = tmpX * sin(ad_hdg) + normY * cos(ad_hdg);
794 
795  tmpX = normX;
796  normX = normX * cos(90 * PI / 180) + turn * normY * sin(90 * PI / 180);
797  normY = -1 * turn * tmpX * sin(90 * PI / 180) + normY * cos(90 * PI / 180);
798 
799  normX = fabs(ad_radius) * normX;
800  normY = fabs(ad_radius) * normY;
801 
802  *ad_x += normX;
803  *ad_y += normY;
804 }
805 
806 
807 void
809  SUMOReal ad_r, SUMOReal ad_length) {
810  double rotAngle = ad_length / fabs(ad_r);
811  double vx = *ad_x - ad_centerX;
812  double vy = *ad_y - ad_centerY;
813  double tmpx;
814 
815  double turn;
816  if (ad_r > 0) {
817  turn = -1; //left
818  } else {
819  turn = 1; //right
820  }
821  tmpx = vx;
822  vx = vx * cos(rotAngle) + turn * vy * sin(rotAngle);
823  vy = -1 * turn * tmpx * sin(rotAngle) + vy * cos(rotAngle);
824  *ad_x = vx + ad_centerX;
825  *ad_y = vy + ad_centerY;
826 }
827 
828 // ---------------------------------------------------------------------------
829 // loader methods
830 // ---------------------------------------------------------------------------
832  std::vector<OpenDriveEdge> &innerEdges,
833  std::vector<OpenDriveEdge> &outerEdges)
834  : GenericSAXHandler(openDriveTags, OPENDRIVE_TAG_NOTHING, openDriveAttrs, OPENDRIVE_ATTR_NOTHING, "opendrive"),
835  myCurrentEdge("", "", -1), myInnerEdges(innerEdges), myOuterEdges(outerEdges) {
836 }
837 
838 
840 }
841 
842 
843 void
845  const SUMOSAXAttributes& attrs) {
846  bool ok = true;
847  switch (element) {
848  case OPENDRIVE_TAG_HEADER: {
849  int majorVersion = attrs.getIntReporting(OPENDRIVE_ATTR_REVMAJOR, 0, ok);
850  int minorVersion = attrs.getIntReporting(OPENDRIVE_ATTR_REVMINOR, 0, ok);
851  if (majorVersion != 1 || minorVersion != 2) {
852  WRITE_WARNING("Given openDrive file '" + getFileName() + "' uses version " + toString(majorVersion) + "." + toString(minorVersion) + ";\n Version 1.2 is supported.");
853  }
854  }
855  break;
856  case OPENDRIVE_TAG_ROAD: {
857  std::string id = attrs.getStringReporting(OPENDRIVE_ATTR_ID, 0, ok);
858  std::string junction = attrs.getStringReporting(OPENDRIVE_ATTR_JUNCTION, id.c_str(), ok);
859  SUMOReal length = attrs.getSUMORealReporting(OPENDRIVE_ATTR_LENGTH, id.c_str(), ok);
860  myCurrentEdge = OpenDriveEdge(id, junction, length);
861  }
862  break;
864  if (myElementStack.size() >= 2 && myElementStack[myElementStack.size() - 2] == OPENDRIVE_TAG_ROAD) {
865  std::string elementType = attrs.getStringReporting(OPENDRIVE_ATTR_ELEMENTTYPE, myCurrentEdge.id.c_str(), ok);
866  std::string elementID = attrs.getStringReporting(OPENDRIVE_ATTR_ELEMENTID, myCurrentEdge.id.c_str(), ok);
867  std::string contactPoint = attrs.hasAttribute(OPENDRIVE_ATTR_CONTACTPOINT)
869  : "end";
870  addLink(OPENDRIVE_LT_PREDECESSOR, elementType, elementID, contactPoint);
871  }
872  if (myElementStack.size() >= 2 && myElementStack[myElementStack.size() - 2] == OPENDRIVE_TAG_LANE) {
873  int no = attrs.getIntReporting(OPENDRIVE_ATTR_ID, myCurrentEdge.id.c_str(), ok);
875  l.predecessor = no;
876  }
877  }
878  break;
880  if (myElementStack.size() >= 2 && myElementStack[myElementStack.size() - 2] == OPENDRIVE_TAG_ROAD) {
881  std::string elementType = attrs.getStringReporting(OPENDRIVE_ATTR_ELEMENTTYPE, myCurrentEdge.id.c_str(), ok);
882  std::string elementID = attrs.getStringReporting(OPENDRIVE_ATTR_ELEMENTID, myCurrentEdge.id.c_str(), ok);
883  std::string contactPoint = attrs.hasAttribute(OPENDRIVE_ATTR_CONTACTPOINT)
885  : "start";
886  addLink(OPENDRIVE_LT_SUCCESSOR, elementType, elementID, contactPoint);
887  }
888  if (myElementStack.size() >= 2 && myElementStack[myElementStack.size() - 2] == OPENDRIVE_TAG_LANE) {
889  int no = attrs.getIntReporting(OPENDRIVE_ATTR_ID, myCurrentEdge.id.c_str(), ok);
891  l.successor = no;
892  }
893  }
894  break;
895  case OPENDRIVE_TAG_GEOMETRY: {
901  myCurrentEdge.geometries.push_back(OpenDriveGeometry(length, s, x, y, hdg));
902  }
903  break;
904  case OPENDRIVE_TAG_LINE: {
905  std::vector<SUMOReal> vals;
907  }
908  break;
909  case OPENDRIVE_TAG_SPIRAL: {
910  std::vector<SUMOReal> vals;
911  vals.push_back(attrs.getSUMORealReporting(OPENDRIVE_ATTR_CURVSTART, myCurrentEdge.id.c_str(), ok));
912  vals.push_back(attrs.getSUMORealReporting(OPENDRIVE_ATTR_CURVEND, myCurrentEdge.id.c_str(), ok));
914  }
915  break;
916  case OPENDRIVE_TAG_ARC: {
917  std::vector<SUMOReal> vals;
918  vals.push_back(attrs.getSUMORealReporting(OPENDRIVE_ATTR_CURVATURE, myCurrentEdge.id.c_str(), ok));
920  }
921  break;
922  case OPENDRIVE_TAG_POLY3: {
923  std::vector<SUMOReal> vals;
924  vals.push_back(attrs.getSUMORealReporting(OPENDRIVE_ATTR_A, myCurrentEdge.id.c_str(), ok));
925  vals.push_back(attrs.getSUMORealReporting(OPENDRIVE_ATTR_B, myCurrentEdge.id.c_str(), ok));
926  vals.push_back(attrs.getSUMORealReporting(OPENDRIVE_ATTR_C, myCurrentEdge.id.c_str(), ok));
927  vals.push_back(attrs.getSUMORealReporting(OPENDRIVE_ATTR_D, myCurrentEdge.id.c_str(), ok));
929  }
930  break;
934  }
935  break;
936  case OPENDRIVE_TAG_LEFT:
938  break;
941  break;
942  case OPENDRIVE_TAG_RIGHT:
944  break;
945  case OPENDRIVE_TAG_LANE: {
946  std::string type = attrs.getStringReporting(OPENDRIVE_ATTR_TYPE, myCurrentEdge.id.c_str(), ok);
947  int id = attrs.getIntReporting(OPENDRIVE_ATTR_ID, myCurrentEdge.id.c_str(), ok);
948  std::string level = attrs.hasAttribute(OPENDRIVE_ATTR_LEVEL)
950  : "";
952  ls.lanesByDir[myCurrentLaneDirection].push_back(OpenDriveLane(id, level, type));
953  }
954  break;
955  default:
956  break;
957  }
958  myElementStack.push_back(element);
959 }
960 
961 
962 void
964  const std::string& chars) {
965  UNUSED_PARAMETER(element);
966  UNUSED_PARAMETER(chars);
967 }
968 
969 
970 
971 void
973  myElementStack.pop_back();
974  switch (element) {
975  case OPENDRIVE_TAG_ROAD:
976  if (myCurrentEdge.junction == "" || myCurrentEdge.junction == "-1") {
977  myOuterEdges.push_back(myCurrentEdge);
978  } else {
979  myInnerEdges.push_back(myCurrentEdge);
980  }
981  break;
982  default:
983  break;
984  }
985 }
986 
987 
988 
989 void
990 NIImporter_OpenDrive::addLink(LinkType lt, const std::string& elementType,
991  const std::string& elementID,
992  const std::string& contactPoint) {
993  OpenDriveLink l(lt, elementID);
994  // elementType
995  if (elementType == "road") {
997  } else if (elementType == "junction") {
999  }
1000  // contact point
1001  if (contactPoint == "start") {
1003  } else if (contactPoint == "end") {
1005  }
1006  // add
1007  myCurrentEdge.links.push_back(l);
1008 }
1009 
1010 
1011 void
1012 NIImporter_OpenDrive::addGeometryShape(GeometryType type, const std::vector<SUMOReal> &vals) {
1013  // checks
1014  if (myCurrentEdge.geometries.size() == 0) {
1015  throw ProcessError("Mismatching paranthesis in geometry definition for road '" + myCurrentEdge.id + "'");
1016  }
1018  if (last.type != OPENDRIVE_GT_UNKNOWN) {
1019  throw ProcessError("Double geometry information for road '" + myCurrentEdge.id + "'");
1020  }
1021  // set
1022  last.type = type;
1023  last.params = vals;
1024 }
1025 
1026 
1027 /****************************************************************************/
1028