SUMO - Simulation of Urban MObility
NWWriter_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) 2011-2017 German Aerospace Center (DLR) and others.
4 /****************************************************************************/
5 //
6 // This program and the accompanying materials
7 // are made available under the terms of the Eclipse Public License v2.0
8 // which accompanies this distribution, and is available at
9 // http://www.eclipse.org/legal/epl-v20.html
10 //
11 /****************************************************************************/
18 // Exporter writing networks using the openDRIVE format
19 /****************************************************************************/
20 
21 
22 // ===========================================================================
23 // included modules
24 // ===========================================================================
25 #ifdef _MSC_VER
26 #include <windows_config.h>
27 #else
28 #include <config.h>
29 #endif
30 
31 #include <ctime>
32 #include "NWWriter_OpenDrive.h"
35 #include <netbuild/NBEdgeCont.h>
36 #include <netbuild/NBNode.h>
37 #include <netbuild/NBNodeCont.h>
38 #include <netbuild/NBNetBuilder.h>
41 #include <utils/geom/bezier.h>
43 #include <utils/common/StdDefs.h>
46 
47 #define INVALID_ID -1
48 
49 //#define DEBUG_SMOOTH_GEOM
50 #define DEBUGCOND true
51 
52 #define MIN_TURN_DIAMETER 2.0
53 
54 
55 // ===========================================================================
56 // method definitions
57 // ===========================================================================
58 // ---------------------------------------------------------------------------
59 // static methods
60 // ---------------------------------------------------------------------------
61 void
63  // check whether an opendrive-file shall be generated
64  if (!oc.isSet("opendrive-output")) {
65  return;
66  }
67  const NBNodeCont& nc = nb.getNodeCont();
68  const NBEdgeCont& ec = nb.getEdgeCont();
69  const bool origNames = oc.getBool("output.original-names");
70  const bool lefthand = oc.getBool("lefthand");
71  const double straightThresh = DEG2RAD(oc.getFloat("opendrive-output.straight-threshold"));
72  // some internal mapping containers
73  int nodeID = 1;
74  int edgeID = nc.size() * 10; // distinct from node ids
75  StringBijection<int> edgeMap;
76  StringBijection<int> nodeMap;
77  //
78  OutputDevice& device = OutputDevice::getDevice(oc.getString("opendrive-output"));
79  device << "<?xml version=\"1.0\" encoding=\"utf-8\"?>\n";
80  device.openTag("OpenDRIVE");
81  time_t now = time(0);
82  std::string dstr(ctime(&now));
84  // write header
85  device.openTag("header");
86  device.writeAttr("revMajor", "1");
87  device.writeAttr("revMinor", "4");
88  device.writeAttr("name", "");
89  device.writeAttr("version", "1.00");
90  device.writeAttr("date", dstr.substr(0, dstr.length() - 1));
91  device.writeAttr("north", b.ymax());
92  device.writeAttr("south", b.ymin());
93  device.writeAttr("east", b.xmax());
94  device.writeAttr("west", b.xmin());
95  /* @note obsolete in 1.4
96  device.writeAttr("maxRoad", ec.size());
97  device.writeAttr("maxJunc", nc.size());
98  device.writeAttr("maxPrg", 0);
99  */
100  device.closeTag();
101 
102  // write normal edges (road)
103  for (std::map<std::string, NBEdge*>::const_iterator i = ec.begin(); i != ec.end(); ++i) {
104  const NBEdge* e = (*i).second;
105  const int fromNodeID = e->getIncomingEdges().size() > 0 ? getID(e->getFromNode()->getID(), nodeMap, nodeID) : INVALID_ID;
106  const int toNodeID = e->getConnections().size() > 0 ? getID(e->getToNode()->getID(), nodeMap, nodeID) : INVALID_ID;
107  writeNormalEdge(device, e,
108  getID(e->getID(), edgeMap, edgeID),
109  fromNodeID, toNodeID,
110  origNames, straightThresh);
111  }
112  device.lf();
113 
114  // write junction-internal edges (road). In OpenDRIVE these are called 'paths' or 'connecting roads'
115  OutputDevice_String junctionOSS(false, 3);
116  for (std::map<std::string, NBNode*>::const_iterator i = nc.begin(); i != nc.end(); ++i) {
117  NBNode* n = (*i).second;
118  int connectionID = 0; // unique within a junction
119  const int nID = getID(n->getID(), nodeMap, nodeID);
120  if (n->numNormalConnections() > 0) {
121  junctionOSS << " <junction name=\"" << n->getID() << "\" id=\"" << nID << "\">\n";
122  }
123  std::vector<NBEdge*> incoming = (*i).second->getIncomingEdges();
124  if (lefthand) {
125  std::reverse(incoming.begin(), incoming.end());
126  }
127  for (NBEdge* inEdge : incoming) {
128  std::string centerMark = "none";
129  const int inEdgeID = getID(inEdge->getID(), edgeMap, edgeID);
130  // group parallel edges
131  const NBEdge* outEdge = 0;
132  bool isOuterEdge = true; // determine where a solid outer border should be drawn
133  int lastFromLane = -1;
134  std::vector<NBEdge::Connection> parallel;
135  std::vector<NBEdge::Connection> connections = inEdge->getConnections();
136  if (lefthand) {
137  std::reverse(connections.begin(), connections.end());
138  }
139  for (const NBEdge::Connection& c : connections) {
140  assert(c.toEdge != 0);
141  if (outEdge != c.toEdge || c.fromLane == lastFromLane) {
142  if (outEdge != 0) {
143  if (isOuterEdge) {
144  addPedestrianConnection(inEdge, outEdge, parallel);
145  }
146  connectionID = writeInternalEdge(device, junctionOSS, inEdge, nID,
147  getID(parallel.back().getInternalLaneID(), edgeMap, edgeID),
148  inEdgeID,
149  getID(outEdge->getID(), edgeMap, edgeID),
150  connectionID,
151  parallel, isOuterEdge, straightThresh, centerMark);
152  parallel.clear();
153  isOuterEdge = false;
154  }
155  outEdge = c.toEdge;
156  }
157  lastFromLane = c.fromLane;
158  parallel.push_back(c);
159  }
160  if (isOuterEdge) {
161  addPedestrianConnection(inEdge, outEdge, parallel);
162  }
163  if (!parallel.empty()) {
164  if (!lefthand && (n->geometryLike() || inEdge->isTurningDirectionAt(outEdge))) {
165  centerMark = "solid";
166  }
167  connectionID = writeInternalEdge(device, junctionOSS, inEdge, nID,
168  getID(parallel.back().getInternalLaneID(), edgeMap, edgeID),
169  inEdgeID,
170  getID(outEdge->getID(), edgeMap, edgeID),
171  connectionID,
172  parallel, isOuterEdge, straightThresh, centerMark);
173  parallel.clear();
174  }
175  }
176  if (n->numNormalConnections() > 0) {
177  junctionOSS << " </junction>\n";
178  }
179  }
180  device.lf();
181  // write junctions (junction)
182  device << junctionOSS.getString();
183 
184  for (std::map<std::string, NBNode*>::const_iterator i = nc.begin(); i != nc.end(); ++i) {
185  NBNode* n = (*i).second;
186  const std::vector<NBEdge*>& incoming = n->getIncomingEdges();
187  // check if any connections must be written
188  int numConnections = 0;
189  for (std::vector<NBEdge*>::const_iterator j = incoming.begin(); j != incoming.end(); ++j) {
190  numConnections += (int)((*j)->getConnections().size());
191  }
192  if (numConnections == 0) {
193  continue;
194  }
195  for (std::vector<NBEdge*>::const_iterator j = incoming.begin(); j != incoming.end(); ++j) {
196  const NBEdge* inEdge = *j;
197  const std::vector<NBEdge::Connection>& elv = inEdge->getConnections();
198  for (std::vector<NBEdge::Connection>::const_iterator k = elv.begin(); k != elv.end(); ++k) {
199  const NBEdge::Connection& c = *k;
200  const NBEdge* outEdge = c.toEdge;
201  if (outEdge == 0) {
202  continue;
203  }
204  }
205  }
206  }
207 
208  device.closeTag();
209  device.close();
210 }
211 
212 
213 void
215  int edgeID, int fromNodeID, int toNodeID,
216  const bool origNames,
217  const double straightThresh) {
218  // buffer output because some fields are computed out of order
219  OutputDevice_String elevationOSS(false, 3);
220  elevationOSS.setPrecision(8);
221  OutputDevice_String planViewOSS(false, 2);
222  planViewOSS.setPrecision(8);
223  double length = 0;
224 
225  planViewOSS.openTag("planView");
226  // for the shape we need to use the leftmost border of the leftmost lane
227  const std::vector<NBEdge::Lane>& lanes = e->getLanes();
229 #ifdef DEBUG_SMOOTH_GEOM
230  if (DEBUGCOND) {
231  std::cout << "write planview for edge " << e->getID() << "\n";
232  }
233 #endif
234 
235  if (ls.size() == 2 || e->getPermissions() == SVC_PEDESTRIAN) {
236  // foot paths may contain sharp angles
237  length = writeGeomLines(ls, planViewOSS, elevationOSS);
238  } else {
239  bool ok = writeGeomSmooth(ls, e->getSpeed(), planViewOSS, elevationOSS, straightThresh, length);
240  if (!ok) {
241  WRITE_WARNING("Could not compute smooth shape for edge '" + e->getID() + "'.");
242  }
243  }
244  planViewOSS.closeTag();
245 
246  device.openTag("road");
247  device.writeAttr("name", StringUtils::escapeXML(e->getStreetName()));
248  device.setPrecision(8); // length requires higher precision
249  device.writeAttr("length", MAX2(POSITION_EPS, length));
250  device.setPrecision(gPrecision);
251  device.writeAttr("id", edgeID);
252  device.writeAttr("junction", -1);
253  if (fromNodeID != INVALID_ID || toNodeID != INVALID_ID) {
254  device.openTag("link");
255  if (fromNodeID != INVALID_ID) {
256  device.openTag("predecessor");
257  device.writeAttr("elementType", "junction");
258  device.writeAttr("elementId", fromNodeID);
259  device.closeTag();
260  }
261  if (toNodeID != INVALID_ID) {
262  device.openTag("successor");
263  device.writeAttr("elementType", "junction");
264  device.writeAttr("elementId", toNodeID);
265  device.closeTag();
266  }
267  device.closeTag();
268  }
269  device.openTag("type").writeAttr("s", 0).writeAttr("type", "town").closeTag();
270  device << planViewOSS.getString();
271  writeElevationProfile(ls, device, elevationOSS);
272  device << " <lateralProfile/>\n";
273  device << " <lanes>\n";
274  device << " <laneSection s=\"0\">\n";
275  const std::string centerMark = e->getPermissions(e->getNumLanes() - 1) == 0 ? "none" : "solid";
276  writeEmptyCenterLane(device, centerMark, 0.13);
277  device << " <right>\n";
278  for (int j = e->getNumLanes(); --j >= 0;) {
279  device << " <lane id=\"-" << e->getNumLanes() - j << "\" type=\"" << getLaneType(e->getPermissions(j)) << "\" level=\"true\">\n";
280  device << " <link/>\n";
281  // this could be used for geometry-link junctions without u-turn,
282  // predecessor and sucessors would be lane indices,
283  // road predecessor / succesfors would be of type 'road' rather than
284  // 'junction'
285  //device << " <predecessor id=\"-1\"/>\n";
286  //device << " <successor id=\"-1\"/>\n";
287  //device << " </link>\n";
288  device << " <width sOffset=\"0\" a=\"" << e->getLaneWidth(j) << "\" b=\"0\" c=\"0\" d=\"0\"/>\n";
289  std::string markType = "broken";
290  if (j == 0) {
291  markType = "solid";
292  } else if (j > 0
293  && (e->getPermissions(j - 1) & ~(SVC_PEDESTRIAN | SVC_BICYCLE)) == 0) {
294  // solid road mark to the left of sidewalk or bicycle lane
295  markType = "solid";
296  } else if (e->getPermissions(j) == 0) {
297  // solid road mark to the right of a forbidden lane
298  markType = "solid";
299  }
300  device << " <roadMark sOffset=\"0\" type=\"" << markType << "\" weight=\"standard\" color=\"standard\" width=\"0.13\"/>\n";
301  device << " <speed sOffset=\"0\" max=\"" << lanes[j].speed << "\"/>\n";
302  device << " </lane>\n";
303  }
304  device << " </right>\n";
305  device << " </laneSection>\n";
306  device << " </lanes>\n";
307  device << " <objects/>\n";
308  device << " <signals/>\n";
309  if (origNames) {
310  device << " <userData code=\"sumoId\" value=\"" << e->getID() << "\"/>\n";
311  }
312  device.closeTag();
314 }
315 
316 void
317 NWWriter_OpenDrive::addPedestrianConnection(const NBEdge* inEdge, const NBEdge* outEdge, std::vector<NBEdge::Connection>& parallel) {
318  // by defaul there are no iternal lanes for pedestrians. Determine if
319  // one is feasible and does not exist yet.
320  if (outEdge != 0
321  && inEdge->getPermissions(0) == SVC_PEDESTRIAN
322  && outEdge->getPermissions(0) == SVC_PEDESTRIAN
323  && (parallel.empty()
324  || parallel.front().fromLane != 0
325  || parallel.front().toLane != 0)) {
326  parallel.insert(parallel.begin(), NBEdge::Connection(0, const_cast<NBEdge*>(outEdge), 0, false));
327  }
328 }
329 
330 
331 int
332 NWWriter_OpenDrive::writeInternalEdge(OutputDevice& device, OutputDevice& junctionDevice, const NBEdge* inEdge, int nodeID,
333  int edgeID, int inEdgeID, int outEdgeID,
334  int connectionID,
335  const std::vector<NBEdge::Connection>& parallel,
336  const bool isOuterEdge,
337  const double straightThresh,
338  const std::string& centerMark) {
339  assert(parallel.size() != 0);
340  const NBEdge::Connection& cLeft = parallel.back();
341  const NBEdge* outEdge = cLeft.toEdge;
342  PositionVector begShape = getLeftLaneBorder(inEdge, cLeft.fromLane);
343  PositionVector endShape = getLeftLaneBorder(outEdge, cLeft.toLane);
344  //std::cout << "computing reference line for internal lane " << cLeft.getInternalLaneID() << " begLane=" << inEdge->getLaneShape(cLeft.fromLane) << " endLane=" << outEdge->getLaneShape(cLeft.toLane) << "\n";
345 
346  double length;
347  double laneOffset = 0;
348  PositionVector fallBackShape;
349  fallBackShape.push_back(begShape.back());
350  fallBackShape.push_back(endShape.front());
351  const bool turnaround = inEdge->isTurningDirectionAt(outEdge);
352  bool ok = true;
353  PositionVector init = NBNode::bezierControlPoints(begShape, endShape, turnaround, 25, 25, ok, 0, straightThresh);
354  if (init.size() == 0) {
355  length = fallBackShape.length2D();
356  // problem with turnarounds is known, method currently returns 'ok' (#2539)
357  if (!ok) {
358  WRITE_WARNING("Could not compute smooth shape from lane '" + inEdge->getLaneID(cLeft.fromLane) + "' to lane '" + outEdge->getLaneID(cLeft.toLane) + "'. Use option 'junctions.scurve-stretch' or increase radius of junction '" + inEdge->getToNode()->getID() + "' to fix this.");
359  } else if (length <= NUMERICAL_EPS) {
360  // left-curving geometry-like edges must use the right
361  // side as reference line and shift
362  begShape = getRightLaneBorder(inEdge, cLeft.fromLane);
363  endShape = getRightLaneBorder(outEdge, cLeft.toLane);
364  init = NBNode::bezierControlPoints(begShape, endShape, turnaround, 25, 25, ok, 0, straightThresh);
365  if (init.size() != 0) {
366  length = bezier(init, 12).length2D();
367  laneOffset = outEdge->getLaneWidth(cLeft.toLane);
368  //std::cout << " internalLane=" << cLeft.getInternalLaneID() << " length=" << length << "\n";
369  }
370  }
371  } else {
372  length = bezier(init, 12).length2D();
373  }
374 
375  junctionDevice << " <connection id=\"" << connectionID << "\" incomingRoad=\"" << inEdgeID << "\" connectingRoad=\"" << edgeID << "\" contactPoint=\"start\">\n";
376  device.openTag("road");
377  device.writeAttr("name", cLeft.id);
378  device.setPrecision(8); // length requires higher precision
379  device.writeAttr("length", MAX2(POSITION_EPS, length));
380  device.setPrecision(gPrecision);
381  device.writeAttr("id", edgeID);
382  device.writeAttr("junction", nodeID);
383  device.openTag("link");
384  device.openTag("predecessor");
385  device.writeAttr("elementType", "road");
386  device.writeAttr("elementId", inEdgeID);
387  device.writeAttr("contactPoint", "end");
388  device.closeTag();
389  device.openTag("successor");
390  device.writeAttr("elementType", "road");
391  device.writeAttr("elementId", outEdgeID);
392  device.writeAttr("contactPoint", "start");
393  device.closeTag();
394  device.closeTag();
395  device.openTag("type").writeAttr("s", 0).writeAttr("type", "town").closeTag();
396  device.openTag("planView");
397  device.setPrecision(8); // geometry hdg requires higher precision
398  OutputDevice_String elevationOSS(false, 3);
399  elevationOSS.setPrecision(8);
400 #ifdef DEBUG_SMOOTH_GEOM
401  if (DEBUGCOND) {
402  std::cout << "write planview for internal edge " << cLeft.id << " init=" << init << " fallback=" << fallBackShape
403  << " begShape=" << begShape << " endShape=" << endShape
404  << "\n";
405  }
406 #endif
407  if (init.size() == 0) {
408  writeGeomLines(fallBackShape, device, elevationOSS);
409  } else {
410  writeGeomPP3(device, elevationOSS, init, length);
411  }
412  device.setPrecision(gPrecision);
413  device.closeTag();
414  writeElevationProfile(fallBackShape, device, elevationOSS);
415  device << " <lateralProfile/>\n";
416  device << " <lanes>\n";
417  if (laneOffset != 0) {
418  device << " <laneOffset s=\"0\" a=\"" << laneOffset << "\" b=\"0\" c=\"0\" d=\"0\"/>\n";
419  }
420  device << " <laneSection s=\"0\">\n";
421  writeEmptyCenterLane(device, centerMark, 0);
422  device << " <right>\n";
423  for (int j = (int)parallel.size(); --j >= 0;) {
424  const NBEdge::Connection& c = parallel[j];
425  const int fromIndex = c.fromLane - inEdge->getNumLanes();
426  const int toIndex = c.toLane - outEdge->getNumLanes();
427  device << " <lane id=\"-" << parallel.size() - j << "\" type=\"" << getLaneType(outEdge->getPermissions(c.toLane)) << "\" level=\"true\">\n";
428  device << " <link>\n";
429  device << " <predecessor id=\"" << fromIndex << "\"/>\n";
430  device << " <successor id=\"" << toIndex << "\"/>\n";
431  device << " </link>\n";
432  device << " <width sOffset=\"0\" a=\"" << outEdge->getLaneWidth(c.toLane) << "\" b=\"0\" c=\"0\" d=\"0\"/>\n";
433  std::string markType = "broken";
434  if (inEdge->isTurningDirectionAt(outEdge)) {
435  markType = "none";
436  } else if (c.fromLane == 0 && c.toLane == 0 && isOuterEdge) {
437  // solid road mark at the outer border
438  markType = "solid";
439  } else if (isOuterEdge && j > 0
440  && (outEdge->getPermissions(parallel[j - 1].toLane) & ~(SVC_PEDESTRIAN | SVC_BICYCLE)) == 0) {
441  // solid road mark to the left of sidewalk or bicycle lane
442  markType = "solid";
443  } else if (!inEdge->getToNode()->geometryLike()) {
444  // draw shorter road marks to indicate turning paths
445  LinkDirection dir = inEdge->getToNode()->getDirection(inEdge, outEdge, OptionsCont::getOptions().getBool("lefthand"));
446  if (dir == LINKDIR_LEFT || dir == LINKDIR_RIGHT || dir == LINKDIR_PARTLEFT || dir == LINKDIR_PARTRIGHT) {
447  // XXX <type><line/><type> is not rendered by odrViewer so cannot be validated
448  // device << " <type name=\"broken\" width=\"0.13\">\n";
449  // device << " <line length=\"0.5\" space=\"0.5\" tOffset=\"0\" sOffset=\"0\" rule=\"none\"/>\n";
450  // device << " </type>\n";
451  markType = "none";
452  }
453  }
454  device << " <roadMark sOffset=\"0\" type=\"" << markType << "\" weight=\"standard\" color=\"standard\" width=\"0.13\"/>\n";
455  device << " <speed sOffset=\"0\" max=\"" << c.vmax << "\"/>\n";
456  device << " </lane>\n";
457 
458  junctionDevice << " <laneLink from=\"" << fromIndex << "\" to=\"" << toIndex << "\"/>\n";
459  connectionID++;
460  }
461  device << " </right>\n";
462  device << " </laneSection>\n";
463  device << " </lanes>\n";
464  device << " <objects/>\n";
465  device << " <signals/>\n";
466  device.closeTag();
467  junctionDevice << " </connection>\n";
468 
469  return connectionID;
470 }
471 
472 
473 double
474 NWWriter_OpenDrive::writeGeomLines(const PositionVector& shape, OutputDevice& device, OutputDevice& elevationDevice, double offset) {
475  for (int j = 0; j < (int)shape.size() - 1; ++j) {
476  const Position& p = shape[j];
477  const Position& p2 = shape[j + 1];
478  const double hdg = shape.angleAt2D(j);
479  const double length = p.distanceTo2D(p2);
480  device.openTag("geometry");
481  device.writeAttr("s", offset);
482  device.writeAttr("x", p.x());
483  device.writeAttr("y", p.y());
484  device.writeAttr("hdg", hdg);
485  device.writeAttr("length", length);
486  device.openTag("line").closeTag();
487  device.closeTag();
488  elevationDevice << " <elevation s=\"" << offset << "\" a=\"" << p.z() << "\" b=\"" << (p2.z() - p.z()) / MAX2(POSITION_EPS, length) << "\" c=\"0\" d=\"0\"/>\n";
489  offset += length;
490  }
491  return offset;
492 }
493 
494 
495 void
496 NWWriter_OpenDrive::writeEmptyCenterLane(OutputDevice& device, const std::string& mark, double markWidth) {
497  device << " <center>\n";
498  device << " <lane id=\"0\" type=\"none\" level=\"true\">\n";
499  device << " <link/>\n";
500  device << " <roadMark sOffset=\"0\" type=\"" << mark << "\" weight=\"standard\" color=\"standard\" width=\"" << markWidth << "\"/>\n";
501  device << " </lane>\n";
502  device << " </center>\n";
503 }
504 
505 
506 int
507 NWWriter_OpenDrive::getID(const std::string& origID, StringBijection<int>& map, int& lastID) {
508  if (map.hasString(origID)) {
509  return map.get(origID);
510  }
511  map.insert(origID, lastID++);
512  return lastID - 1;
513 }
514 
515 
516 std::string
518  switch (permissions) {
519  case SVC_PEDESTRIAN:
520  return "sidewalk";
521  //case (SVC_BICYCLE | SVC_PEDESTRIAN):
522  // WRITE_WARNING("Ambiguous lane type (biking+driving) for road '" + roadID + "'");
523  // return "sidewalk";
524  case SVC_BICYCLE:
525  return "biking";
526  case 0:
527  // ambiguous
528  return "none";
529  case SVC_RAIL:
530  case SVC_RAIL_URBAN:
531  case SVC_RAIL_ELECTRIC:
532  return "rail";
533  case SVC_TRAM:
534  return "tram";
535  default: {
536  // complex permissions
537  if (permissions == SVCAll) {
538  return "driving";
539  } else if (isRailway(permissions)) {
540  return "rail";
541  } else if ((permissions & SVC_PASSENGER) != 0) {
542  return "driving";
543  } else {
544  return "restricted";
545  }
546  }
547  }
548 }
549 
550 
552 NWWriter_OpenDrive::getLeftLaneBorder(const NBEdge* edge, int laneIndex, double widthOffset) {
553  const bool lefthand = OptionsCont::getOptions().getBool("lefthand");
554  if (laneIndex == -1) {
555  // leftmost lane
556  laneIndex = lefthand ? 0 : (int)edge->getNumLanes() - 1;
557  }
559  // PositionVector result = edge->getLaneShape(laneIndex);
560  // (and the moveo2side)
561  // However, the lanes in SUMO have a small lateral gap (SUMO_const_laneOffset) to account for markings
562  // In OpenDRIVE this gap does not exists so we have to do all lateral
563  // computations based on the reference line
564  // This assumes that the 'stop line' for all lanes is colinear!
565  const int leftmost = lefthand ? 0 : (int)edge->getNumLanes() - 1;
566  widthOffset -= (edge->getLaneWidth(leftmost) / 2);
567  // collect lane widths from left border of edge to left border of lane to connect to
568  if (lefthand) {
569  for (int i = leftmost; i < laneIndex; i++) {
570  widthOffset += edge->getLaneWidth(i);
571  }
572  } else {
573  for (int i = leftmost; i > laneIndex; i--) {
574  widthOffset += edge->getLaneWidth(i);
575  }
576  }
577  PositionVector result = edge->getLaneShape(leftmost);
578  try {
579  result.move2side(widthOffset);
580  } catch (InvalidArgument&) { }
581  return result;
582 }
583 
585 NWWriter_OpenDrive::getRightLaneBorder(const NBEdge* edge, int laneIndex) {
586  return getLeftLaneBorder(edge, laneIndex, edge->getLaneWidth(laneIndex));
587 }
588 
589 
590 double
592  OutputDevice& device,
593  OutputDevice& elevationDevice,
594  PositionVector init,
595  double length,
596  double offset) {
597  assert(init.size() == 3 || init.size() == 4);
598 
599  // avoid division by 0
600  length = MAX2(POSITION_EPS, length);
601 
602  const Position p = init.front();
603  const double hdg = init.angleAt2D(0);
604 
605  // backup elevation values
606  const PositionVector initZ = init;
607  // translate to u,v coordinates
608  init.add(-p.x(), -p.y(), -p.z());
609  init.rotate2D(-hdg);
610 
611  // parametric coefficients
612  double aU, bU, cU, dU;
613  double aV, bV, cV, dV;
614  double aZ, bZ, cZ, dZ;
615 
616  // unfactor the Bernstein polynomials of degree 2 (or 3) and collect the coefficients
617  if (init.size() == 3) {
618  //f(x, a, b ,c) = a + (2*b - 2*a)*x + (a - 2*b + c)*x*x
619  aU = init[0].x();
620  bU = 2 * init[1].x() - 2 * init[0].x();
621  cU = init[0].x() - 2 * init[1].x() + init[2].x();
622  dU = 0;
623 
624  aV = init[0].y();
625  bV = 2 * init[1].y() - 2 * init[0].y();
626  cV = init[0].y() - 2 * init[1].y() + init[2].y();
627  dV = 0;
628 
629  // elevation is not parameteric on [0:1] but on [0:length]
630  aZ = initZ[0].z();
631  bZ = (2 * initZ[1].z() - 2 * initZ[0].z()) / length;
632  cZ = (initZ[0].z() - 2 * initZ[1].z() + initZ[2].z()) / (length * length);
633  dZ = 0;
634 
635  } else {
636  // f(x, a, b, c, d) = a + (x*((3*b) - (3*a))) + ((x*x)*((3*a) + (3*c) - (6*b))) + ((x*x*x)*((3*b) - (3*c) - a + d))
637  aU = init[0].x();
638  bU = 3 * init[1].x() - 3 * init[0].x();
639  cU = 3 * init[0].x() - 6 * init[1].x() + 3 * init[2].x();
640  dU = -init[0].x() + 3 * init[1].x() - 3 * init[2].x() + init[3].x();
641 
642  aV = init[0].y();
643  bV = 3 * init[1].y() - 3 * init[0].y();
644  cV = 3 * init[0].y() - 6 * init[1].y() + 3 * init[2].y();
645  dV = -init[0].y() + 3 * init[1].y() - 3 * init[2].y() + init[3].y();
646 
647  // elevation is not parameteric on [0:1] but on [0:length]
648  aZ = initZ[0].z();
649  bZ = (3 * initZ[1].z() - 3 * initZ[0].z()) / length;
650  cZ = (3 * initZ[0].z() - 6 * initZ[1].z() + 3 * initZ[2].z()) / (length * length);
651  dZ = (-initZ[0].z() + 3 * initZ[1].z() - 3 * initZ[2].z() + initZ[3].z()) / (length * length * length);
652  }
653 
654  device.openTag("geometry");
655  device.writeAttr("s", offset);
656  device.writeAttr("x", p.x());
657  device.writeAttr("y", p.y());
658  device.writeAttr("hdg", hdg);
659  device.writeAttr("length", length);
660 
661  device.openTag("paramPoly3");
662  device.writeAttr("aU", aU);
663  device.writeAttr("bU", bU);
664  device.writeAttr("cU", cU);
665  device.writeAttr("dU", dU);
666  device.writeAttr("aV", aV);
667  device.writeAttr("bV", bV);
668  device.writeAttr("cV", cV);
669  device.writeAttr("dV", dV);
670  device.closeTag();
671  device.closeTag();
672 
673  // write elevation
674  elevationDevice.openTag("elevation");
675  elevationDevice.writeAttr("s", offset);
676  elevationDevice.writeAttr("a", aZ);
677  elevationDevice.writeAttr("b", bZ);
678  elevationDevice.writeAttr("c", cZ);
679  elevationDevice.writeAttr("d", dZ);
680  elevationDevice.closeTag();
681 
682  return offset + length;
683 }
684 
685 
686 bool
687 NWWriter_OpenDrive::writeGeomSmooth(const PositionVector& shape, double speed, OutputDevice& device, OutputDevice& elevationDevice, double straightThresh, double& length) {
688 #ifdef DEBUG_SMOOTH_GEOM
689  if (DEBUGCOND) {
690  std::cout << "writeGeomSmooth\n n=" << shape.size() << " shape=" << toString(shape) << "\n";
691  }
692 #endif
693  bool ok = true;
694  const double longThresh = speed; // 16.0; // make user-configurable (should match the sampling rate of the source data)
695  const double curveCutout = longThresh / 2; // 8.0; // make user-configurable (related to the maximum turning rate)
696  // the length of the segment that is added for cutting a corner can be bounded by 2*curveCutout (prevent the segment to be classified as 'long')
697  assert(longThresh >= 2 * curveCutout);
698  assert(shape.size() > 2);
699  // add intermediate points wherever there is a strong angular change between long segments
700  // assume the geometry is simplified so as not to contain consecutive colinear points
701  PositionVector shape2 = shape;
702  double maxAngleDiff = 0;
703  double offset = 0;
704  for (int j = 1; j < (int)shape.size() - 1; ++j) {
705  //const double hdg = shape.angleAt2D(j);
706  const Position& p0 = shape[j - 1];
707  const Position& p1 = shape[j];
708  const Position& p2 = shape[j + 1];
709  const double dAngle = fabs(GeomHelper::angleDiff(p0.angleTo2D(p1), p1.angleTo2D(p2)));
710  const double length1 = p0.distanceTo2D(p1);
711  const double length2 = p1.distanceTo2D(p2);
712  maxAngleDiff = MAX2(maxAngleDiff, dAngle);
713 #ifdef DEBUG_SMOOTH_GEOM
714  if (DEBUGCOND) {
715  std::cout << " j=" << j << " dAngle=" << RAD2DEG(dAngle) << " length1=" << length1 << " length2=" << length2 << "\n";
716  }
717 #endif
718  if (dAngle > straightThresh
719  && (length1 > longThresh || j == 1)
720  && (length2 > longThresh || j == (int)shape.size() - 2)) {
721  shape2.insertAtClosest(shape.positionAtOffset2D(offset + length1 - MIN2(length1 - POSITION_EPS, curveCutout)));
722  shape2.insertAtClosest(shape.positionAtOffset2D(offset + length1 + MIN2(length2 - POSITION_EPS, curveCutout)));
723  shape2.removeClosest(p1);
724  }
725  offset += length1;
726  }
727  const int numPoints = (int)shape2.size();
728 #ifdef DEBUG_SMOOTH_GEOM
729  if (DEBUGCOND) {
730  std::cout << " n=" << numPoints << " shape2=" << toString(shape2) << "\n";
731  }
732 #endif
733 
734  if (maxAngleDiff < straightThresh) {
735  length = writeGeomLines(shape2, device, elevationDevice, 0);
736 #ifdef DEBUG_SMOOTH_GEOM
737  if (DEBUGCOND) {
738  std::cout << " special case: all lines. maxAngleDiff=" << maxAngleDiff << "\n";
739  }
740 #endif
741  return ok;
742  }
743 
744  // write the long segments as lines, short segments as curves
745  offset = 0;
746  for (int j = 0; j < numPoints - 1; ++j) {
747  const Position& p0 = shape2[j];
748  const Position& p1 = shape2[j + 1];
749  PositionVector line;
750  line.push_back(p0);
751  line.push_back(p1);
752  const double lineLength = line.length2D();
753  if (lineLength >= longThresh) {
754  offset = writeGeomLines(line, device, elevationDevice, offset);
755 #ifdef DEBUG_SMOOTH_GEOM
756  if (DEBUGCOND) {
757  std::cout << " writeLine=" << toString(line) << "\n";
758  }
759 #endif
760  } else {
761  // find control points
762  PositionVector begShape;
763  PositionVector endShape;
764  if (j == 0 || j == numPoints - 2) {
765  // keep the angle of the first/last segment but end at the front of the shape
766  begShape = line;
767  begShape.add(p0 - begShape.back());
768  } else if (j == 1 || p0.distanceTo2D(shape2[j - 1]) > longThresh) {
769  // use the previous segment if it is long or the first one
770  begShape.push_back(shape2[j - 1]);
771  begShape.push_back(p0);
772  } else {
773  // end at p0 with mean angle of the previous and current segment
774  begShape.push_back(shape2[j - 1]);
775  begShape.push_back(p1);
776  begShape.add(p0 - begShape.back());
777  }
778 
779  if (j == 0 || j == numPoints - 2) {
780  // keep the angle of the first/last segment but start at the end of the shape
781  endShape = line;
782  endShape.add(p1 - endShape.front());
783  } else if (j == numPoints - 3 || p1.distanceTo2D(shape2[j + 2]) > longThresh) {
784  // use the next segment if it is long or the final one
785  endShape.push_back(p1);
786  endShape.push_back(shape2[j + 2]);
787  } else {
788  // start at p1 with mean angle of the current and next segment
789  endShape.push_back(p0);
790  endShape.push_back(shape2[j + 2]);
791  endShape.add(p1 - endShape.front());
792  }
793  const double extrapolateLength = MIN2((double)25, lineLength / 4);
794  PositionVector init = NBNode::bezierControlPoints(begShape, endShape, false, extrapolateLength, extrapolateLength, ok, 0, straightThresh);
795  if (init.size() == 0) {
796  // could not compute control points, write line
797  offset = writeGeomLines(line, device, elevationDevice, offset);
798 #ifdef DEBUG_SMOOTH_GEOM
799  if (DEBUGCOND) {
800  std::cout << " writeLine lineLength=" << lineLength << " begShape" << j << "=" << toString(begShape) << " endShape" << j << "=" << toString(endShape) << " init" << j << "=" << toString(init) << "\n";
801  }
802 #endif
803  } else {
804  // write bezier
805  const double curveLength = bezier(init, 12).length2D();
806  offset = writeGeomPP3(device, elevationDevice, init, curveLength, offset);
807 #ifdef DEBUG_SMOOTH_GEOM
808  if (DEBUGCOND) {
809  std::cout << " writeCurve lineLength=" << lineLength << " curveLength=" << curveLength << " begShape" << j << "=" << toString(begShape) << " endShape" << j << "=" << toString(endShape) << " init" << j << "=" << toString(init) << "\n";
810  }
811 #endif
812  }
813  }
814  }
815  length = offset;
816  return ok;
817 }
818 
819 
820 void
822  // check if the shape is flat
823  bool flat = true;
824  double z = shape.size() == 0 ? 0 : shape[0].z();
825  for (int i = 1; i < (int)shape.size(); ++i) {
826  if (fabs(shape[i].z() - z) > NUMERICAL_EPS) {
827  flat = false;
828  break;
829  }
830  }
831  device << " <elevationProfile>\n";
832  if (flat) {
833  device << " <elevation s=\"0\" a=\"" << z << "\" b=\"0\" c=\"0\" d=\"0\"/>\n";
834  } else {
835  device << elevationDevice.getString();
836  }
837  device << " </elevationProfile>\n";
838 
839 }
840 
841 
842 void
844  if (e->getNumLanes() > 1) {
845  // compute 'stop line' of rightmost lane
846  const PositionVector shape0 = e->getLaneShape(0);
847  assert(shape0.size() >= 2);
848  const Position& from = shape0[-2];
849  const Position& to = shape0[-1];
850  PositionVector stopLine;
851  stopLine.push_back(to);
852  stopLine.push_back(to - PositionVector::sideOffset(from, to, -1000.0));
853  // endpoints of all other lanes should be on the stop line
854  for (int lane = 1; lane < e->getNumLanes(); ++lane) {
855  const double dist = stopLine.distance2D(e->getLaneShape(lane)[-1]);
856  if (dist > NUMERICAL_EPS) {
857  WRITE_WARNING("Uneven stop line at lane '" + e->getLaneID(lane) + "' (dist=" + toString(dist) + ") cannot be represented in OpenDRIVE.");
858  }
859  }
860  }
861 }
862 
863 /****************************************************************************/
864 
#define DEBUGCOND
The link is a partial left direction.
double vmax
maximun velocity
Definition: NBEdge.h:222
OutputDevice & writeAttr(const SumoXMLAttr attr, const T &val)
writes a named attribute
Definition: OutputDevice.h:260
double ymin() const
Returns minimum y-coordinate.
Definition: Boundary.cpp:137
double length2D() const
Returns the length.
double xmax() const
Returns maximum x-coordinate.
Definition: Boundary.cpp:131
A structure which describes a connection between edges or lanes.
Definition: NBEdge.h:161
int toLane
The lane the connections yields in.
Definition: NBEdge.h:189
int numNormalConnections() const
return the number of lane-to-lane connections at this junction (excluding crossings) ...
Definition: NBNode.cpp:2683
static void writeEmptyCenterLane(OutputDevice &device, const std::string &mark, double markWidth)
is a pedestrian
std::map< std::string, NBNode * >::const_iterator begin() const
Returns the pointer to the begin of the stored nodes.
Definition: NBNodeCont.h:117
double z() const
Returns the z-position.
Definition: Position.h:72
double distance2D(const Position &p, bool perpendicular=false) const
closest 2D-distance to point p (or -1 if perpendicular is true and the point is beyond this vector) ...
NBEdge * toEdge
The edge the connections yields in.
Definition: NBEdge.h:186
EdgeVector getIncomingEdges() const
Returns the list of incoming edges unsorted.
Definition: NBEdge.cpp:1146
const Boundary & getConvBoundary() const
Returns the converted boundary.
std::map< std::string, NBNode * >::const_iterator end() const
Returns the pointer to the end of the stored nodes.
Definition: NBNodeCont.h:122
vehicle is a not electrified rail
double distanceTo2D(const Position &p2) const
returns the euclidean distance in the x-y-plane
Definition: Position.h:249
int gPrecision
the precision for floating point outputs
Definition: StdDefs.cpp:29
Position positionAtOffset2D(double pos, double lateralOffset=0) const
Returns the position at the given length.
vehicle is a bicycle
static void writeNormalEdge(OutputDevice &device, const NBEdge *e, int edgeID, int fromNodeID, int toNodeID, const bool origNames, const double straightThresh)
write normal edge to device
std::string getString() const
Returns the current content as a string.
double y() const
Returns the y-position.
Definition: Position.h:67
int SVCPermissions
bitset where each bit declares whether a certain SVC may use this edge/lane
The representation of a single edge during network building.
Definition: NBEdge.h:70
double x() const
Returns the x-position.
Definition: Position.h:62
vehicle is a light rail
void setPrecision(int precision=gPrecision)
Sets the precison or resets it to default.
double angleTo2D(const Position &other) const
returns the angle in the plane of the vector pointing from here to the other position ...
Definition: Position.h:259
T MAX2(T a, T b)
Definition: StdDefs.h:73
const std::vector< NBEdge::Lane > & getLanes() const
Returns the lane definitions.
Definition: NBEdge.h:569
std::map< std::string, NBEdge * >::const_iterator end() const
Returns the pointer to the end of the stored edges.
Definition: NBEdgeCont.h:198
#define RAD2DEG(x)
Definition: GeomHelper.h:45
bool getBool(const std::string &name) const
Returns the boolean-value of the named option (only for Option_Bool)
const std::string & getID() const
Returns the id.
Definition: Named.h:74
bool isRailway(SVCPermissions permissions)
Returns whether an edge with the given permission is a railway edge.
const SVCPermissions SVCAll
all VClasses are allowed
A class that stores a 2D geometrical boundary.
Definition: Boundary.h:47
The link is a (hard) left direction.
vehicle is a (possibly fast moving) electric rail
#define WRITE_WARNING(msg)
Definition: MsgHandler.h:199
static OptionsCont & getOptions()
Retrieves the options.
Definition: OptionsCont.cpp:64
vehicle is a city rail
LinkDirection
The different directions a link between two lanes may take (or a stream between two edges)...
bool isSet(const std::string &name, bool failOnNonExistant=true) const
Returns the information whether the named option is set.
void insert(const std::string str, const T key, bool checkDuplicates=true)
static void writeNetwork(const OptionsCont &oc, NBNetBuilder &nb)
Writes the network into a openDRIVE-file.
static double writeGeomPP3(OutputDevice &device, OutputDevice &elevationDevice, PositionVector init, double length, double offset=0)
write geometry as a single bezier curve (paramPoly3)
std::map< std::string, NBEdge * >::const_iterator begin() const
Returns the pointer to the begin of the stored edges.
Definition: NBEdgeCont.h:190
std::string toString(const T &t, std::streamsize accuracy=gPrecision)
Definition: ToString.h:55
static bool writeGeomSmooth(const PositionVector &shape, double speed, OutputDevice &device, OutputDevice &elevationDevice, double straightThresh, double &length)
int size() const
Returns the number of nodes stored in this container.
Definition: NBNodeCont.h:243
std::string getLaneID(int lane) const
get Lane ID (Secure)
Definition: NBEdge.cpp:2783
int getNumLanes() const
Returns the number of lanes.
Definition: NBEdge.h:412
int fromLane
The lane the connections starts at.
Definition: NBEdge.h:183
A point in 2D or 3D with translation and scaling methods.
Definition: Position.h:45
NBEdgeCont & getEdgeCont()
Definition: NBNetBuilder.h:156
A list of positions.
static PositionVector getRightLaneBorder(const NBEdge *edge, int laneIndex=-1)
T get(const std::string &str) const
std::string getString(const std::string &name) const
Returns the string-value of the named option (only for Option_String)
bool geometryLike() const
whether this is structurally similar to a geometry node
Definition: NBNode.cpp:2596
Storage for edges, including some functionality operating on multiple edges.
Definition: NBEdgeCont.h:66
T MIN2(T a, T b)
Definition: StdDefs.h:67
double xmin() const
Returns minimum x-coordinate.
Definition: Boundary.cpp:125
The link is a (hard) right direction.
#define POSITION_EPS
Definition: config.h:175
const std::string & getStreetName() const
Returns the street name of this edge.
Definition: NBEdge.h:535
static PositionVector getLeftLaneBorder(const NBEdge *edge, int laneIndex=-1, double widthOffset=0)
get the left border of the given lane (the leftmost one by default)
#define DEG2RAD(x)
Definition: GeomHelper.h:44
static std::string escapeXML(const std::string &orig, const bool maskDoubleHyphen=false)
Replaces the standard escapes by their XML entities.
The link is a partial right direction.
double getFloat(const std::string &name) const
Returns the double-value of the named option (only for Option_Float)
SVCPermissions getPermissions(int lane=-1) const
get the union of allowed classes over all lanes or for a specific lane
Definition: NBEdge.cpp:3025
void move2side(double amount)
move position vector to side using certain ammount
vehicle is a passenger car (a "normal" car)
LinkDirection getDirection(const NBEdge *const incoming, const NBEdge *const outgoing, bool leftHand=false) const
Returns the representation of the described stream&#39;s direction.
Definition: NBNode.cpp:1539
double getSpeed() const
Returns the speed allowed on this edge.
Definition: NBEdge.h:506
double getLaneWidth() const
Returns the default width of lanes of this edge.
Definition: NBEdge.h:522
void rotate2D(double angle)
static std::string getLaneType(SVCPermissions permissions)
static double writeGeomLines(const PositionVector &shape, OutputDevice &device, OutputDevice &elevationDevice, double offset=0)
write geometry as sequence of lines (sumo style)
const PositionVector & getLaneShape(int i) const
Returns the shape of the nth lane.
Definition: NBEdge.cpp:778
const EdgeVector & getIncomingEdges() const
Returns this node&#39;s incoming edges (The edges which yield in this node)
Definition: NBNode.h:249
const std::vector< Connection > & getConnections() const
Returns the connections.
Definition: NBEdge.h:845
NBNodeCont & getNodeCont()
Returns a reference to the node container.
Definition: NBNetBuilder.h:161
Instance responsible for building networks.
Definition: NBNetBuilder.h:115
static OutputDevice & getDevice(const std::string &name)
Returns the described OutputDevice.
static Position sideOffset(const Position &beg, const Position &end, const double amount)
get a side position of position vector using a offset
#define INVALID_ID
A storage for options typed value containers)
Definition: OptionsCont.h:98
double angleAt2D(int pos) const
get angle in certain position of position vector
static const GeoConvHelper & getFinal()
the coordinate transformation for writing the location element and for tracking the original coordina...
static void addPedestrianConnection(const NBEdge *inEdge, const NBEdge *outEdge, std::vector< NBEdge::Connection > &parallel)
bool isTurningDirectionAt(const NBEdge *const edge) const
Returns whether the given edge is the opposite direction to this edge.
Definition: NBEdge.cpp:2450
Represents a single node (junction) during network building.
Definition: NBNode.h:74
Static storage of an output device and its base (abstract) implementation.
Definition: OutputDevice.h:70
bool closeTag()
Closes the most recently opened tag.
#define NUMERICAL_EPS
Definition: config.h:151
NBNode * getFromNode() const
Returns the origin node of the edge.
Definition: NBEdge.h:426
Container for nodes during the netbuilding process.
Definition: NBNodeCont.h:66
bool hasString(const std::string &str) const
static double angleDiff(const double angle1, const double angle2)
Returns the difference of the second angle to the first angle in radiants.
Definition: GeomHelper.cpp:173
static void checkLaneGeometries(const NBEdge *e)
check if the lane geometries are compatible with OpenDRIVE assumptions (colinear stop line) ...
void add(double xoff, double yoff, double zoff)
double ymax() const
Returns maximum y-coordinate.
Definition: Boundary.cpp:143
static int writeInternalEdge(OutputDevice &device, OutputDevice &junctionDevice, const NBEdge *inEdge, int nodeID, int edgeID, int inEdgeID, int outEdgeID, int connectionID, const std::vector< NBEdge::Connection > &parallel, const bool isOuterEdge, const double straightThresh, const std::string &centerMark)
write internal edge to device, return next connectionID
static int getID(const std::string &origID, StringBijection< int > &map, int &lastID)
static PositionVector bezierControlPoints(const PositionVector &begShape, const PositionVector &endShape, bool isTurnaround, double extrapolateBeg, double extrapolateEnd, bool &ok, NBNode *recordError=0, double straightThresh=DEG2RAD(5))
get bezier control points
Definition: NBNode.cpp:486
NBNode * getToNode() const
Returns the destination node of the edge.
Definition: NBEdge.h:433
static void writeElevationProfile(const PositionVector &shape, OutputDevice &device, const OutputDevice_String &elevationDevice)
OutputDevice & openTag(const std::string &xmlElement)
Opens an XML tag.
void lf()
writes a line feed if applicable
Definition: OutputDevice.h:238
An output device that encapsulates an ofstream.
void bezier(int npts, double b[], int cpts, double p[])
Definition: bezier.cpp:96