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