Line data Source code
1 : /****************************************************************************/
2 : // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.dev/sumo
3 : // Copyright (C) 2001-2025 German Aerospace Center (DLR) and others.
4 : // This program and the accompanying materials are made available under the
5 : // terms of the Eclipse Public License 2.0 which is available at
6 : // https://www.eclipse.org/legal/epl-2.0/
7 : // This Source Code may also be made available under the following Secondary
8 : // Licenses when the conditions for such availability set forth in the Eclipse
9 : // Public License 2.0 are satisfied: GNU General Public License, version 2
10 : // or later which is available at
11 : // https://www.gnu.org/licenses/old-licenses/gpl-2.0-standalone.html
12 : // SPDX-License-Identifier: EPL-2.0 OR GPL-2.0-or-later
13 : /****************************************************************************/
14 : /// @file NIImporter_OpenDrive.cpp
15 : /// @author Daniel Krajzewicz
16 : /// @author Jakob Erdmann
17 : /// @author Michael Behrisch
18 : /// @author Laura Bieker
19 : /// @author Mirko Barthauer
20 : /// @date Mon, 14.04.2008
21 : ///
22 : // Importer for networks stored in openDrive format
23 : /****************************************************************************/
24 : #include <config.h>
25 : #include <string>
26 : #include <cmath>
27 : #include <iterator>
28 : #include <utils/xml/SUMOSAXHandler.h>
29 : #include <utils/common/UtilExceptions.h>
30 : #include <utils/common/StringUtils.h>
31 : #include <utils/common/ToString.h>
32 : #include <utils/common/StringUtils.h>
33 : #include <utils/common/MsgHandler.h>
34 : #include <utils/common/SUMOVehicleClass.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/NBLoadedSUMOTLDef.h>
45 : #include <netbuild/NBTrafficLightLogicCont.h>
46 : #include <utils/xml/SUMOXMLDefinitions.h>
47 : #include <utils/geom/GeoConvHelper.h>
48 : #include <utils/geom/GeomConvHelper.h>
49 : #include <foreign/eulerspiral/odrSpiral.h>
50 : #include <utils/options/OptionsCont.h>
51 : #include <utils/common/FileHelpers.h>
52 : #include <utils/xml/SUMOXMLDefinitions.h>
53 : #include <utils/xml/XMLSubSys.h>
54 : #include <utils/geom/Boundary.h>
55 : #include "NILoader.h"
56 : #include "NIImporter_OpenDrive.h"
57 :
58 : //#define DEBUG_VARIABLE_WIDTHS
59 : //#define DEBUG_VARIABLE_SPEED
60 : //#define DEBUG_CONNECTIONS
61 : //#define DEBUG_SPIRAL
62 : //#define DEBUG_INTERNALSHAPES
63 : //#define DEBUG_SHAPE
64 :
65 : #define DEBUG_ID ""
66 : #define DEBUG_COND(road) ((road)->id == DEBUG_ID)
67 : #define DEBUG_COND2(edgeID) (StringUtils::startsWith((edgeID), DEBUG_ID))
68 : #define DEBUG_COND3(roadID) (roadID == DEBUG_ID)
69 : //#define DEBUG_COND(road) (true)
70 : //#define DEBUG_COND2(edgeID) (true)
71 : //#define DEBUG_COND3(roadID) (true)
72 :
73 : // ===========================================================================
74 : // definitions
75 : // ===========================================================================
76 :
77 : // ===========================================================================
78 : // static variables
79 : // ===========================================================================
80 : SequentialStringBijection::Entry NIImporter_OpenDrive::openDriveTags[] = {
81 : { "header", NIImporter_OpenDrive::OPENDRIVE_TAG_HEADER },
82 : { "road", NIImporter_OpenDrive::OPENDRIVE_TAG_ROAD },
83 : { "predecessor", NIImporter_OpenDrive::OPENDRIVE_TAG_PREDECESSOR },
84 : { "successor", NIImporter_OpenDrive::OPENDRIVE_TAG_SUCCESSOR },
85 : { "geometry", NIImporter_OpenDrive::OPENDRIVE_TAG_GEOMETRY },
86 : { "line", NIImporter_OpenDrive::OPENDRIVE_TAG_LINE },
87 : { "spiral", NIImporter_OpenDrive::OPENDRIVE_TAG_SPIRAL },
88 : { "arc", NIImporter_OpenDrive::OPENDRIVE_TAG_ARC },
89 : { "poly3", NIImporter_OpenDrive::OPENDRIVE_TAG_POLY3 },
90 : { "paramPoly3", NIImporter_OpenDrive::OPENDRIVE_TAG_PARAMPOLY3 },
91 : { "laneSection", NIImporter_OpenDrive::OPENDRIVE_TAG_LANESECTION },
92 : { "laneOffset", NIImporter_OpenDrive::OPENDRIVE_TAG_LANEOFFSET },
93 : { "left", NIImporter_OpenDrive::OPENDRIVE_TAG_LEFT },
94 : { "center", NIImporter_OpenDrive::OPENDRIVE_TAG_CENTER },
95 : { "right", NIImporter_OpenDrive::OPENDRIVE_TAG_RIGHT },
96 : { "lane", NIImporter_OpenDrive::OPENDRIVE_TAG_LANE },
97 : { "access", NIImporter_OpenDrive::OPENDRIVE_TAG_ACCESS },
98 : { "signal", NIImporter_OpenDrive::OPENDRIVE_TAG_SIGNAL },
99 : { "signalReference", NIImporter_OpenDrive::OPENDRIVE_TAG_SIGNALREFERENCE },
100 : { "controller", NIImporter_OpenDrive::OPENDRIVE_TAG_CONTROLLER },
101 : { "control", NIImporter_OpenDrive::OPENDRIVE_TAG_CONTROL },
102 : { "validity", NIImporter_OpenDrive::OPENDRIVE_TAG_VALIDITY },
103 : { "semantics", NIImporter_OpenDrive::OPENDRIVE_TAG_SEMANTICS },
104 : { "priority", NIImporter_OpenDrive::OPENDRIVE_TAG_PRIORITY },
105 : { "junction", NIImporter_OpenDrive::OPENDRIVE_TAG_JUNCTION },
106 : { "connection", NIImporter_OpenDrive::OPENDRIVE_TAG_CONNECTION },
107 : { "laneLink", NIImporter_OpenDrive::OPENDRIVE_TAG_LANELINK },
108 : { "width", NIImporter_OpenDrive::OPENDRIVE_TAG_WIDTH },
109 : { "speed", NIImporter_OpenDrive::OPENDRIVE_TAG_SPEED },
110 : { "elevation", NIImporter_OpenDrive::OPENDRIVE_TAG_ELEVATION },
111 : { "geoReference", NIImporter_OpenDrive::OPENDRIVE_TAG_GEOREFERENCE },
112 : { "offset", NIImporter_OpenDrive::OPENDRIVE_TAG_OFFSET },
113 : { "object", NIImporter_OpenDrive::OPENDRIVE_TAG_OBJECT },
114 : { "repeat", NIImporter_OpenDrive::OPENDRIVE_TAG_REPEAT },
115 : { "include", NIImporter_OpenDrive::OPENDRIVE_TAG_INCLUDE },
116 :
117 : { "", NIImporter_OpenDrive::OPENDRIVE_TAG_NOTHING }
118 : };
119 :
120 :
121 : SequentialStringBijection::Entry NIImporter_OpenDrive::openDriveAttrs[] = {
122 : { "revMajor", NIImporter_OpenDrive::OPENDRIVE_ATTR_REVMAJOR },
123 : { "revMinor", NIImporter_OpenDrive::OPENDRIVE_ATTR_REVMINOR },
124 : { "id", NIImporter_OpenDrive::OPENDRIVE_ATTR_ID },
125 : { "length", NIImporter_OpenDrive::OPENDRIVE_ATTR_LENGTH },
126 : { "width", NIImporter_OpenDrive::OPENDRIVE_ATTR_WIDTH },
127 : { "radius", NIImporter_OpenDrive::OPENDRIVE_ATTR_RADIUS },
128 : { "distance", NIImporter_OpenDrive::OPENDRIVE_ATTR_DISTANCE },
129 : { "tStart", NIImporter_OpenDrive::OPENDRIVE_ATTR_TSTART },
130 : { "tEnd", NIImporter_OpenDrive::OPENDRIVE_ATTR_TEND },
131 : { "widthStart", NIImporter_OpenDrive::OPENDRIVE_ATTR_WIDTHSTART },
132 : { "widthEnd", NIImporter_OpenDrive::OPENDRIVE_ATTR_WIDTHEND },
133 : { "junction", NIImporter_OpenDrive::OPENDRIVE_ATTR_JUNCTION },
134 : { "elementType", NIImporter_OpenDrive::OPENDRIVE_ATTR_ELEMENTTYPE },
135 : { "elementId", NIImporter_OpenDrive::OPENDRIVE_ATTR_ELEMENTID },
136 : { "contactPoint", NIImporter_OpenDrive::OPENDRIVE_ATTR_CONTACTPOINT },
137 : { "s", NIImporter_OpenDrive::OPENDRIVE_ATTR_S },
138 : { "t", NIImporter_OpenDrive::OPENDRIVE_ATTR_T },
139 : { "x", NIImporter_OpenDrive::OPENDRIVE_ATTR_X },
140 : { "y", NIImporter_OpenDrive::OPENDRIVE_ATTR_Y },
141 : { "z", NIImporter_OpenDrive::OPENDRIVE_ATTR_Z },
142 : { "hdg", NIImporter_OpenDrive::OPENDRIVE_ATTR_HDG },
143 : { "curvStart", NIImporter_OpenDrive::OPENDRIVE_ATTR_CURVSTART },
144 : { "curvEnd", NIImporter_OpenDrive::OPENDRIVE_ATTR_CURVEND },
145 : { "curvature", NIImporter_OpenDrive::OPENDRIVE_ATTR_CURVATURE },
146 : { "a", NIImporter_OpenDrive::OPENDRIVE_ATTR_A },
147 : { "b", NIImporter_OpenDrive::OPENDRIVE_ATTR_B },
148 : { "c", NIImporter_OpenDrive::OPENDRIVE_ATTR_C },
149 : { "d", NIImporter_OpenDrive::OPENDRIVE_ATTR_D },
150 : { "aU", NIImporter_OpenDrive::OPENDRIVE_ATTR_AU },
151 : { "bU", NIImporter_OpenDrive::OPENDRIVE_ATTR_BU },
152 : { "cU", NIImporter_OpenDrive::OPENDRIVE_ATTR_CU },
153 : { "dU", NIImporter_OpenDrive::OPENDRIVE_ATTR_DU },
154 : { "aV", NIImporter_OpenDrive::OPENDRIVE_ATTR_AV },
155 : { "bV", NIImporter_OpenDrive::OPENDRIVE_ATTR_BV },
156 : { "cV", NIImporter_OpenDrive::OPENDRIVE_ATTR_CV },
157 : { "dV", NIImporter_OpenDrive::OPENDRIVE_ATTR_DV },
158 : { "pRange", NIImporter_OpenDrive::OPENDRIVE_ATTR_PRANGE },
159 : { "type", NIImporter_OpenDrive::OPENDRIVE_ATTR_TYPE },
160 : { "level", NIImporter_OpenDrive::OPENDRIVE_ATTR_LEVEL },
161 : { "orientation", NIImporter_OpenDrive::OPENDRIVE_ATTR_ORIENTATION },
162 : { "dynamic", NIImporter_OpenDrive::OPENDRIVE_ATTR_DYNAMIC },
163 : { "incomingRoad", NIImporter_OpenDrive::OPENDRIVE_ATTR_INCOMINGROAD },
164 : { "connectingRoad", NIImporter_OpenDrive::OPENDRIVE_ATTR_CONNECTINGROAD },
165 : { "from", NIImporter_OpenDrive::OPENDRIVE_ATTR_FROM },
166 : { "to", NIImporter_OpenDrive::OPENDRIVE_ATTR_TO },
167 : { "fromLane", NIImporter_OpenDrive::OPENDRIVE_ATTR_FROMLANE },
168 : { "toLane", NIImporter_OpenDrive::OPENDRIVE_ATTR_TOLANE },
169 : { "max", NIImporter_OpenDrive::OPENDRIVE_ATTR_MAX },
170 : { "sOffset", NIImporter_OpenDrive::OPENDRIVE_ATTR_SOFFSET },
171 : { "rule", NIImporter_OpenDrive::OPENDRIVE_ATTR_RULE },
172 : { "restriction", NIImporter_OpenDrive::OPENDRIVE_ATTR_RESTRICTION },
173 : { "name", NIImporter_OpenDrive::OPENDRIVE_ATTR_NAME },
174 : { "signalId", NIImporter_OpenDrive::OPENDRIVE_ATTR_SIGNALID },
175 : { "file", NIImporter_OpenDrive::OPENDRIVE_ATTR_FILE },
176 : // towards xodr v1.4 speed:unit
177 : { "unit", NIImporter_OpenDrive::OPENDRIVE_ATTR_UNIT },
178 :
179 : { "", NIImporter_OpenDrive::OPENDRIVE_ATTR_NOTHING }
180 : };
181 :
182 :
183 : bool NIImporter_OpenDrive::myImportAllTypes;
184 : bool NIImporter_OpenDrive::myImportWidths;
185 : double NIImporter_OpenDrive::myMinWidth;
186 : bool NIImporter_OpenDrive::myIgnoreMisplacedSignals;
187 : bool NIImporter_OpenDrive::myImportInternalShapes;
188 : NIImporter_OpenDrive::OpenDriveController NIImporter_OpenDrive::myDummyController("", "");
189 :
190 : // ===========================================================================
191 : // method definitions
192 : // ===========================================================================
193 : // ---------------------------------------------------------------------------
194 : // static methods (interface in this case)
195 : // ---------------------------------------------------------------------------
196 : void
197 2003 : NIImporter_OpenDrive::loadNetwork(const OptionsCont& oc, NBNetBuilder& nb) {
198 : // check whether the option is set properly and all files exist
199 4006 : if (!oc.isUsableFileList("opendrive-files")) {
200 1977 : return;
201 : }
202 : // prepare types
203 26 : myImportAllTypes = oc.getBool("opendrive.import-all-lanes");
204 26 : myImportWidths = !oc.getBool("opendrive.ignore-widths");
205 26 : myMinWidth = oc.getFloat("opendrive.min-width");
206 26 : myImportInternalShapes = oc.getBool("opendrive.internal-shapes");
207 26 : myIgnoreMisplacedSignals = oc.getBool("opendrive.ignore-misplaced-signals");
208 52 : const bool customLaneShapes = oc.getBool("opendrive.lane-shapes");
209 : NBTypeCont& tc = nb.getTypeCont();
210 : NBNodeCont& nc = nb.getNodeCont();
211 : // build the handler
212 : std::map<std::string, OpenDriveEdge*> edges;
213 26 : NIImporter_OpenDrive handler(nb.getTypeCont(), edges);
214 : handler.needsCharacterData();
215 : // parse file(s)
216 78 : for (const std::string& file : oc.getStringVector("opendrive-files")) {
217 26 : handler.setFileName(file);
218 78 : PROGRESS_BEGIN_MESSAGE("Parsing opendrive from '" + file + "'");
219 26 : XMLSubSys::runParser(handler, file, false, false, true);
220 26 : PROGRESS_DONE_MESSAGE();
221 : }
222 : // apply signal reference information
223 908 : for (auto& item : edges) {
224 912 : for (OpenDriveSignal& signal : item.second->signals) {
225 30 : if (signal.type == "") {
226 0 : if (handler.getSignals().count(signal.id) == 0) {
227 0 : WRITE_WARNINGF(TL("Could not find signal reference '%'."), signal.id);
228 : } else {
229 0 : const OpenDriveSignal& ref = handler.getSignals()[signal.id];
230 0 : signal.type = ref.type;
231 0 : signal.name = ref.name;
232 0 : signal.dynamic = ref.dynamic;
233 0 : signal.controller = ref.controller;
234 : }
235 : }
236 : }
237 : }
238 :
239 : // split inner/outer edges
240 : std::map<std::string, OpenDriveEdge*> innerEdges, outerEdges;
241 908 : for (std::map<std::string, OpenDriveEdge*>::iterator i = edges.begin(); i != edges.end(); ++i) {
242 882 : if ((*i).second->isInner) {
243 630 : innerEdges[(*i).first] = (*i).second;
244 : } else {
245 252 : outerEdges[(*i).first] = (*i).second;
246 : }
247 : }
248 :
249 : // convert geometries into a discretised representation
250 26 : computeShapes(edges);
251 : // check whether lane sections are valid and whether further must be introduced
252 26 : revisitLaneSections(tc, edges);
253 :
254 : // -------------------------
255 : // node building
256 : // -------------------------
257 : // build nodes#1
258 : // look at all links which belong to a node, collect their bounding boxes
259 : // and place the node in the middle of this bounding box
260 : std::map<std::string, Boundary> posMap;
261 : std::map<std::string, std::string> edge2junction;
262 : std::vector<NodeSet> joinedNodeIDs;
263 : // compute node positions
264 656 : for (std::map<std::string, OpenDriveEdge*>::iterator i = innerEdges.begin(); i != innerEdges.end(); ++i) {
265 630 : OpenDriveEdge* e = (*i).second;
266 : assert(e->junction != "-1" && e->junction != "");
267 630 : edge2junction[e->id] = e->junction;
268 630 : if (posMap.find(e->junction) == posMap.end()) {
269 85 : posMap[e->junction] = Boundary();
270 : }
271 : // Check if geometry is valid before adding to boundary
272 630 : if (e->geom.size() > 0) {
273 630 : Boundary geomBoundary = e->geom.getBoxBoundary();
274 : // Only add if the boundary doesn't contain NaN values
275 1260 : if (geomBoundary.isInitialised() &&
276 1890 : !std::isnan(geomBoundary.xmin()) && !std::isnan(geomBoundary.xmax()) &&
277 1890 : !std::isnan(geomBoundary.ymin()) && !std::isnan(geomBoundary.ymax())) {
278 630 : posMap[e->junction].add(geomBoundary);
279 : } else {
280 0 : WRITE_WARNINGF(TL("Ignoring invalid geometry for inner edge '%' (xodr road '%') in junction '%'."),
281 : e->id, e->id, e->junction);
282 : }
283 : } else {
284 0 : WRITE_WARNINGF(TL("Inner edge '%' (xodr road '%') in junction '%' has no geometry."),
285 : e->id, e->id, e->junction);
286 : }
287 : }
288 : // build nodes
289 111 : for (std::map<std::string, Boundary>::iterator i = posMap.begin(); i != posMap.end(); ++i) {
290 : //std::cout << " import node=" << (*i).first << " z=" << (*i).second.getCenter() << " boundary=" << (*i).second << "\n";
291 : // Check if the boundary was properly initialized
292 85 : if (!(*i).second.isInitialised()) {
293 0 : WRITE_ERRORF(TL("Junction '%' has no valid inner edges to determine its position."), (*i).first);
294 0 : throw ProcessError(TLF("Could not determine position for junction '%'.", (*i).first));
295 : }
296 85 : Position center = (*i).second.getCenter();
297 : // Double-check that the center position is valid
298 : if (center.isNAN()) {
299 0 : WRITE_ERRORF(TL("Junction '%' resulted in invalid (NaN) position."), (*i).first);
300 0 : throw ProcessError(TLF("Could not compute valid position for junction '%'.", (*i).first));
301 : }
302 85 : if (!nb.getNodeCont().insert((*i).first, center)) {
303 0 : throw ProcessError(TLF("Could not add node '%'.", (*i).first));
304 : }
305 : }
306 : // assign built nodes
307 278 : for (std::map<std::string, OpenDriveEdge*>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
308 252 : OpenDriveEdge* e = (*i).second;
309 678 : for (std::vector<OpenDriveLink>::iterator j = e->links.begin(); j != e->links.end(); ++j) {
310 : OpenDriveLink& l = *j;
311 426 : const std::string& nid = l.elementID;
312 426 : if (l.elementType != OPENDRIVE_ET_ROAD) {
313 426 : if (nb.getNodeCont().retrieve(nid) == nullptr) {
314 : // not yet seen, build (possibly a junction without connections)
315 3 : Position pos = l.linkType == OPENDRIVE_LT_SUCCESSOR ? e->geom[-1] : e->geom[0];
316 3 : if (!nb.getNodeCont().insert(nid, pos)) {
317 0 : throw ProcessError(TLF("Could not build node '%'.", nid));
318 : }
319 : }
320 : // set node information
321 426 : setNodeSecure(nb.getNodeCont(), *e, l.elementID, l.linkType, joinedNodeIDs);
322 426 : continue;
323 426 : }
324 0 : if (edge2junction.find(l.elementID) != edge2junction.end()) {
325 : // set node information of an internal road
326 0 : setNodeSecure(nb.getNodeCont(), *e, edge2junction[l.elementID], l.linkType, joinedNodeIDs);
327 0 : continue;
328 : }
329 : }
330 : }
331 : // we should now have all nodes set for links which are not outer edge-to-outer edge links
332 :
333 :
334 : // build nodes#2
335 : // build nodes for all outer edge-to-outer edge connections
336 278 : for (std::map<std::string, OpenDriveEdge*>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
337 252 : OpenDriveEdge* e = (*i).second;
338 678 : for (std::vector<OpenDriveLink>::iterator j = e->links.begin(); j != e->links.end(); ++j) {
339 : OpenDriveLink& l = *j;
340 426 : if (l.elementType != OPENDRIVE_ET_ROAD || edge2junction.find(l.elementID) != edge2junction.end()) {
341 : // is a connection to an internal edge, or a node, skip
342 426 : continue;
343 : }
344 : // we have a direct connection between to external edges
345 : std::string id1 = e->id;
346 : std::string id2 = l.elementID;
347 0 : if (id1 < id2) {
348 : std::swap(id1, id2);
349 : }
350 0 : std::string nid = id1 + "." + id2;
351 0 : if (nb.getNodeCont().retrieve(nid) == nullptr) {
352 : // not yet seen, build
353 0 : Position pos = l.linkType == OPENDRIVE_LT_SUCCESSOR ? e->geom[-1] : e->geom[0];
354 0 : if (!nb.getNodeCont().insert(nid, pos)) {
355 0 : throw ProcessError(TLF("Could not build node '%'.", nid));
356 : }
357 : }
358 : /* debug-stuff
359 : else {
360 : Position pos = l.linkType==OPENDRIVE_LT_SUCCESSOR ? e.geom[e.geom.size()-1] : e.geom[0];
361 : cout << nid << " " << pos << " " << nb.getNodeCont().retrieve(nid)->getPosition() << endl;
362 : }
363 : */
364 0 : setNodeSecure(nb.getNodeCont(), *e, nid, l.linkType, joinedNodeIDs);
365 : }
366 : }
367 : // we should now have start/end nodes for all outer edge-to-outer edge connections
368 :
369 :
370 : // build nodes#3
371 : // assign further nodes generated from inner-edges
372 : // these nodes have not been assigned earlier, because the connections are referenced in inner-edges
373 278 : for (std::map<std::string, OpenDriveEdge*>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
374 252 : OpenDriveEdge* e = (*i).second;
375 252 : if (e->to != nullptr && e->from != nullptr) {
376 200 : continue;
377 : }
378 2562 : for (std::map<std::string, OpenDriveEdge*>::iterator j = innerEdges.begin(); j != innerEdges.end(); ++j) {
379 2510 : OpenDriveEdge* ie = (*j).second;
380 7530 : for (std::vector<OpenDriveLink>::iterator k = ie->links.begin(); k != ie->links.end(); ++k) {
381 : OpenDriveLink& il = *k;
382 5020 : if (il.elementType != OPENDRIVE_ET_ROAD || il.elementID != e->id) {
383 : // not conneted to the currently investigated outer edge
384 4925 : continue;
385 : }
386 95 : std::string nid = edge2junction[ie->id];
387 95 : if (il.contactPoint == OPENDRIVE_CP_START) {
388 22 : setNodeSecure(nb.getNodeCont(), *e, nid, OPENDRIVE_LT_PREDECESSOR, joinedNodeIDs);
389 : } else {
390 73 : setNodeSecure(nb.getNodeCont(), *e, nid, OPENDRIVE_LT_SUCCESSOR, joinedNodeIDs);
391 : }
392 : }
393 : }
394 :
395 : }
396 :
397 : // build start/end nodes which were not defined previously
398 278 : for (std::map<std::string, OpenDriveEdge*>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
399 252 : OpenDriveEdge* e = (*i).second;
400 252 : if ((e->from == nullptr || e->to == nullptr) && e->geom.size() == 0) {
401 0 : continue;
402 : }
403 252 : if (e->from == nullptr) {
404 45 : const std::string nid = e->id + ".begin";
405 45 : e->from = getOrBuildNode(nid, e->geom.front(), nb.getNodeCont());
406 : }
407 252 : if (e->to == nullptr) {
408 33 : const std::string nid = e->id + ".end";
409 33 : e->to = getOrBuildNode(nid, e->geom.back(), nb.getNodeCont());
410 : }
411 : }
412 :
413 : std::map<NBNode*, NBNode*> joinedNodes;
414 26 : for (NodeSet& joined : joinedNodeIDs) {
415 : Position joinedPos(0, 0);
416 0 : for (NBNode* j : joined) {
417 : joinedPos.add(j->getPosition());
418 : }
419 0 : joinedPos.mul(1. / (double)joined.size());
420 0 : const std::string joinedID = nc.createClusterId(joined);
421 0 : if (!nc.insert(joinedID, joinedPos)) {
422 0 : throw ProcessError(TLF("Could not add node '%'.", joinedID));
423 : }
424 0 : NBNode* n = nc.retrieve(joinedID);
425 0 : for (NBNode* j : joined) {
426 0 : joinedNodes[j] = n;
427 : }
428 : }
429 278 : for (std::map<std::string, OpenDriveEdge*>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
430 252 : OpenDriveEdge* e = (*i).second;
431 : if (joinedNodes.count(e->from) != 0) {
432 0 : nc.extract(e->from, true);
433 0 : e->from = joinedNodes[e->from];
434 : }
435 : if (joinedNodes.count(e->to) != 0) {
436 0 : nc.extract(e->to, true);
437 0 : e->to = joinedNodes[e->to];
438 : }
439 : }
440 :
441 :
442 : // -------------------------
443 : // edge building
444 : // -------------------------
445 26 : const double defaultSpeed = tc.getEdgeTypeSpeed("");
446 26 : const bool saveOrigIDs = OptionsCont::getOptions().getBool("output.original-names");
447 52 : const bool positionIDs = OptionsCont::getOptions().getBool("opendrive.position-ids");
448 : // lane-id-map sumoEdge,sumoLaneIndex->odrLaneIndex
449 : std::map<std::pair<NBEdge*, int>, int> laneIndexMap;
450 : // build edges
451 278 : for (std::map<std::string, OpenDriveEdge*>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
452 252 : OpenDriveEdge* e = (*i).second;
453 252 : if (e->geom.size() < 2) {
454 0 : WRITE_WARNINGF(TL("Ignoring road '%' without geometry."), e->id);
455 0 : continue;
456 : }
457 : bool lanesBuilt = false;
458 :
459 : // go along the lane sections, build a node in between of each pair
460 :
461 : /// @todo: One could think of determining whether lane sections may be joined when being equal in SUMO's sense
462 : /// Their naming would have to be updated, too, also in TraCI
463 :
464 : /// @todo: probably, the lane offsets to the center are not right
465 252 : NBNode* sFrom = e->from;
466 252 : NBNode* sTo = e->to;
467 252 : int priorityR = e->getPriority(OPENDRIVE_TAG_RIGHT);
468 252 : int priorityL = e->getPriority(OPENDRIVE_TAG_LEFT);
469 : double sB = 0;
470 : double sE = e->length;
471 : // 0-length geometries are possible if only the inner points are represented
472 : PositionVector geomWithOffset = e->geom;
473 252 : if (e->laneOffsets.size() > 0) {
474 : try {
475 3 : geomWithOffset.move2sideCustom(e->laneOffsets);
476 : //std::cout << " e=" << e->id << " offsets=" << e->laneOffsets << " geom=" << e->geom << " geom2=" << geomWithOffset << "\n";
477 0 : } catch (InvalidArgument&) {
478 0 : WRITE_WARNINGF(TL("Could not apply laneOffsets for edge '%'"), e->id);
479 0 : }
480 : }
481 : #ifdef DEBUG_SHAPE
482 : if (DEBUG_COND3(e->id)) {
483 : std::cout << " geomWithOffset=" << geomWithOffset << "\n";
484 : }
485 : #endif
486 252 : const double length2D = geomWithOffset.length2D();
487 252 : double cF = length2D == 0 ? 1 : e->length / length2D;
488 : NBEdge* prevRight = nullptr;
489 : NBEdge* prevLeft = nullptr;
490 :
491 : // starting at the same node as ending, and no lane sections?
492 252 : if (sFrom == sTo && e->laneSections.size() == 1) {
493 : // --> loop, split!
494 0 : OpenDriveLaneSection ls = e->laneSections[0];
495 0 : ls.s = e->length / 2.;
496 0 : e->laneSections.push_back(ls);
497 0 : WRITE_WARNING("Edge '" + e->id + "' has to be split as it connects same junctions.")
498 0 : }
499 252 : sanitizeWidths(e);
500 252 : if (myMinWidth > 0) {
501 252 : const double minDist = oc.getFloat("opendrive.curve-resolution");
502 252 : splitMinWidths(e, tc, minDist);
503 : }
504 :
505 : // build along lane sections
506 : int sectionIndex = 0;
507 582 : for (std::vector<OpenDriveLaneSection>::iterator j = e->laneSections.begin(); j != e->laneSections.end(); ++j) {
508 : // add internal node if needed
509 330 : if (j == e->laneSections.end() - 1) {
510 251 : sTo = e->to;
511 251 : sE = e->length / cF;
512 : } else {
513 79 : double nextS = (j + 1)->s;
514 250 : const std::string nodeID = e->id + (positionIDs ? "." + toString(nextS) : "#" + toString(sectionIndex + 1));
515 79 : sTo = new NBNode(nodeID, geomWithOffset.positionAtOffset(nextS));
516 79 : if (!nb.getNodeCont().insert(sTo)) {
517 0 : throw ProcessError(TLF("Could not add node '%'.", sTo->getID()));
518 : }
519 79 : sE = nextS / cF;
520 : }
521 330 : const PositionVector geom = geomWithOffset.getSubpart2D(sB, sE).simplified2(false);
522 : std::string id = e->id;
523 330 : if (positionIDs) {
524 74 : if (sFrom != e->from || sTo != e->to) {
525 148 : id = id + "." + toString((*j).s);
526 0 : } else if (e->laneSections.size() == 1) {
527 0 : id = id + ".0.00";
528 : }
529 256 : } else if (e->laneSections.size() > 1) {
530 36 : id = id + "#" + toString(sectionIndex++);
531 : }
532 : #ifdef DEBUG_VARIABLE_WIDTHS
533 : if (DEBUG_COND(e)) {
534 : std::cout << " id=" << id << " sB=" << sB << " sE=" << sE << " geom=" << geom << "\n";
535 : }
536 : #endif
537 :
538 : // build lanes to right
539 : NBEdge* currRight = nullptr;
540 330 : if ((*j).rightLaneNumber > 0) {
541 327 : std::vector<double> offsets(geom.size(), 0);
542 : bool useOffsets = false;
543 : PositionVector rightGeom = geom;
544 : #ifdef DEBUG_SHAPE
545 : if (DEBUG_COND3(e->id)) {
546 : gDebugFlag1 = true;
547 : }
548 : #endif
549 327 : rightGeom.move2side((*j).discardedInnerWidthRight);
550 : #ifdef DEBUG_SHAPE
551 : if (DEBUG_COND3(e->id)) {
552 : std::cout << " -" << id << "_geom=" << geom << " -" << id << "_rightGeom=" << rightGeom << "\n";
553 : gDebugFlag1 = false;
554 : }
555 : #endif
556 : PositionVector laneGeom = rightGeom;
557 654 : currRight = new NBEdge("-" + id, sFrom, sTo, (*j).rightType, defaultSpeed, NBEdge::UNSPECIFIED_FRICTION, (*j).rightLaneNumber, priorityR,
558 981 : NBEdge::UNSPECIFIED_WIDTH, NBEdge::UNSPECIFIED_OFFSET, rightGeom, LaneSpreadFunction::RIGHT, e->streetName, "", true);
559 : lanesBuilt = true;
560 327 : std::vector<OpenDriveLane>& lanes = (*j).lanesByDir[OPENDRIVE_TAG_RIGHT];
561 327 : std::sort(lanes.begin(), lanes.end(), LaneSorter());
562 969 : for (const OpenDriveLane& odl : lanes) {
563 : std::map<int, int>::const_iterator lp = (*j).laneMap.find(odl.id);
564 642 : if (lp != (*j).laneMap.end()) {
565 379 : int sumoLaneIndex = lp->second;
566 379 : setLaneAttributes(e, currRight->getLaneStruct(sumoLaneIndex), odl, saveOrigIDs, tc);
567 379 : laneIndexMap[std::make_pair(currRight, sumoLaneIndex)] = odl.id;
568 379 : if (useOffsets) {
569 : PositionVector laneShape = laneGeom;
570 0 : laneShape.move2sideCustom(offsets);
571 : currRight->getLaneStruct(sumoLaneIndex).customShape = laneShape;
572 0 : }
573 263 : } else if (customLaneShapes) {
574 : useOffsets = true;
575 : }
576 379 : if (customLaneShapes) {
577 0 : addOffsets(false, laneGeom, odl.widthData, e->id + "_" + toString(odl.id), offsets);
578 : }
579 : }
580 327 : if (!nb.getEdgeCont().insert(currRight, myImportAllTypes)) {
581 0 : throw ProcessError(TLF("Could not add edge '%'.", currRight->getID()));
582 : }
583 654 : if (nb.getEdgeCont().wasIgnored("-" + id)) {
584 : prevRight = nullptr;
585 : } else {
586 : // connect lane sections
587 289 : if (prevRight != nullptr) {
588 46 : std::map<int, int> connections = (*j).getInnerConnections(OPENDRIVE_TAG_RIGHT, *(j - 1));
589 106 : for (std::map<int, int>::const_iterator k = connections.begin(); k != connections.end(); ++k) {
590 : #ifdef DEBUG_CONNECTIONS
591 : if (DEBUG_COND(e)) {
592 : std::cout << "addCon1 from=" << prevRight->getID() << "_" << (*k).first << " to=" << currRight->getID() << "_" << (*k).second << "\n";
593 : }
594 : #endif
595 120 : prevRight->addLane2LaneConnection((*k).first, currRight, (*k).second, NBEdge::Lane2LaneInfoType::VALIDATED);
596 : }
597 : }
598 : prevRight = currRight;
599 : }
600 327 : }
601 :
602 : // build lanes to left
603 : NBEdge* currLeft = nullptr;
604 330 : if ((*j).leftLaneNumber > 0) {
605 109 : std::vector<double> offsets(geom.size(), 0);
606 : bool useOffsets = false;
607 : PositionVector leftGeom = geom;
608 109 : leftGeom.move2side(-(*j).discardedInnerWidthLeft);
609 : PositionVector laneGeom = leftGeom;
610 : #ifdef DEBUG_SHAPE
611 : if (DEBUG_COND3(e->id)) {
612 : std::cout << " " << id << "_geom=" << geom << " " << id << "_leftGeom=" << leftGeom << "\n";
613 : }
614 : #endif
615 : currLeft = new NBEdge(id, sTo, sFrom, (*j).leftType, defaultSpeed, NBEdge::UNSPECIFIED_FRICTION, (*j).leftLaneNumber, priorityL,
616 327 : NBEdge::UNSPECIFIED_WIDTH, NBEdge::UNSPECIFIED_OFFSET, leftGeom.reverse(), LaneSpreadFunction::RIGHT, e->streetName, "", true);
617 : lanesBuilt = true;
618 109 : std::vector<OpenDriveLane>& lanes = (*j).lanesByDir[OPENDRIVE_TAG_LEFT];
619 109 : std::sort(lanes.begin(), lanes.end(), LaneSorter());
620 418 : for (std::vector<OpenDriveLane>::const_iterator k = lanes.begin(); k != lanes.end(); ++k) {
621 : std::map<int, int>::const_iterator lp = (*j).laneMap.find((*k).id);
622 309 : if (lp != (*j).laneMap.end()) {
623 141 : int sumoLaneIndex = lp->second;
624 141 : setLaneAttributes(e, currLeft->getLaneStruct(sumoLaneIndex), *k, saveOrigIDs, tc);
625 141 : laneIndexMap[std::make_pair(currLeft, sumoLaneIndex)] = (*k).id;
626 141 : if (useOffsets) {
627 : PositionVector laneShape = laneGeom;
628 0 : laneShape.move2sideCustom(offsets);
629 0 : currLeft->getLaneStruct(sumoLaneIndex).customShape = laneShape.reverse();
630 0 : }
631 168 : } else if (customLaneShapes) {
632 : useOffsets = true;
633 : }
634 141 : if (customLaneShapes) {
635 0 : addOffsets(true, laneGeom, (*k).widthData, e->id + "_" + toString((*k).id), offsets);
636 : }
637 : }
638 109 : if (!nb.getEdgeCont().insert(currLeft, myImportAllTypes)) {
639 0 : throw ProcessError(TLF("Could not add edge '%'.", currLeft->getID()));
640 : }
641 109 : if (nb.getEdgeCont().wasIgnored(id)) {
642 : prevLeft = nullptr;
643 : } else {
644 : // connect lane sections
645 71 : if (prevLeft != nullptr) {
646 46 : std::map<int, int> connections = (*j).getInnerConnections(OPENDRIVE_TAG_LEFT, *(j - 1));
647 108 : for (std::map<int, int>::const_iterator k = connections.begin(); k != connections.end(); ++k) {
648 : #ifdef DEBUG_CONNECTIONS
649 : if (DEBUG_COND(e)) {
650 : std::cout << "addCon2 from=" << currLeft->getID() << "_" << (*k).first << " to=" << prevLeft->getID() << "_" << (*k).second << "\n";
651 : }
652 : #endif
653 124 : currLeft->addLane2LaneConnection((*k).first, prevLeft, (*k).second, NBEdge::Lane2LaneInfoType::VALIDATED);
654 : }
655 : }
656 : prevLeft = currLeft;
657 : }
658 109 : }
659 330 : (*j).sumoID = id;
660 :
661 :
662 : sB = sE;
663 : sFrom = sTo;
664 330 : }
665 504 : if (oc.isSet("polygon-output")) {
666 0 : writeRoadObjects(e);
667 : }
668 252 : if (!lanesBuilt) {
669 3 : WRITE_WARNINGF(TL("Edge '%' has no lanes."), e->id);
670 : }
671 252 : }
672 52 : if (oc.isSet("polygon-output")) {
673 0 : for (auto item : innerEdges) {
674 0 : writeRoadObjects(item.second);
675 : }
676 : }
677 :
678 : // -------------------------
679 : // connections building
680 : // -------------------------
681 : // generate explicit lane-to-lane connections
682 908 : for (std::map<std::string, OpenDriveEdge*>::iterator i = edges.begin(); i != edges.end(); ++i) {
683 882 : setEdgeLinks2(*(*i).second, edges);
684 : }
685 : // compute connections across intersections, if any
686 : std::vector<Connection> connections2;
687 908 : for (std::map<std::string, OpenDriveEdge*>::iterator j = edges.begin(); j != edges.end(); ++j) {
688 882 : const std::set<Connection>& conns = (*j).second->connections;
689 :
690 2277 : for (std::set<Connection>::const_iterator i = conns.begin(); i != conns.end(); ++i) {
691 1395 : if (innerEdges.find((*i).fromEdge) != innerEdges.end()) {
692 : // connections starting at inner edges are processed by starting from outer edges
693 685 : continue;
694 : }
695 710 : if (innerEdges.find((*i).toEdge) != innerEdges.end()) {
696 : std::set<Connection> seen;
697 710 : buildConnectionsToOuter(*i, innerEdges, edges, tc, connections2, seen);
698 : } else {
699 0 : connections2.push_back(*i);
700 : }
701 : }
702 : }
703 : // set connections
704 722 : for (std::vector<Connection>::const_iterator i = connections2.begin(); i != connections2.end(); ++i) {
705 : #ifdef DEBUG_CONNECTIONS
706 : std::cout << "connections2 " << (*i).getDescription() << "\n";
707 : #endif
708 : std::string fromEdge = (*i).fromEdge;
709 696 : if (edges.find(fromEdge) == edges.end()) {
710 0 : WRITE_WARNINGF(TL("While setting connections: from-edge '%' is not known."), fromEdge);
711 0 : continue;
712 : }
713 696 : OpenDriveEdge* odFrom = edges[fromEdge];
714 696 : int fromLane = (*i).fromLane;
715 696 : bool fromLast = ((*i).fromCP == OPENDRIVE_CP_END) && ((*i).fromLane < 0);
716 696 : fromEdge = fromLast ? odFrom->laneSections.back().sumoID : odFrom->laneSections[0].sumoID;
717 :
718 : std::string toEdge = (*i).toEdge;
719 696 : if (edges.find(toEdge) == edges.end()) {
720 0 : WRITE_WARNINGF(TL("While setting connections: to-edge '%' is not known."), toEdge);
721 0 : continue;
722 : }
723 :
724 696 : OpenDriveEdge* odTo = edges[toEdge];
725 696 : int toLane = (*i).toLane;
726 696 : bool toLast = ((*i).toCP == OPENDRIVE_CP_END) || ((*i).toLane > 0);
727 696 : toEdge = toLast ? odTo->laneSections.back().sumoID : odTo->laneSections[0].sumoID;
728 :
729 696 : if (fromLane == UNSET_CONNECTION) {
730 26 : continue;
731 : }
732 670 : if (fromLane < 0) {
733 1292 : fromEdge = reversedEdgeID(fromEdge);
734 : }
735 670 : if (toLane == UNSET_CONNECTION) {
736 0 : continue;
737 : }
738 670 : if (toLane < 0) {
739 1222 : toEdge = reversedEdgeID(toEdge);
740 : }
741 670 : fromLane = fromLast ? odFrom->laneSections.back().laneMap[fromLane] : odFrom->laneSections[0].laneMap[fromLane];
742 670 : toLane = toLast ? odTo->laneSections.back().laneMap[toLane] : odTo->laneSections[0].laneMap[toLane];
743 670 : NBEdge* from = nb.getEdgeCont().retrieve(fromEdge);
744 670 : NBEdge* to = nb.getEdgeCont().retrieve(toEdge);
745 670 : if (from == nullptr) {
746 42 : WRITE_WARNINGF(TL("Could not find fromEdge representation of '%' in connection '%'."), fromEdge, (*i).origID);
747 : }
748 670 : if (to == nullptr) {
749 42 : WRITE_WARNINGF(TL("Could not find fromEdge representation of '%' in connection '%'."), toEdge, (*i).origID);
750 : }
751 670 : if (from == nullptr || to == nullptr) {
752 14 : continue;
753 : }
754 :
755 : #ifdef DEBUG_CONNECTIONS
756 : if (DEBUG_COND2(from->getID())) {
757 : std::cout << "addCon3 from=" << from->getID() << "_" << fromLane << " to=" << to->getID() << "_" << toLane << "\n";
758 : }
759 : #endif
760 656 : from->addLane2LaneConnection(fromLane, to, toLane, NBEdge::Lane2LaneInfoType::USER, false, false,
761 : KEEPCLEAR_UNSPECIFIED,
762 : NBEdge::UNSPECIFIED_CONTPOS,
763 : NBEdge::UNSPECIFIED_VISIBILITY_DISTANCE,
764 : NBEdge::UNSPECIFIED_SPEED,
765 : NBEdge::UNSPECIFIED_FRICTION,
766 : NBEdge::UNSPECIFIED_LOADED_LENGTH,
767 656 : (*i).shape);
768 :
769 656 : if ((*i).origID != "" && saveOrigIDs) {
770 : // @todo: this is the most silly way to determine the connection
771 : std::vector<NBEdge::Connection>& cons = from->getConnections();
772 47 : for (std::vector<NBEdge::Connection>::iterator k = cons.begin(); k != cons.end(); ++k) {
773 40 : if ((*k).fromLane == fromLane && (*k).toEdge == to && (*k).toLane == toLane) {
774 32 : (*k).setParameter(SUMO_PARAM_ORIGID, (*i).origID + "_" + toString((*i).origLane));
775 16 : break;
776 : }
777 : }
778 : }
779 : }
780 :
781 :
782 : // -------------------------
783 : // traffic lights
784 : // -------------------------
785 : std::map<std::string, std::string> signal2junction;
786 : std::map<std::string, OpenDriveController>& controllers = handler.getControllers();
787 :
788 908 : for (const auto& it : edges) {
789 882 : const OpenDriveEdge* e = it.second;
790 912 : for (const OpenDriveSignal& signal : e->signals) { // signal for which junction?
791 30 : if (signal.controller.size() == 0) {
792 30 : continue;
793 : }
794 : std::string junctionID;
795 0 : for (auto connection : e->connections) {
796 0 : if ((connection.fromLane < 0 && signal.orientation < 0) || (connection.fromLane > 0 && signal.orientation > 0)) {
797 : continue;
798 : }
799 0 : if ((signal.minLane == 0 && signal.maxLane == 0) || (signal.maxLane >= connection.fromLane && signal.minLane <= connection.fromLane)) {
800 0 : const OpenDriveEdge* connectedEdge = edges[connection.toEdge];
801 0 : if (controllers[signal.controller].junction.size() > 0 && controllers[signal.controller].junction != connectedEdge->junction) {
802 0 : WRITE_WARNINGF(TL("Controlling multiple junctions by the same controller '%' is currently not implemented."), signal.controller);
803 : }
804 0 : controllers[signal.controller].junction = connectedEdge->junction;
805 : }
806 0 : }
807 : }
808 : }
809 :
810 52 : const bool importSignalGroups = oc.getBool("opendrive.signal-groups");
811 908 : for (std::map<std::string, OpenDriveEdge*>::iterator i = edges.begin(); i != edges.end(); ++i) {
812 882 : OpenDriveEdge* e = (*i).second;
813 912 : for (const OpenDriveSignal& signal : e->signals) {
814 : int intType = -1;
815 : try {
816 30 : intType = StringUtils::toInt(signal.type);
817 5 : } catch (NumberFormatException&) {}
818 25 : if (intType < 1000001 || (intType > 1000013 && intType != 1000020) || intType == 1000008) {
819 : // not a traffic_light (Section 6.11)
820 5 : continue;
821 : }
822 25 : if (e->laneSections.size() == 0) {
823 0 : WRITE_WARNINGF(TL("Edge '%' has signals but no lane sections."), e->id);
824 0 : continue;
825 : }
826 : std::vector<OpenDriveLaneSection>::iterator k = e->laneSections.begin();
827 : bool found = false;
828 25 : for (; k != e->laneSections.end() - 1 && !found;) {
829 0 : if (signal.s > (*k).s && signal.s <= (*(k + 1)).s) {
830 : found = true;
831 : } else {
832 : ++k;
833 : }
834 : }
835 :
836 : std::string id = (*k).sumoID;
837 25 : if (id == "") {
838 : // traffic light on connecting road
839 0 : if (e->junction != "") {
840 : //WRITE_WARNINGF(TL("Found a traffic light signal on an internal edge; will not build it (original edge id='%')."), e->id);
841 : std::string fromID, toID;
842 0 : for (std::vector<OpenDriveLink>::const_iterator l = e->links.begin(); l != e->links.end(); ++l) {
843 0 : if ((*l).linkType == OPENDRIVE_LT_PREDECESSOR && (*l).elementType == OPENDRIVE_ET_ROAD) {
844 0 : if (fromID != "") {
845 0 : WRITE_WARNING(TL("Ambiguous start of connection."));
846 : }
847 0 : const OpenDriveEdge* const ode = edges[(*l).elementID];
848 0 : if ((*l).contactPoint == OPENDRIVE_CP_START) {
849 0 : fromID = ode->laneSections[0].sumoID;
850 0 : if (signal.orientation < 0) {
851 0 : fromID = "-" + fromID;
852 : }
853 : } else {
854 0 : fromID = ode->laneSections.back().sumoID;
855 0 : if (signal.orientation > 0) {
856 0 : fromID = "-" + fromID;
857 : }
858 : }
859 : }
860 0 : if ((*l).linkType == OPENDRIVE_LT_SUCCESSOR && (*l).elementType == OPENDRIVE_ET_ROAD) {
861 0 : if (toID != "") {
862 0 : WRITE_WARNING(TL("Ambiguous end of connection."));
863 : }
864 0 : const OpenDriveEdge* const ode = edges[(*l).elementID];
865 0 : toID = (*l).contactPoint == OPENDRIVE_CP_START ? ode->laneSections[0].sumoID : ode->laneSections.back().sumoID;
866 : }
867 : }
868 : // figure out the correct combination of directions
869 : NBEdge* from;
870 : NBEdge* to;
871 0 : auto fromTo = retrieveSignalEdges(nb, fromID, toID, signal.minLane);
872 0 : from = fromTo.first;
873 0 : to = fromTo.second;
874 0 : if (from == nullptr) {
875 0 : WRITE_WARNINGF(TL("Could not find edge '%' for signal '%'."), fromID, signal.id);
876 0 : continue;
877 : }
878 :
879 : // consider signal validity to determine direction
880 0 : if (signal.maxLane != 0) {
881 0 : bool fromForward = from->getID()[0] == '-';
882 0 : bool lanesForward = signal.maxLane < 0;
883 0 : if (fromForward != lanesForward) {
884 : std::swap(fromID, toID);
885 :
886 0 : const auto& signalFromTo = retrieveSignalEdges(nb, fromID, toID, signal.minLane);
887 0 : from = signalFromTo.first;
888 0 : to = signalFromTo.second;
889 0 : if (from == nullptr) {
890 0 : WRITE_WARNINGF(TL("Could not find edge '%' for signal '%'."), fromID, signal.id);
891 0 : continue;
892 : }
893 : }
894 : }
895 0 : for (NBEdge::Connection& c : from->getConnections()) {
896 0 : if (c.toEdge == to) {
897 0 : int odLane = laneIndexMap[std::make_pair(from, c.fromLane)];
898 : //std::cout << " e=" << e->id << " from=" << from->getID() << " fromLane=" << c.fromLane << " odLane=" << odLane << " sMin=" << signal.minLane << " sMax=" << signal.maxLane << "\n";
899 0 : if (signal.minLane == 0 || (signal.minLane <= odLane && signal.maxLane >= odLane)) {
900 0 : if (c.hasParameter("signalID")) {
901 0 : c.setParameter("signalID", c.getParameter("signalID") + " " + signal.id);
902 : } else {
903 0 : c.setParameter("signalID", signal.id);
904 : }
905 : }
906 : // set tlIndex to allow signal groups (defined in OpenDRIVE controller elements)
907 0 : if (importSignalGroups) {
908 0 : const OpenDriveController& controller = handler.getController(signal.id);
909 0 : if (controller.id != "") {
910 0 : if (c.getParameter("controllerID") != "") {
911 0 : WRITE_WARNINGF(TL("The signaling of the connection from '%' to '%' (controller '%') is ambiguous because it is overwritten signal '%' and with controller '%'."), from->getID(), c.toEdge->getID(), c.getParameter("controllerID"), signal.id, controller.id);
912 : }
913 : //junctionsWithControllers.insert(from->getToNode()->getID());
914 0 : int tlIndex = handler.getTLIndexForController(controller.id);
915 0 : c.tlLinkIndex = tlIndex;
916 0 : c.setParameter("controllerID", controller.id);
917 : }
918 : }
919 : }
920 : }
921 0 : getTLSSecure(from, nb);
922 :
923 : //std::cout << "odrEdge=" << e->id << " fromID=" << fromID << " toID=" << toID << " from=" << from->getID() << " to=" << to->getID()
924 : // << " signal=" << signal.id << " minLane=" << signal.minLane << " maxLane=" << signal.maxLane << "\n";
925 : } else {
926 0 : WRITE_WARNINGF(TL("Found a traffic light signal on an unknown edge (original edge id='%')."), e->id);
927 0 : continue;
928 : }
929 : } else {
930 : // traffic light on normal road
931 25 : if (signal.orientation == 1) {
932 : // forward direction has negative lane indices and gets a negative prefix in sumo
933 50 : id = "-" + id;
934 : }
935 25 : NBEdge* edge = nb.getEdgeCont().retrieve(id);
936 25 : if (edge == nullptr) {
937 0 : WRITE_WARNINGF(TL("Could not find edge '%' for signal '%'."), id, signal.id);
938 : continue;
939 : }
940 :
941 : /// XXX consider lane validity
942 126 : for (NBEdge::Connection& c : edge->getConnections()) {
943 101 : int odLane = laneIndexMap[std::make_pair(edge, c.fromLane)];
944 101 : if (signal.minLane == 0 || (signal.minLane <= odLane && signal.maxLane >= odLane)) {
945 138 : if (c.hasParameter("signalID")) {
946 176 : c.setParameter("signalID", c.getParameter("signalID") + " " + signal.id);
947 : } else {
948 50 : c.setParameter("signalID", signal.id);
949 : }
950 : }
951 :
952 : // set tlIndex to allow signal groups (defined in OpenDRIVE controller elements)
953 101 : if (importSignalGroups) {
954 0 : const OpenDriveController& controller = handler.getController(signal.id);
955 0 : if (controller.id != "") {
956 0 : if (c.getParameter("controllerID") != "") {
957 0 : WRITE_WARNINGF(TL("The signaling of the connection from '%' to '%' (controller '%') is ambiguous because it is overwritten with signal '%' and controller '%'."), edge->getID(), c.toEdge->getID(), c.getParameter("controllerID"), signal.id, controller.id);
958 : }
959 : //junctionsWithControllers.insert(edge->getToNode()->getID());
960 0 : int tlIndex = handler.getTLIndexForController(controller.id);
961 0 : c.tlLinkIndex = tlIndex;
962 0 : c.setParameter("controllerID", controller.id);
963 : }
964 : }
965 : }
966 25 : getTLSSecure(edge, nb);
967 : //std::cout << "odrEdge=" << e->id << " sumoID=" << (*k).sumoID << " sumoEdge=" << edge->getID()
968 : // << " signal=" << signal.id << " minLane=" << signal.minLane << " maxLane=" << signal.maxLane << "\n";
969 : }
970 : // @note: tls 'signalID' parameters are set via NBTrafficLightLogicCont::setOpenDriveSignalParameters
971 : // @note: OpenDRIVE controllers are applied to the signal programs in NBTrafficLightLogicCont::applyOpenDriveControllers
972 : }
973 : }
974 :
975 : // -------------------------
976 : // clean up
977 : // -------------------------
978 908 : for (std::map<std::string, OpenDriveEdge*>::iterator i = edges.begin(); i != edges.end(); ++i) {
979 882 : delete (*i).second;
980 : }
981 78 : }
982 :
983 :
984 : void
985 0 : NIImporter_OpenDrive::writeRoadObjects(const OpenDriveEdge* e) {
986 0 : OptionsCont& oc = OptionsCont::getOptions();
987 0 : const bool writeGeo = GeoConvHelper::getLoaded().usingGeoProjection() && (
988 0 : oc.isDefault("proj.plain-geo") || oc.getBool("proj.plain-geo"));
989 0 : OutputDevice& dev = OutputDevice::getDevice(oc.getString("polygon-output"));
990 0 : dev.writeXMLHeader("additional", "additional_file.xsd");
991 : //SUMOPolygon poly("road_" + e->id, "road", RGBColor::BLUE, e->geom, true, false);
992 : //poly.writeXML(dev, false);
993 0 : for (auto& o : e->objects) {
994 0 : Position ref = e->geom.positionAtOffset2D(o.s, -o.t);
995 0 : if (o.radius >= 0) {
996 : // cicrular shape
997 : // GeoConvHelper::getFinal is not ready yet
998 0 : GeoConvHelper::getLoaded().cartesian2geo(ref);
999 0 : PointOfInterest POI(o.id, o.type, RGBColor::YELLOW, ref, true, "", -1, false, 0, SUMOXMLDefinitions::POIIcons.getString(POIIcon::NONE));
1000 0 : POI.setParameter("name", o.name);
1001 0 : POI.writeXML(dev, writeGeo);
1002 0 : } else {
1003 : // rectangular shape
1004 0 : PositionVector centerLine;
1005 0 : centerLine.push_back(Position(-o.length / 2, 0));
1006 0 : centerLine.push_back(Position(o.length / 2, 0));
1007 0 : double roadHdg = e->geom.rotationAtOffset(o.s);
1008 0 : centerLine.rotate2D(roadHdg + o.hdg);
1009 : //PointOfInterest poiRef("ref_" + o.id, "", RGBColor::CYAN, ref, false, "", 0, 0, Shape::DEFAULT_LAYER + 2);
1010 : //poiRef.writeXML(dev, false);
1011 0 : centerLine.add(ref);
1012 : //SUMOPolygon polyCenter("center_" + o.id, "", RGBColor::MAGENTA, centerLine, true, false, Shape::DEFAULT_LAYER + 1);
1013 : //polyCenter.writeXML(dev, false);
1014 0 : centerLine.move2side(o.width / 2);
1015 : PositionVector shape = centerLine;
1016 0 : centerLine.move2side(-o.width);
1017 0 : shape.append(centerLine.reverse(), POSITION_EPS);
1018 0 : if (writeGeo) {
1019 : // GeoConvHelper::getFinal is not ready yet
1020 0 : for (Position& p : shape) {
1021 0 : GeoConvHelper::getLoaded().cartesian2geo(p);
1022 : }
1023 : }
1024 0 : SUMOPolygon poly(o.id, o.type, RGBColor::YELLOW, shape, true, true, 1, 7);
1025 0 : poly.setParameter("name", o.name);
1026 0 : poly.writeXML(dev, writeGeo);
1027 0 : }
1028 : }
1029 0 : }
1030 :
1031 :
1032 :
1033 : std::pair<NBEdge*, NBEdge*>
1034 0 : NIImporter_OpenDrive::retrieveSignalEdges(NBNetBuilder& nb, const std::string& fromID, const std::string& toID, int signalMinLane) {
1035 : NBEdge* from;
1036 : NBEdge* to;
1037 : NBEdge* fromReverse;
1038 : NBEdge* toReverse;
1039 0 : from = nb.getEdgeCont().retrieve(fromID);
1040 0 : to = nb.getEdgeCont().retrieve(toID);
1041 0 : fromReverse = nb.getEdgeCont().retrieve(reversedEdgeID(fromID));
1042 0 : toReverse = nb.getEdgeCont().retrieve(reversedEdgeID(toID));
1043 :
1044 0 : if (from == nullptr) {
1045 : from = fromReverse;
1046 : }
1047 0 : if (to == nullptr) {
1048 : to = toReverse;
1049 : }
1050 0 : if (from != nullptr && to != nullptr && from->getToNode() != to->getFromNode()) {
1051 0 : if (from->getFromNode() == to->getToNode() && signalMinLane >= 0) {
1052 : std::swap(from, to);
1053 0 : } else if (fromReverse != nullptr && toReverse != nullptr && fromReverse->getToNode() == toReverse->getFromNode() && signalMinLane <= 0) {
1054 : from = fromReverse;
1055 : to = toReverse;
1056 : } else {
1057 : if (from->getFromNode() == to->getFromNode()
1058 0 : && fromReverse != nullptr && fromReverse->getToNode() == to->getFromNode()) {
1059 : from = fromReverse;
1060 : }
1061 : if (to->getToNode() == from->getToNode()
1062 0 : && toReverse != nullptr && toReverse->getFromNode() == from->getToNode()) {
1063 : to = toReverse;
1064 : }
1065 : }
1066 : }
1067 : //std::cout << fromID << " " << toID << " from=" << Named::getIDSecure(from) << " to=" << Named::getIDSecure(to) << " sMin=" << signalMinLane << "\n";
1068 0 : return std::make_pair(from, to);
1069 : }
1070 :
1071 :
1072 : NBTrafficLightDefinition*
1073 25 : NIImporter_OpenDrive::getTLSSecure(NBEdge* inEdge, /*const NBEdge::Connection& conn,*/ NBNetBuilder& nb) {
1074 : NBNode* toNode = inEdge->getToNode();
1075 25 : if (!toNode->isTLControlled()) {
1076 3 : TrafficLightType type = SUMOXMLDefinitions::TrafficLightTypes.get(OptionsCont::getOptions().getString("tls.default-type"));
1077 3 : NBOwnTLDef* tlDef = new NBOwnTLDef(toNode->getID(), toNode, 0, type);
1078 3 : if (!nb.getTLLogicCont().insert(tlDef)) {
1079 : // actually, nothing should fail here
1080 0 : delete tlDef;
1081 0 : throw ProcessError();
1082 : }
1083 3 : toNode->addTrafficLight(tlDef);
1084 : //tlDef->setSinglePhase();
1085 : }
1086 25 : return *toNode->getControllingTLS().begin();
1087 : }
1088 :
1089 : void
1090 520 : NIImporter_OpenDrive::setLaneAttributes(const OpenDriveEdge* e, NBEdge::Lane& sumoLane, const OpenDriveLane& odLane, bool saveOrigIDs, const NBTypeCont& tc) {
1091 520 : if (saveOrigIDs) {
1092 196 : sumoLane.setParameter(SUMO_PARAM_ORIGID, e->id + "_" + toString(odLane.id));
1093 : }
1094 520 : sumoLane.speed = odLane.speed != 0 ? odLane.speed : tc.getEdgeTypeSpeed(odLane.type);
1095 520 : sumoLane.permissions = odLane.permission > 0 ? odLane.permission : tc.getEdgeTypePermissions(odLane.type); // TODO: how to get the OD lane specific restrictions?
1096 520 : sumoLane.width = myImportWidths && odLane.width != NBEdge::UNSPECIFIED_WIDTH ? odLane.width : tc.getEdgeTypeWidth(odLane.type);
1097 520 : sumoLane.type = odLane.type;
1098 :
1099 520 : const double widthResolution = tc.getEdgeTypeWidthResolution(odLane.type);
1100 520 : const double maxWidth = tc.getEdgeTypeMaxWidth(odLane.type);
1101 :
1102 520 : const bool forbiddenNarrow = (sumoLane.width < myMinWidth
1103 11 : && (sumoLane.permissions & ~SVC_VULNERABLE) != 0
1104 531 : && sumoLane.width < tc.getEdgeTypeWidth(odLane.type));
1105 :
1106 520 : if (sumoLane.width >= 0 && widthResolution > 0) {
1107 0 : sumoLane.width = floor(sumoLane.width / widthResolution + 0.5) * widthResolution;
1108 0 : if (forbiddenNarrow && sumoLane.width >= myMinWidth) {
1109 0 : sumoLane.width -= widthResolution;
1110 0 : if (sumoLane.width <= 0) {
1111 0 : sumoLane.width = MAX2(POSITION_EPS, myMinWidth - POSITION_EPS);
1112 : }
1113 0 : } else if (sumoLane.width == 0) {
1114 : // round up when close to 0
1115 0 : sumoLane.width = widthResolution;
1116 : }
1117 : }
1118 520 : if (maxWidth > 0) {
1119 0 : sumoLane.width = MIN2(sumoLane.width, maxWidth);
1120 : }
1121 520 : if (forbiddenNarrow) {
1122 : // avoid narrow passenger car lanes (especially at sections with varying width)
1123 11 : sumoLane.permissions = SVC_EMERGENCY | SVC_AUTHORITY;
1124 : }
1125 520 : }
1126 :
1127 : void
1128 710 : NIImporter_OpenDrive::buildConnectionsToOuter(const Connection& c,
1129 : const std::map<std::string, OpenDriveEdge*>& innerEdges,
1130 : const std::map<std::string, OpenDriveEdge*>& edges,
1131 : const NBTypeCont& tc,
1132 : std::vector<Connection>& into, std::set<Connection>& seen) {
1133 :
1134 710 : OpenDriveEdge* dest = innerEdges.find(c.toEdge)->second;
1135 : #ifdef DEBUG_CONNECTIONS
1136 : if (DEBUG_COND3(c.fromEdge)) {
1137 : std::cout << " buildConnectionsToOuter " << c.getDescription() << "\n";
1138 : std::cout << " dest=" << (dest == nullptr ? "NULL" : dest->id) << " seenlist=";
1139 : for (std::set<Connection>::const_iterator i = seen.begin(); i != seen.end(); ++i) {
1140 : std::cout << " " << (*i).fromEdge << "," << (*i).toEdge << " ";
1141 : }
1142 : std::cout << "\n";
1143 : }
1144 : #endif
1145 710 : if (dest == nullptr) {
1146 : /// !!! should not, look in all?
1147 : return;
1148 : }
1149 : seen.insert(c);
1150 1764 : for (const Connection& destCon : dest->connections) {
1151 1054 : auto innerEdgesIt = innerEdges.find(destCon.toEdge);
1152 : #ifdef DEBUG_CONNECTIONS
1153 : if (DEBUG_COND3(c.fromEdge)) {
1154 : std::cout << " toInner=" << (innerEdgesIt != innerEdges.end()) << " destCon " << destCon.getDescription() << "\n";
1155 : }
1156 : #endif
1157 1054 : if (innerEdgesIt != innerEdges.end()) {
1158 : std::vector<Connection> t;
1159 : if (seen.count(destCon) == 0) {
1160 0 : buildConnectionsToOuter(destCon, innerEdges, edges, tc, t, seen);
1161 0 : for (std::vector<Connection>::const_iterator j = t.begin(); j != t.end(); ++j) {
1162 : // @todo this section is unverified
1163 0 : Connection cn = (*j);
1164 0 : cn.fromEdge = c.fromEdge;
1165 0 : cn.fromLane = c.fromLane;
1166 0 : cn.fromCP = c.fromCP;
1167 0 : cn.all = c.all; // @todo "all" is a hack trying to avoid the "from is zero" problem;
1168 0 : if (myImportInternalShapes) {
1169 0 : cn.shape = innerEdgesIt->second->geom + c.shape;
1170 : }
1171 0 : into.push_back(cn);
1172 0 : }
1173 : } else {
1174 0 : WRITE_WARNING("Circular connections in junction including roads '" + c.fromEdge + "' and '" + c.toEdge + "', loop size " + toString(seen.size()));
1175 : }
1176 0 : } else {
1177 1054 : int in = c.toLane;
1178 1054 : int out = destCon.fromLane;
1179 1054 : if (c.toCP == OPENDRIVE_CP_END) {
1180 : // inner edge runs in reverse direction
1181 : std::swap(in, out);
1182 : }
1183 : #ifdef DEBUG_CONNECTIONS
1184 : if (DEBUG_COND3(c.fromEdge)) {
1185 : std::cout << " laneSectionsConnected dest=" << dest->id << " in=" << in << " out=" << out
1186 : << " connected=" << laneSectionsConnected(dest, in, out) << "\n";
1187 : }
1188 : #endif
1189 :
1190 1054 : if (laneSectionsConnected(dest, in, out)) {
1191 696 : Connection cn = destCon;
1192 696 : cn.fromEdge = c.fromEdge;
1193 696 : cn.fromLane = c.fromLane;
1194 696 : cn.fromCP = c.fromCP;
1195 696 : cn.all = c.all;
1196 : cn.origID = c.toEdge;
1197 696 : cn.origLane = c.toLane;
1198 696 : if (myImportInternalShapes) {
1199 : OpenDriveXMLTag lanesDir;
1200 : cn.shape = dest->geom;
1201 : // determine which lane of dest belongs to this connection
1202 : int referenceLane = 0;
1203 : int offsetFactor = 1;
1204 49 : if (c.toCP == OPENDRIVE_CP_END) {
1205 : offsetFactor = -1;
1206 33 : lanesDir = OPENDRIVE_TAG_LEFT;
1207 45 : for (const auto& destLane : dest->laneSections.front().lanesByDir[lanesDir]) {
1208 45 : if (destLane.successor == c.fromLane) {
1209 33 : referenceLane = destLane.id;
1210 33 : break;
1211 : }
1212 : }
1213 : } else {
1214 16 : lanesDir = OPENDRIVE_TAG_RIGHT;
1215 30 : for (const auto& destLane : dest->laneSections.front().lanesByDir[lanesDir]) {
1216 23 : if (destLane.predecessor == c.fromLane) {
1217 9 : referenceLane = destLane.id;
1218 9 : break;
1219 : }
1220 : }
1221 : }
1222 : // compute offsets
1223 : //if (cn.fromEdge == "1014000" && dest->id == "3001022") {
1224 : // std::cout << "computeOffsets\n";
1225 : //}
1226 49 : std::vector<double> offsets(dest->geom.size(), 0);
1227 49 : if (dest->laneOffsets.size() > 0) {
1228 0 : offsets = dest->laneOffsets;
1229 : }
1230 : #ifdef DEBUG_INTERNALSHAPES
1231 : std::string destPred;
1232 : #endif
1233 : double s = 0;
1234 : int iShape = 0;
1235 98 : for (int laneSectionIndex = 0; laneSectionIndex < (int)dest->laneSections.size(); laneSectionIndex++) {
1236 49 : OpenDriveLaneSection& laneSection = dest->laneSections[laneSectionIndex];
1237 49 : const double nextS = laneSectionIndex + 1 < (int)dest->laneSections.size() ? dest->laneSections[laneSectionIndex + 1].s : std::numeric_limits<double>::max();
1238 : double sStart = s; // distance offset a the start of the current lane section
1239 : double finalS = s; // final distance value after processing this segment
1240 : int finalI = iShape;
1241 258 : for (const OpenDriveLane& destLane : laneSection.lanesByDir[lanesDir]) {
1242 : // each lane of the current segment repeats the same section of shape points and distance offsets
1243 : double sectionS = 0;
1244 : int i = iShape; // shape index at the start of the current lane section
1245 : s = sStart;
1246 : #ifdef DEBUG_INTERNALSHAPES
1247 : destPred += " lane=" + toString(destLane.id)
1248 : + " pred=" + toString(destLane.predecessor)
1249 : + " succ=" + toString(destLane.successor)
1250 : + " wStart=" + (destLane.widthData.empty() ? "?" : toString(destLane.widthData.front().computeAt(0)))
1251 : + " wEnd=" + (destLane.widthData.empty() ? "?" : toString(destLane.widthData.front().computeAt(cn.shape.length2D())))
1252 : + " width=" + toString(destLane.width) + "\n";
1253 : #endif
1254 209 : if (abs(destLane.id) <= abs(referenceLane) || abs(destLane.id) == abs(c.toLane)) {
1255 183 : const double multiplier = offsetFactor * (destLane.id == referenceLane ? 0.5 : 1);
1256 : #ifdef DEBUG_INTERNALSHAPES
1257 : destPred += " multiplier=" + toString(multiplier) + "\n";
1258 : #endif
1259 : int widthDataIndex = 0;
1260 9591 : while (s < nextS && i < (int)cn.shape.size()) {
1261 9408 : if (i > 0) {
1262 9225 : const double dist = cn.shape[i - 1].distanceTo2D(cn.shape[i]);
1263 9225 : s += dist;
1264 9225 : sectionS += dist;
1265 :
1266 : }
1267 9408 : while (widthDataIndex + 1 < (int)destLane.widthData.size()
1268 9408 : && sectionS >= destLane.widthData[widthDataIndex + 1].s) {
1269 : widthDataIndex++;
1270 : }
1271 9408 : double width = tc.getEdgeTypeWidth(destLane.type);
1272 9408 : if (destLane.widthData.size() > 0) {
1273 9408 : width = destLane.widthData[widthDataIndex].computeAt(sectionS);
1274 : } else {
1275 : #ifdef DEBUG_INTERNALSHAPES
1276 : std::cout << " missing width data at inner edge " << dest->id << " to=" << cn.toEdge << "_" << cn.toLane << " cp=" << cn.toCP << "\n";
1277 : #endif
1278 : // use first width of the target lane
1279 0 : OpenDriveEdge* const outerToEdge = edges.find(cn.toEdge)->second;
1280 0 : OpenDriveLaneSection& toLaneSection = cn.toCP == OPENDRIVE_CP_END ? outerToEdge->laneSections.front() : outerToEdge->laneSections.back();
1281 0 : const OpenDriveXMLTag laneDir = cn.toLane < 0 ? OPENDRIVE_TAG_RIGHT : OPENDRIVE_TAG_LEFT;
1282 0 : for (const OpenDriveLane& outerToLane : toLaneSection.lanesByDir[laneDir]) {
1283 : if (outerToLane.id == cn.toLane && outerToLane.width > 0) {
1284 : #ifdef DEBUG_INTERNALSHAPES
1285 : std::cout << " using toLane width " << width << "\n";
1286 : #endif
1287 : break;
1288 : }
1289 : }
1290 : }
1291 9408 : offsets[i] += width * multiplier;
1292 : //if (cn.fromEdge == "1014000" && dest->id == "3001022") {
1293 : // std::cout << " i=" << i << " s=" << s << " lane=" << destLane.id << " rlane=" << referenceLane /*<< " nextS=" << nextS << */ << " lsIndex=" << laneSectionIndex << " wI=" << widthDataIndex << " wSize=" << destLane.widthData.size() << " m=" << multiplier << " o=" << offsets[i] << "\n";
1294 : //}
1295 9408 : i++;
1296 : }
1297 : finalS = s;
1298 : finalI = i;
1299 26 : } else if (finalS == s) {
1300 : // update finalS without changing offsets
1301 700 : while (s < nextS && i < (int)cn.shape.size()) {
1302 686 : if (i > 0) {
1303 672 : const double dist = cn.shape[i - 1].distanceTo2D(cn.shape[i]);
1304 672 : s += dist;
1305 672 : finalS += dist;
1306 :
1307 : }
1308 686 : i++;
1309 : }
1310 : finalI = i;
1311 :
1312 : }
1313 : }
1314 : // advance values for the next lane section
1315 : iShape = finalI;
1316 : s = finalS;
1317 : }
1318 : try {
1319 49 : cn.shape.move2sideCustom(offsets);
1320 0 : } catch (InvalidArgument&) {
1321 0 : WRITE_WARNING("Could not import internal lane shape from edge '" + c.fromEdge + "' to edge '" + c.toEdge);
1322 : cn.shape.clear();
1323 0 : }
1324 : #ifdef DEBUG_INTERNALSHAPES
1325 : std::cout << "internalShape "
1326 : << c.getDescription()
1327 : << " dest=" << dest->id
1328 : << " refLane=" << referenceLane
1329 : << "\n destPred=" << destPred
1330 : << "\n offsets=" << offsets
1331 : << "\n shape=" << dest->geom
1332 : << "\n shape2=" << cn.shape
1333 : << "\n";
1334 : #endif
1335 49 : if (c.toCP == OPENDRIVE_CP_END) {
1336 66 : cn.shape = cn.shape.reverse();
1337 : }
1338 49 : }
1339 : #ifdef DEBUG_CONNECTIONS
1340 : if (DEBUG_COND3(c.fromEdge)) {
1341 : std::cout << " added connection\n";
1342 : }
1343 : #endif
1344 696 : into.push_back(cn);
1345 696 : }
1346 : }
1347 : }
1348 : }
1349 :
1350 :
1351 : bool
1352 1054 : NIImporter_OpenDrive::laneSectionsConnected(OpenDriveEdge* edge, int in, int out) {
1353 1054 : if (edge->laneSections.size() == 1) {
1354 1029 : return in == out;
1355 : } else {
1356 : // there could be spacing lanes (type 'none') that lead to a shift in lane index
1357 66 : for (auto it = edge->laneSections.begin(); it + 1 < edge->laneSections.end(); it++) {
1358 : OpenDriveLaneSection& laneSection = *it;
1359 41 : if (laneSection.lanesByDir.find(OPENDRIVE_TAG_RIGHT) != laneSection.lanesByDir.end()) {
1360 169 : for (OpenDriveLane& lane : laneSection.lanesByDir.find(OPENDRIVE_TAG_RIGHT)->second) {
1361 168 : if (lane.id == in) {
1362 40 : in = lane.successor;
1363 40 : break;
1364 : }
1365 : }
1366 : }
1367 41 : if (laneSection.lanesByDir.find(OPENDRIVE_TAG_LEFT) != laneSection.lanesByDir.end()) {
1368 65 : for (OpenDriveLane& lane : laneSection.lanesByDir.find(OPENDRIVE_TAG_LEFT)->second) {
1369 25 : if (lane.id == in) {
1370 1 : in = lane.successor;
1371 1 : break;
1372 : }
1373 : }
1374 : }
1375 : }
1376 25 : return in == out;
1377 : }
1378 : }
1379 :
1380 :
1381 : void
1382 882 : NIImporter_OpenDrive::setEdgeLinks2(OpenDriveEdge& e, const std::map<std::string, OpenDriveEdge*>& edges) {
1383 2568 : for (std::vector<OpenDriveLink>::iterator i = e.links.begin(); i != e.links.end(); ++i) {
1384 : OpenDriveLink& l = *i;
1385 1686 : if (l.elementType != OPENDRIVE_ET_ROAD) {
1386 : // we assume that links to nodes are later given as connections to edges
1387 426 : continue;
1388 : }
1389 : // get the right direction of the connected edge
1390 : std::string connectedEdge = l.elementID;
1391 : std::string edgeID = e.id;
1392 :
1393 1260 : OpenDriveLaneSection& laneSection = l.linkType == OPENDRIVE_LT_SUCCESSOR ? e.laneSections.back() : e.laneSections[0];
1394 : const std::map<int, int>& laneMap = laneSection.laneMap;
1395 : #ifdef DEBUG_CONNECTIONS
1396 : if (DEBUG_COND(&e)) {
1397 : std::cout << "edge=" << e.id << " eType=" << l.elementType << " lType=" << l.linkType << " connectedEdge=" << connectedEdge << " laneSection=" << laneSection.s << " map:\n";
1398 : std::cout << joinToString(laneMap, "\n", ":") << "\n";
1399 : }
1400 : #endif
1401 1260 : if (laneSection.lanesByDir.find(OPENDRIVE_TAG_RIGHT) != laneSection.lanesByDir.end()) {
1402 : const std::vector<OpenDriveLane>& lanes = laneSection.lanesByDir.find(OPENDRIVE_TAG_RIGHT)->second;
1403 2637 : for (std::vector<OpenDriveLane>::const_iterator j = lanes.begin(); j != lanes.end(); ++j) {
1404 2736 : if (!myImportAllTypes && laneMap.find((*j).id) == laneMap.end()) {
1405 83 : continue;
1406 : }
1407 1294 : Connection c; // @todo: give Connection a new name and a constructor
1408 1294 : c.fromEdge = e.id;
1409 1294 : c.fromLane = (*j).id;
1410 1294 : c.fromCP = OPENDRIVE_CP_END;
1411 1294 : c.toLane = l.linkType == OPENDRIVE_LT_SUCCESSOR ? (*j).successor : (*j).predecessor;
1412 : c.toEdge = connectedEdge;
1413 1294 : c.toCP = l.contactPoint;
1414 1294 : c.all = false;
1415 1294 : if (l.linkType != OPENDRIVE_LT_SUCCESSOR) {
1416 : std::swap(c.fromEdge, c.toEdge);
1417 : std::swap(c.fromLane, c.toLane);
1418 647 : c.fromCP = c.toCP;
1419 647 : c.toCP = OPENDRIVE_CP_START;
1420 : }
1421 1294 : if (edges.find(c.fromEdge) == edges.end()) {
1422 0 : WRITE_ERRORF(TL("While setting connections: incoming road '%' is not known."), c.fromEdge);
1423 : } else {
1424 1294 : OpenDriveEdge* src = edges.find(c.fromEdge)->second;
1425 : src->connections.insert(c);
1426 : #ifdef DEBUG_CONNECTIONS
1427 : if (DEBUG_COND(src)) {
1428 : std::cout << "insertConRight from=" << src->id << "_" << c.fromLane << " to=" << c.toEdge << "_" << c.toLane << "\n";
1429 : }
1430 : #endif
1431 : }
1432 1294 : }
1433 : }
1434 1260 : if (laneSection.lanesByDir.find(OPENDRIVE_TAG_LEFT) != laneSection.lanesByDir.end()) {
1435 : const std::vector<OpenDriveLane>& lanes = laneSection.lanesByDir.find(OPENDRIVE_TAG_LEFT)->second;
1436 1458 : for (std::vector<OpenDriveLane>::const_iterator j = lanes.begin(); j != lanes.end(); ++j) {
1437 330 : if (!myImportAllTypes && laneMap.find((*j).id) == laneMap.end()) {
1438 122 : continue;
1439 : }
1440 76 : Connection c;
1441 76 : c.toEdge = e.id;
1442 76 : c.toLane = (*j).id;
1443 76 : c.toCP = OPENDRIVE_CP_END;
1444 76 : c.fromLane = l.linkType == OPENDRIVE_LT_SUCCESSOR ? (*j).successor : (*j).predecessor;
1445 : c.fromEdge = connectedEdge;
1446 76 : c.fromCP = l.contactPoint;
1447 76 : c.all = false;
1448 76 : if (l.linkType != OPENDRIVE_LT_SUCCESSOR) {
1449 : std::swap(c.fromEdge, c.toEdge);
1450 : std::swap(c.fromLane, c.toLane);
1451 38 : c.fromCP = c.toCP;
1452 38 : c.toCP = OPENDRIVE_CP_START;
1453 : }
1454 76 : if (edges.find(c.fromEdge) == edges.end()) {
1455 0 : WRITE_ERRORF(TL("While setting connections: incoming road '%' is not known."), c.fromEdge);
1456 : } else {
1457 76 : OpenDriveEdge* src = edges.find(c.fromEdge)->second;
1458 : src->connections.insert(c);
1459 : #ifdef DEBUG_CONNECTIONS
1460 : if (DEBUG_COND(src)) {
1461 : std::cout << "insertConLeft from=" << src->id << "_" << c.fromLane << " to=" << c.toEdge << "_" << c.toLane << "\n";
1462 : }
1463 : #endif
1464 : }
1465 76 : }
1466 : }
1467 : }
1468 882 : }
1469 :
1470 :
1471 : std::string
1472 1257 : NIImporter_OpenDrive::reversedEdgeID(const std::string& edgeID) {
1473 1257 : return edgeID[0] == '-' ? edgeID.substr(1) : "-" + edgeID;
1474 : }
1475 :
1476 :
1477 : NBNode*
1478 78 : NIImporter_OpenDrive::getOrBuildNode(const std::string& id, const Position& pos,
1479 : NBNodeCont& nc) {
1480 78 : if (nc.retrieve(id) == nullptr) {
1481 : // not yet built; build now
1482 78 : if (!nc.insert(id, pos)) {
1483 : // !!! clean up
1484 0 : throw ProcessError(TLF("Could not add node '%'.", id));
1485 : }
1486 : }
1487 78 : return nc.retrieve(id);
1488 : }
1489 :
1490 :
1491 : void
1492 521 : NIImporter_OpenDrive::setNodeSecure(NBNodeCont& nc, OpenDriveEdge& e,
1493 : const std::string& nodeID, NIImporter_OpenDrive::LinkType lt, std::vector<NodeSet>& joinedNodeIDs) {
1494 521 : NBNode* n = nc.retrieve(nodeID);
1495 521 : if (n == nullptr) {
1496 0 : throw ProcessError(TLF("Could not find node '%'.", nodeID));
1497 : }
1498 521 : NBNode* toJoin = nullptr;
1499 521 : if (lt == OPENDRIVE_LT_SUCCESSOR) {
1500 292 : if (e.to != nullptr && e.to != n) {
1501 0 : toJoin = e.to;
1502 : }
1503 292 : e.to = n;
1504 : } else {
1505 229 : if (e.from != nullptr && e.from != n) {
1506 0 : toJoin = e.from;
1507 : }
1508 229 : e.from = n;
1509 : }
1510 521 : if (toJoin != nullptr) {
1511 : // join nodes
1512 : NodeSet* set1 = nullptr;
1513 : NodeSet* set2 = nullptr;
1514 0 : for (NodeSet& joined : joinedNodeIDs) {
1515 : if (joined.count(toJoin) != 0) {
1516 : set1 = &joined;
1517 : }
1518 : if (joined.count(n) != 0) {
1519 : set2 = &joined;
1520 : }
1521 : }
1522 0 : if (set1 == nullptr && set2 == nullptr) {
1523 0 : joinedNodeIDs.push_back(NodeSet());
1524 : joinedNodeIDs.back().insert(n);
1525 : joinedNodeIDs.back().insert(toJoin);
1526 0 : } else if (set1 == nullptr && set2 != nullptr) {
1527 : set2->insert(toJoin);
1528 0 : } else if (set1 != nullptr && set2 == nullptr) {
1529 : set1->insert(n);
1530 : } else {
1531 0 : set1->insert(set2->begin(), set2->end());
1532 0 : joinedNodeIDs.erase(std::find(joinedNodeIDs.begin(), joinedNodeIDs.end(), *set2));
1533 : }
1534 : }
1535 521 : }
1536 :
1537 : bool
1538 882 : NIImporter_OpenDrive::hasNonLinearElevation(const OpenDriveEdge& e) {
1539 882 : if (e.elevations.size() > 1) {
1540 : return true;
1541 : }
1542 1699 : for (const OpenDriveElevation& el : e.elevations) {
1543 828 : if (el.c != 0 || el.d != 0) {
1544 : return true;
1545 : }
1546 : }
1547 : return false;
1548 : }
1549 :
1550 : void
1551 26 : NIImporter_OpenDrive::computeShapes(std::map<std::string, OpenDriveEdge*>& edges) {
1552 26 : OptionsCont& oc = OptionsCont::getOptions();
1553 52 : const double res = oc.getFloat("opendrive.curve-resolution");
1554 908 : for (const auto& i : edges) {
1555 882 : OpenDriveEdge& e = *i.second;
1556 : GeometryType prevType = OPENDRIVE_GT_UNKNOWN;
1557 882 : const double lineRes = hasNonLinearElevation(e) ? res : -1;
1558 : Position last;
1559 : int index = 0;
1560 2208 : for (const OpenDriveGeometry& g : e.geometries) {
1561 1326 : PositionVector geom;
1562 1326 : switch (g.type) {
1563 : case OPENDRIVE_GT_UNKNOWN:
1564 : break;
1565 424 : case OPENDRIVE_GT_LINE:
1566 848 : geom = geomFromLine(e, g, lineRes);
1567 424 : break;
1568 0 : case OPENDRIVE_GT_SPIRAL:
1569 0 : geom = geomFromSpiral(e, g, res);
1570 0 : break;
1571 29 : case OPENDRIVE_GT_ARC:
1572 58 : geom = geomFromArc(e, g, res);
1573 29 : break;
1574 0 : case OPENDRIVE_GT_POLY3:
1575 0 : geom = geomFromPoly(e, g, res);
1576 0 : break;
1577 873 : case OPENDRIVE_GT_PARAMPOLY3:
1578 1746 : geom = geomFromParamPoly(e, g, res);
1579 873 : break;
1580 : default:
1581 : break;
1582 : }
1583 1326 : if (e.geom.size() > 0 && prevType == OPENDRIVE_GT_LINE) {
1584 : // remove redundant end point of the previous geometry segment
1585 : // (the start point of the current segment should have the same value)
1586 : // this avoids geometry errors due to imprecision
1587 159 : if (!e.geom.back().almostSame(geom.front())) {
1588 3 : WRITE_WARNINGF(TL("Mismatched geometry for edge '%' between geometry segments % and %."), e.id, index - 1, index);
1589 : }
1590 : e.geom.pop_back();
1591 : }
1592 : //std::cout << " adding geometry to road=" << e.id << " old=" << e.geom << " new=" << geom << "\n";
1593 10803 : for (PositionVector::iterator k = geom.begin(); k != geom.end(); ++k) {
1594 9477 : last = *k;
1595 9477 : e.geom.push_back_noDoublePos(*k);
1596 : }
1597 1326 : prevType = g.type;
1598 1326 : index++;
1599 1326 : }
1600 882 : if (e.geom.size() == 1 && e.geom.front() != last) {
1601 : // avoid length-1 geometry due to almostSame check
1602 1 : e.geom.push_back(last);
1603 : }
1604 : #ifdef DEBUG_SHAPE
1605 : if (DEBUG_COND3(e.id)) {
1606 : std::cout << e.id << " initialGeom=" << e.geom << "\n";
1607 : }
1608 : #endif
1609 2625 : if (oc.exists("geometry.min-dist") && !oc.isDefault("geometry.min-dist")) {
1610 : // simplify geometry for both directions consistently but ensure
1611 : // that start and end angles are preserved
1612 21 : if (e.geom.size() > 4) {
1613 28 : e.geom.removeDoublePoints(oc.getFloat("geometry.min-dist"), true, 1, 1, true);
1614 : }
1615 : }
1616 : #ifdef DEBUG_SHAPE
1617 : if (DEBUG_COND3(e.id)) {
1618 : std::cout << e.id << " reducedGeom=" << e.geom << "\n";
1619 : }
1620 : #endif
1621 882 : if (!NBNetBuilder::transformCoordinates(e.geom)) {
1622 0 : WRITE_ERRORF(TL("Unable to project coordinates for edge '%'."), e.id);
1623 : }
1624 : // add z-data
1625 : int k = 0;
1626 : double pos = 0;
1627 : //std::cout << " edge=" << e.id << " geom.size=" << e.geom.size() << " geom.len=" << e.geom.length2D() << " ele.size=" << e.elevations.size() << "\n";
1628 1764 : if (!oc.getBool("flatten")) {
1629 1738 : for (std::vector<OpenDriveElevation>::iterator j = e.elevations.begin(); j != e.elevations.end(); ++j) {
1630 : const OpenDriveElevation& el = *j;
1631 860 : const double sNext = (j + 1) == e.elevations.end() ? std::numeric_limits<double>::max() : (*(j + 1)).s;
1632 9228 : while (k < (int)e.geom.size() && pos < sNext) {
1633 : const double z = el.computeAt(pos);
1634 : //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";
1635 8368 : e.geom[k].add(0, 0, z);
1636 8368 : k++;
1637 8368 : if (k < (int)e.geom.size()) {
1638 : // XXX pos understimates the actual position since the
1639 : // actual geometry between k-1 and k could be curved
1640 7533 : pos += e.geom[k - 1].distanceTo2D(e.geom[k]);
1641 : }
1642 : }
1643 : }
1644 : }
1645 1764 : e.geom = e.geom.simplified2(false);
1646 : // add laneoffset
1647 882 : if (e.offsets.size() > 0) {
1648 176 : e.laneOffsets = discretizeOffsets(e.geom, e.offsets, e.id);
1649 : }
1650 : //std::cout << " loaded geometry " << e.id << "=" << e.geom << "\n";
1651 : }
1652 26 : }
1653 :
1654 :
1655 : std::vector<double>
1656 176 : NIImporter_OpenDrive::discretizeOffsets(PositionVector& geom, const std::vector<OpenDriveLaneOffset>& offsets, const std::string& id) {
1657 : UNUSED_PARAMETER(id);
1658 : std::vector<double> laneOffsets;
1659 : // make sure there are intermediate points for each offset-section
1660 355 : for (const OpenDriveLaneOffset& el : offsets) {
1661 : // check wether we need to insert a new point at dist
1662 179 : Position pS = geom.positionAtOffset2D(el.s);
1663 179 : int iS = geom.indexOfClosest(pS);
1664 : // prevent close spacing to reduce impact of rounding errors in z-axis
1665 179 : if (pS.distanceTo2D(geom[iS]) > POSITION_EPS) {
1666 3 : geom.insertAtClosest(pS, false);
1667 : //std::cout << " edge=" << e.id << " inserting pos=" << pS << " s=" << el.s << " iS=" << iS << " dist=" << pS.distanceTo2D(geom[iS]) << "\n";
1668 : }
1669 : }
1670 : // XXX add further points for sections with non-constant offset
1671 : // shift each point orthogonally by the specified offset
1672 : int kk = 0;
1673 : double ppos = 0;
1674 355 : for (auto j = offsets.begin(); j != offsets.end(); ++j) {
1675 : const OpenDriveLaneOffset& el = *j;
1676 179 : const double sNext = (j + 1) == offsets.end() ? std::numeric_limits<double>::max() : (*(j + 1)).s;
1677 1242 : while (kk < (int)geom.size() && ppos < sNext) {
1678 : const double offset = el.computeAt(ppos);
1679 1063 : laneOffsets.push_back(fabs(offset) > POSITION_EPS ? -offset : 0);
1680 1063 : kk++;
1681 1063 : if (kk < (int)geom.size()) {
1682 : // XXX pos understimates the actual position since the
1683 : // actual geometry between k-1 and k could be curved
1684 887 : ppos += geom[kk - 1].distanceTo2D(geom[kk]);
1685 : }
1686 : }
1687 : }
1688 176 : return laneOffsets;
1689 0 : }
1690 :
1691 :
1692 : void
1693 0 : NIImporter_OpenDrive::addOffsets(bool left, PositionVector& geom, const std::vector<OpenDriveWidth>& offsets, const std::string& id, std::vector<double>& result) {
1694 : UNUSED_PARAMETER(id);
1695 : // make sure there are intermediate points for each offset-section
1696 0 : for (const OpenDriveLaneOffset& el : offsets) {
1697 : // check wether we need to insert a new point at dist
1698 0 : Position pS = geom.positionAtOffset2D(el.s);
1699 0 : int iS = geom.indexOfClosest(pS);
1700 : // prevent close spacing to reduce impact of rounding errors in z-axis
1701 0 : if (pS.distanceTo2D(geom[iS]) > POSITION_EPS) {
1702 : //std::cout << " edge=" << id << " inserting pos=" << pS << " s=" << el.s << " iS=" << iS << " dist=" << pS.distanceTo2D(geom[iS]) << "\n";
1703 0 : int at = geom.insertAtClosest(pS, false);
1704 : double interpolatedOffset = 0;
1705 0 : if (at == 0) {
1706 0 : interpolatedOffset = result.front();
1707 0 : } else if (at == (int)geom.size() - 1) {
1708 0 : interpolatedOffset = result.back();
1709 : } else {
1710 0 : interpolatedOffset = (result[at - 1] + result[at]) / 2;
1711 : }
1712 0 : result.insert(result.begin() + at, interpolatedOffset);
1713 : }
1714 : }
1715 : // shift each point orthogonally by the specified offset
1716 : int kk = 0;
1717 : double ppos = 0;
1718 0 : const int sign = left ? -1 : 1;
1719 0 : for (auto j = offsets.begin(); j != offsets.end(); ++j) {
1720 : const OpenDriveWidth& el = *j;
1721 0 : const double sNext = (j + 1) == offsets.end() ? std::numeric_limits<double>::max() : (*(j + 1)).s;
1722 0 : while (kk < (int)geom.size() && ppos < sNext) {
1723 : const double offset = el.computeAt(ppos);
1724 0 : result[kk] += fabs(offset) > POSITION_EPS ? sign * offset : 0;
1725 0 : kk++;
1726 0 : if (kk < (int)geom.size()) {
1727 : // XXX pos understimates the actual position since the
1728 : // actual geometry between k-1 and k could be curved
1729 0 : ppos += geom[kk - 1].distanceTo2D(geom[kk]);
1730 : }
1731 : }
1732 : }
1733 0 : }
1734 :
1735 :
1736 : void
1737 26 : NIImporter_OpenDrive::revisitLaneSections(const NBTypeCont& tc, std::map<std::string, OpenDriveEdge*>& edges) {
1738 908 : for (const auto& i : edges) {
1739 882 : OpenDriveEdge& e = *i.second;
1740 : #ifdef DEBUG_VARIABLE_SPEED
1741 : if (DEBUG_COND(&e)) {
1742 : gDebugFlag1 = true;
1743 : std::cout << "revisitLaneSections e=" << e.id << "\n";
1744 : }
1745 : #endif
1746 : // split by speed limits or by access restrictions
1747 : std::vector<OpenDriveLaneSection> newSections;
1748 1844 : for (OpenDriveLaneSection& section : e.laneSections) {
1749 : std::vector<OpenDriveLaneSection> splitSections;
1750 962 : const bool splitByAttrChange = section.buildAttributeChanges(tc, splitSections);
1751 962 : if (!splitByAttrChange) {
1752 165 : newSections.push_back(section);
1753 : } else {
1754 : std::copy(splitSections.begin(), splitSections.end(), back_inserter(newSections));
1755 : }
1756 962 : }
1757 :
1758 882 : e.laneSections = newSections;
1759 : double lastS = -1.;
1760 : // check whether the lane sections are in the right order
1761 : bool sorted = true;
1762 1846 : for (const OpenDriveLaneSection& section : e.laneSections) {
1763 964 : if (section.s <= lastS) {
1764 : sorted = false;
1765 : break;
1766 : }
1767 : lastS = section.s;
1768 : }
1769 882 : if (!sorted) {
1770 0 : WRITE_WARNINGF(TL("The sections of edge '%' are not sorted properly."), e.id);
1771 0 : sort(e.laneSections.begin(), e.laneSections.end(), sections_by_s_sorter());
1772 : }
1773 : // check whether duplicate s-values occur
1774 : // but keep all lane sections for connecting roads because they are
1775 : // needed to establish connectivity (laneSectionsConnected)
1776 : // TODO recheck whether removing short sections is a good idea at all: once we parse linkage info, it will be lost.
1777 882 : if (e.laneSections.size() > 1 && !e.isInner) {
1778 84 : for (std::vector<OpenDriveLaneSection>::iterator j = e.laneSections.begin(); j != e.laneSections.end() - 1;) {
1779 72 : if ((j + 1)->s - j->s < POSITION_EPS) {
1780 0 : WRITE_WARNINGF(TL("Almost duplicate s-value '%' for lane sections occurred at edge '%'; first entry was removed."), toString(j->s), e.id);
1781 : j = e.laneSections.erase(j);
1782 : } else {
1783 : ++j;
1784 : }
1785 : }
1786 : }
1787 : #ifdef DEBUG_VARIABLE_SPEED
1788 : gDebugFlag1 = false;
1789 : #endif
1790 882 : }
1791 26 : }
1792 :
1793 :
1794 : PositionVector
1795 424 : NIImporter_OpenDrive::geomFromLine(const OpenDriveEdge& e, const OpenDriveGeometry& g, double resolution) {
1796 : UNUSED_PARAMETER(e);
1797 424 : PositionVector ret;
1798 424 : Position start(g.x, g.y);
1799 424 : Position end = calculateStraightEndPoint(g.hdg, g.length, start);
1800 424 : if (resolution > 0 && g.length > 0) {
1801 22 : const int numPoints = (int)ceil(g.length / resolution) + 1;
1802 22 : double dx = (end.x() - start.x()) / (numPoints - 1);
1803 22 : double dy = (end.y() - start.y()) / (numPoints - 1);
1804 1031 : for (int i = 0; i < numPoints; i++) {
1805 1009 : ret.push_back(Position(g.x + i * dx, g.y + i * dy));
1806 : }
1807 : } else {
1808 402 : ret.push_back(start);
1809 402 : ret.push_back(end);
1810 : }
1811 424 : return ret;
1812 0 : }
1813 :
1814 :
1815 : PositionVector
1816 0 : NIImporter_OpenDrive::geomFromSpiral(const OpenDriveEdge& e, const OpenDriveGeometry& g, double resolution) {
1817 : UNUSED_PARAMETER(e);
1818 0 : PositionVector ret;
1819 0 : double curveStart = g.params[0];
1820 0 : double curveEnd = g.params[1];
1821 : try {
1822 0 : double cDot = (curveEnd - curveStart) / g.length;
1823 0 : if (cDot == 0 || g.length == 0) {
1824 0 : WRITE_WARNINGF(TL("Could not compute spiral geometry for edge '%' (cDot=% length=%)."), e.id, toString(cDot), toString(g.length));
1825 0 : ret.push_back(Position(g.x, g.y));
1826 0 : return ret;
1827 : }
1828 0 : double sStart = curveStart / cDot;
1829 0 : double sEnd = curveEnd / cDot;
1830 0 : double x = 0;
1831 0 : double y = 0;
1832 0 : double t = 0;
1833 0 : double tStart = 0;
1834 : double s;
1835 0 : odrSpiral(sStart, cDot, &x, &y, &tStart);
1836 0 : for (s = sStart; s <= sEnd; s += resolution) {
1837 0 : odrSpiral(s, cDot, &x, &y, &t);
1838 0 : ret.push_back(Position(x, y));
1839 : }
1840 0 : if (s != sEnd /*&& ret.size() == 1*/) {
1841 0 : odrSpiral(sEnd, cDot, &x, &y, &t);
1842 0 : ret.push_back(Position(x, y));
1843 : }
1844 : //if (s != sEnd && ret.size() > 2) {
1845 : // ret.pop_back();
1846 : //}
1847 : assert(ret.size() >= 2);
1848 : assert(ret[0] != ret[1]);
1849 : // shift start to coordinate origin
1850 : PositionVector ret1 = ret;
1851 0 : ret.add(ret.front() * -1);
1852 : // rotate
1853 : PositionVector ret2 = ret;
1854 0 : ret.rotate2D(g.hdg - tStart);
1855 : #ifdef DEBUG_SPIRAL
1856 : std::cout
1857 : << std::setprecision(4)
1858 : << "edge=" << e.id << " s=" << g.s
1859 : << " cStart=" << curveStart
1860 : << " cEnd=" << curveEnd
1861 : << " cDot=" << cDot
1862 : << " sStart=" << sStart
1863 : << " sEnd=" << sEnd
1864 : << " g.hdg=" << GeomHelper::naviDegree(g.hdg)
1865 : << " tStart=" << GeomHelper::naviDegree(tStart)
1866 : << "\n beforeShift=" << ret1
1867 : << "\n beforeRot=" << ret2
1868 : << "\n";
1869 : #endif
1870 : // shift to geometry start
1871 0 : ret.add(g.x, g.y, 0);
1872 0 : } catch (const std::runtime_error& error) {
1873 0 : WRITE_WARNINGF(TL("Could not compute spiral geometry for edge '%' (%)."), e.id, error.what());
1874 0 : ret.push_back(Position(g.x, g.y));
1875 0 : }
1876 0 : return ret.getSubpart2D(0, g.length);
1877 0 : }
1878 :
1879 :
1880 : PositionVector
1881 29 : NIImporter_OpenDrive::geomFromArc(const OpenDriveEdge& e, const OpenDriveGeometry& g, double resolution) {
1882 : UNUSED_PARAMETER(e);
1883 29 : PositionVector ret;
1884 29 : double centerX = g.x;
1885 29 : double centerY = g.y;
1886 : // left: positive value
1887 29 : double curvature = g.params[0];
1888 29 : double radius = 1. / curvature;
1889 : // center point
1890 29 : calculateCurveCenter(¢erX, ¢erY, radius, g.hdg);
1891 29 : double endX = g.x;
1892 29 : double endY = g.y;
1893 : double startX = g.x;
1894 : double startY = g.y;
1895 29 : double geo_posS = g.s;
1896 : double geo_posE = g.s;
1897 : bool end = false;
1898 : do {
1899 243 : geo_posE += resolution;
1900 243 : if (geo_posE - g.s > g.length) {
1901 28 : geo_posE = g.s + g.length;
1902 : }
1903 243 : if (geo_posE - g.s > g.length) {
1904 0 : geo_posE = g.s + g.length;
1905 : }
1906 243 : calcPointOnCurve(&endX, &endY, centerX, centerY, radius, geo_posE - geo_posS);
1907 243 : ret.push_back(Position(startX, startY));
1908 :
1909 243 : startX = endX;
1910 243 : startY = endY;
1911 : geo_posS = geo_posE;
1912 :
1913 243 : if (geo_posE - (g.s + g.length) < 0.001 && geo_posE - (g.s + g.length) > -0.001) {
1914 : end = true;
1915 : }
1916 243 : } while (!end);
1917 29 : ret.push_back(Position(startX, startY));
1918 58 : return ret.getSubpart2D(0, g.length);
1919 29 : }
1920 :
1921 :
1922 : PositionVector
1923 0 : NIImporter_OpenDrive::geomFromPoly(const OpenDriveEdge& e, const OpenDriveGeometry& g, double resolution) {
1924 : UNUSED_PARAMETER(e);
1925 0 : const double s = sin(g.hdg);
1926 0 : const double c = cos(g.hdg);
1927 0 : PositionVector ret;
1928 0 : for (double off = 0; off < g.length + 2.; off += resolution) {
1929 : double x = off;
1930 0 : double y = g.params[0] + g.params[1] * off + g.params[2] * pow(off, 2.) + g.params[3] * pow(off, 3.);
1931 0 : double xnew = x * c - y * s;
1932 0 : double ynew = x * s + y * c;
1933 0 : ret.push_back(Position(g.x + xnew, g.y + ynew));
1934 : }
1935 0 : return ret.getSubpart2D(0, g.length);
1936 0 : }
1937 :
1938 :
1939 : PositionVector
1940 873 : NIImporter_OpenDrive::geomFromParamPoly(const OpenDriveEdge& e, const OpenDriveGeometry& g, double resolution) {
1941 : UNUSED_PARAMETER(e);
1942 873 : const double s = sin(g.hdg);
1943 873 : const double c = cos(g.hdg);
1944 873 : const double pMax = g.params[8] <= 0 ? g.length : g.params[8];
1945 873 : const double pStep = pMax / ceil(g.length / resolution);
1946 873 : PositionVector ret;
1947 9081 : for (double p = 0; p <= pMax + pStep; p += pStep) {
1948 8208 : double x = g.params[0] + g.params[1] * p + g.params[2] * pow(p, 2.) + g.params[3] * pow(p, 3.);
1949 8208 : double y = g.params[4] + g.params[5] * p + g.params[6] * pow(p, 2.) + g.params[7] * pow(p, 3.);
1950 8208 : double xnew = x * c - y * s;
1951 8208 : double ynew = x * s + y * c;
1952 8208 : ret.push_back(Position(g.x + xnew, g.y + ynew));
1953 : }
1954 1746 : return ret.getSubpart2D(0, g.length);
1955 873 : }
1956 :
1957 :
1958 : Position
1959 424 : NIImporter_OpenDrive::calculateStraightEndPoint(double hdg, double length, const Position& start) {
1960 : double normx = 1.0f;
1961 : double normy = 0.0f;
1962 424 : double x2 = normx * cos(hdg) - normy * sin(hdg);
1963 424 : double y2 = normx * sin(hdg) + normy * cos(hdg);
1964 424 : normx = x2 * length;
1965 424 : normy = y2 * length;
1966 424 : return Position(start.x() + normx, start.y() + normy);
1967 : }
1968 :
1969 :
1970 : void
1971 29 : NIImporter_OpenDrive::calculateCurveCenter(double* ad_x, double* ad_y, double ad_radius, double ad_hdg) {
1972 : double normX = 1.0;
1973 : double normY = 0.0;
1974 : double tmpX;
1975 : double turn;
1976 29 : if (ad_radius > 0) {
1977 : turn = -1.0;
1978 : } else {
1979 : turn = 1.0;
1980 : }
1981 :
1982 : tmpX = normX;
1983 29 : normX = normX * cos(ad_hdg) + normY * sin(ad_hdg);
1984 29 : normY = tmpX * sin(ad_hdg) + normY * cos(ad_hdg);
1985 :
1986 : tmpX = normX;
1987 29 : normX = turn * normY;
1988 29 : normY = -turn * tmpX;
1989 :
1990 29 : normX = fabs(ad_radius) * normX;
1991 29 : normY = fabs(ad_radius) * normY;
1992 :
1993 29 : *ad_x += normX;
1994 29 : *ad_y += normY;
1995 29 : }
1996 :
1997 :
1998 : void
1999 243 : NIImporter_OpenDrive::calcPointOnCurve(double* ad_x, double* ad_y, double ad_centerX, double ad_centerY,
2000 : double ad_r, double ad_length) {
2001 243 : double rotAngle = ad_length / fabs(ad_r);
2002 243 : double vx = *ad_x - ad_centerX;
2003 243 : double vy = *ad_y - ad_centerY;
2004 : double tmpx;
2005 :
2006 : double turn;
2007 243 : if (ad_r > 0) {
2008 : turn = -1; //left
2009 : } else {
2010 : turn = 1; //right
2011 : }
2012 : tmpx = vx;
2013 243 : vx = vx * cos(rotAngle) + turn * vy * sin(rotAngle);
2014 243 : vy = -1 * turn * tmpx * sin(rotAngle) + vy * cos(rotAngle);
2015 243 : *ad_x = vx + ad_centerX;
2016 243 : *ad_y = vy + ad_centerY;
2017 243 : }
2018 :
2019 :
2020 : // ---------------------------------------------------------------------------
2021 : // section
2022 : // ---------------------------------------------------------------------------
2023 962 : NIImporter_OpenDrive::OpenDriveLaneSection::OpenDriveLaneSection(double sArg) : s(sArg), sOrig(sArg) {
2024 962 : lanesByDir[OPENDRIVE_TAG_LEFT] = std::vector<OpenDriveLane>();
2025 962 : lanesByDir[OPENDRIVE_TAG_RIGHT] = std::vector<OpenDriveLane>();
2026 962 : lanesByDir[OPENDRIVE_TAG_CENTER] = std::vector<OpenDriveLane>();
2027 962 : }
2028 :
2029 :
2030 : void
2031 963 : NIImporter_OpenDrive::OpenDriveLaneSection::buildLaneMapping(const NBTypeCont& tc) {
2032 963 : discardedInnerWidthRight = 0;
2033 : int sumoLane = 0;
2034 : bool singleType = true;
2035 : std::vector<std::string> types;
2036 : const std::vector<OpenDriveLane>& dirLanesR = lanesByDir.find(OPENDRIVE_TAG_RIGHT)->second;
2037 2311 : for (std::vector<OpenDriveLane>::const_reverse_iterator i = dirLanesR.rbegin(); i != dirLanesR.rend(); ++i) {
2038 1348 : if (myImportAllTypes || (tc.knows((*i).type) && !tc.getEdgeTypeShallBeDiscarded((*i).type))) {
2039 1023 : discardedInnerWidthRight = 0;
2040 1023 : laneMap[(*i).id] = sumoLane++;
2041 1023 : types.push_back((*i).type);
2042 1023 : if (types.front() != types.back()) {
2043 : singleType = false;
2044 : }
2045 : } else {
2046 325 : discardedInnerWidthRight += (*i).width;
2047 : }
2048 : }
2049 963 : discardedInnerWidthLeft = 0;
2050 963 : rightLaneNumber = sumoLane;
2051 2884 : rightType = sumoLane > 0 ? (singleType ? types.front() : joinToString(types, "|")) : "";
2052 : sumoLane = 0;
2053 : singleType = true;
2054 : types.clear();
2055 : const std::vector<OpenDriveLane>& dirLanesL = lanesByDir.find(OPENDRIVE_TAG_LEFT)->second;
2056 1376 : for (std::vector<OpenDriveLane>::const_iterator i = dirLanesL.begin(); i != dirLanesL.end(); ++i) {
2057 413 : if (myImportAllTypes || (tc.knows((*i).type) && !tc.getEdgeTypeShallBeDiscarded((*i).type))) {
2058 158 : discardedInnerWidthLeft = 0;
2059 158 : laneMap[(*i).id] = sumoLane++;
2060 158 : types.push_back((*i).type);
2061 158 : if (types.front() != types.back()) {
2062 : singleType = false;
2063 : }
2064 : } else {
2065 255 : discardedInnerWidthLeft += (*i).width;
2066 : }
2067 : }
2068 963 : leftLaneNumber = sumoLane;
2069 1062 : leftType = sumoLane > 0 ? (singleType ? types.front() : joinToString(types, "|")) : "";
2070 963 : }
2071 :
2072 :
2073 : std::map<int, int>
2074 92 : NIImporter_OpenDrive::OpenDriveLaneSection::getInnerConnections(OpenDriveXMLTag dir, const OpenDriveLaneSection& prev) {
2075 : std::map<int, int> ret;
2076 : const std::vector<OpenDriveLane>& dirLanes = lanesByDir.find(dir)->second;
2077 411 : for (std::vector<OpenDriveLane>::const_reverse_iterator i = dirLanes.rbegin(); i != dirLanes.rend(); ++i) {
2078 : std::map<int, int>::const_iterator toP = laneMap.find((*i).id);
2079 319 : if (toP == laneMap.end()) {
2080 : // the current lane is not available in SUMO
2081 186 : continue;
2082 : }
2083 133 : int to = (*toP).second;
2084 133 : int from = UNSET_CONNECTION;
2085 133 : if ((*i).predecessor != UNSET_CONNECTION) {
2086 125 : from = (*i).predecessor;
2087 : }
2088 133 : if (from != UNSET_CONNECTION) {
2089 : std::map<int, int>::const_iterator fromP = prev.laneMap.find(from);
2090 125 : if (fromP != prev.laneMap.end()) {
2091 125 : from = (*fromP).second;
2092 : } else {
2093 0 : from = UNSET_CONNECTION;
2094 : }
2095 : }
2096 133 : if (from != UNSET_CONNECTION && to != UNSET_CONNECTION) {
2097 : if (ret.find(from) != ret.end()) {
2098 : // WRITE_WARNING(TL("double connection"));
2099 : }
2100 125 : if (dir == OPENDRIVE_TAG_LEFT) {
2101 : std::swap(from, to);
2102 : }
2103 125 : ret[from] = to;
2104 : } else {
2105 : // WRITE_WARNING(TL("missing connection"));
2106 : }
2107 : }
2108 92 : return ret;
2109 : }
2110 :
2111 :
2112 : NIImporter_OpenDrive::OpenDriveLaneSection
2113 2 : NIImporter_OpenDrive::OpenDriveLaneSection::buildLaneSection(const NBTypeCont& tc, double startPos) {
2114 2 : OpenDriveLaneSection ret(*this);
2115 2 : ret.s += startPos;
2116 4 : for (int k = 0; k < (int)ret.lanesByDir[OPENDRIVE_TAG_RIGHT].size(); ++k) {
2117 2 : OpenDriveLane& l = ret.lanesByDir[OPENDRIVE_TAG_RIGHT][k];
2118 2 : l.speed = 0;
2119 2 : l.permission = 0;
2120 2 : std::vector<std::pair<double, LaneAttributeChange> >::const_iterator it = std::find_if(l.attributeChanges.begin(), l.attributeChanges.end(), same_position_finder(startPos));
2121 2 : if (it != l.attributeChanges.end()) {
2122 2 : l.speed = (*it).second.speed;
2123 2 : l.permission = l.computePermission(tc, (*it).second.allowed, (*it).second.denied);
2124 : }
2125 : }
2126 4 : for (int k = 0; k < (int)ret.lanesByDir[OPENDRIVE_TAG_LEFT].size(); ++k) {
2127 2 : OpenDriveLane& l = ret.lanesByDir[OPENDRIVE_TAG_LEFT][k];
2128 2 : l.speed = 0;
2129 2 : l.permission = 0;
2130 2 : std::vector<std::pair<double, LaneAttributeChange> >::const_iterator it = std::find_if(l.attributeChanges.begin(), l.attributeChanges.end(), same_position_finder(startPos));
2131 2 : if (it != l.attributeChanges.end()) {
2132 2 : l.speed = (*it).second.speed;
2133 2 : l.permission = l.computePermission(tc, (*it).second.allowed, (*it).second.denied);
2134 : }
2135 : }
2136 2 : return ret;
2137 0 : }
2138 :
2139 :
2140 : SVCPermissions
2141 830 : NIImporter_OpenDrive::OpenDriveLane::computePermission(const NBTypeCont& tc, const std::vector<std::string>& allowed,
2142 : const std::vector<std::string>& denied) const {
2143 830 : SVCPermissions perms = tc.getEdgeTypePermissions(type);
2144 830 : if (allowed.size() > 0 && denied.size() > 0) {
2145 0 : WRITE_WARNING(TL("Will discard access settings as both denied and allowed classes have been specified."));
2146 830 : } else if (allowed.size() > 0) {
2147 : perms = SVC_IGNORING;
2148 4 : for (const std::string& allow : allowed) {
2149 2 : if (allow == "simulator") {
2150 : perms = SVC_IGNORING;
2151 : break;
2152 2 : } else if (allow == "autonomousTraffic" || allow == "autonomous traffic" || allow == "throughTraffic") {
2153 0 : perms = tc.getEdgeTypePermissions(type);
2154 : break;
2155 2 : } else if (allow == "pedestrian") {
2156 0 : perms |= SVC_PEDESTRIAN;
2157 2 : } else if (allow == "passengerCar") {
2158 0 : perms |= SVC_PASSENGER;
2159 2 : } else if (allow == "bus") {
2160 2 : perms |= SVC_BUS;
2161 0 : } else if (allow == "delivery") {
2162 0 : perms |= SVC_DELIVERY;
2163 0 : } else if (allow == "emergency") {
2164 0 : perms |= SVC_EMERGENCY;
2165 0 : } else if (allow == "taxi") {
2166 0 : perms |= SVC_TAXI;
2167 0 : } else if (allow == "bicycle") {
2168 0 : perms |= SVC_BICYCLE;
2169 0 : } else if (allow == "motorcycle") {
2170 0 : perms |= SVC_MOTORCYCLE;
2171 0 : } else if (allow == "truck" || allow == "trucks") {
2172 : perms |= SVC_TRUCK;
2173 0 : perms |= SVC_TRAILER;
2174 : }
2175 : }
2176 828 : } else if (denied.size() > 0) {
2177 2 : for (const std::string& deny : denied) {
2178 2 : if (deny == "none") {
2179 2 : perms = tc.getEdgeTypePermissions(type);
2180 : break;
2181 0 : } else if (deny == "autonomousTraffic" || deny == "autonomous traffic" || deny == "throughTraffic") {
2182 : perms = SVC_IGNORING;
2183 : break;
2184 0 : } else if (deny == "pedestrian") {
2185 0 : perms &= ~SVC_PEDESTRIAN;
2186 0 : } else if (deny == "passengerCar") {
2187 0 : perms &= ~SVC_PASSENGER;
2188 0 : } else if (deny == "bus") {
2189 0 : perms &= ~SVC_BUS;
2190 0 : } else if (deny == "delivery") {
2191 0 : perms &= ~SVC_DELIVERY;
2192 0 : } else if (deny == "emergency") {
2193 0 : perms &= ~SVC_EMERGENCY;
2194 0 : } else if (deny == "taxi") {
2195 0 : perms &= ~SVC_TAXI;
2196 0 : } else if (deny == "bicycle") {
2197 0 : perms &= ~SVC_BICYCLE;
2198 0 : } else if (deny == "motorcycle") {
2199 0 : perms &= ~SVC_MOTORCYCLE;
2200 0 : } else if (deny == "truck" || deny == "trucks") {
2201 : perms &= ~SVC_TRUCK;
2202 0 : perms &= ~SVC_TRAILER;
2203 : }
2204 : }
2205 : }
2206 830 : return perms;
2207 : }
2208 :
2209 :
2210 : bool
2211 962 : NIImporter_OpenDrive::OpenDriveLaneSection::buildAttributeChanges(const NBTypeCont& tc, std::vector<OpenDriveLaneSection>& newSections) {
2212 : std::set<double> attributeChangePositions;
2213 : // collect speed change and access restriction positions and apply initial values to the begin
2214 2309 : for (std::vector<OpenDriveLane>::iterator k = lanesByDir[OPENDRIVE_TAG_RIGHT].begin(); k != lanesByDir[OPENDRIVE_TAG_RIGHT].end(); ++k) {
2215 2175 : for (std::vector<std::pair<double, LaneAttributeChange> >::const_iterator l = (*k).attributeChanges.begin(); l != (*k).attributeChanges.end(); ++l) {
2216 828 : attributeChangePositions.insert((*l).first);
2217 828 : if ((*l).first == 0) {
2218 826 : (*k).speed = (*l).second.speed;
2219 826 : (*k).permission = (*k).computePermission(tc, (*l).second.allowed, (*l).second.denied);
2220 : }
2221 : }
2222 : }
2223 1374 : for (std::vector<OpenDriveLane>::iterator k = lanesByDir[OPENDRIVE_TAG_LEFT].begin(); k != lanesByDir[OPENDRIVE_TAG_LEFT].end(); ++k) {
2224 414 : for (std::vector<std::pair<double, LaneAttributeChange> >::const_iterator l = (*k).attributeChanges.begin(); l != (*k).attributeChanges.end(); ++l) {
2225 2 : attributeChangePositions.insert((*l).first);
2226 2 : if ((*l).first == 0) {
2227 0 : (*k).speed = (*l).second.speed;
2228 0 : (*k).permission = (*k).computePermission(tc, (*l).second.allowed, (*l).second.denied);
2229 : }
2230 : }
2231 : }
2232 :
2233 : // do nothing if there is none
2234 962 : if (attributeChangePositions.size() == 0) {
2235 : return false;
2236 : }
2237 :
2238 797 : if (*attributeChangePositions.begin() > 0) {
2239 1 : attributeChangePositions.insert(0);
2240 : }
2241 : #ifdef DEBUG_VARIABLE_SPEED
2242 : if (gDebugFlag1) std::cout
2243 : << " buildSpeedChanges sectionStart=" << s
2244 : << " speedChangePositions=" << joinToString(speedChangePositions, ", ")
2245 : << "\n";
2246 : #endif
2247 1596 : for (std::set<double>::iterator i = attributeChangePositions.begin(); i != attributeChangePositions.end(); ++i) {
2248 799 : if (i == attributeChangePositions.begin()) {
2249 797 : newSections.push_back(*this);
2250 : } else {
2251 4 : newSections.push_back(buildLaneSection(tc, *i));
2252 : }
2253 : }
2254 : // propagate speeds and access restrictions
2255 1596 : for (int i = 0; i != (int)newSections.size(); ++i) {
2256 3196 : for (auto& k : newSections[i].lanesByDir) {
2257 4030 : for (int j = 0; j != (int)k.second.size(); ++j) {
2258 1633 : OpenDriveLane& l = k.second[j];
2259 1633 : if (l.speed == 0) {
2260 807 : if (i > 0) {
2261 6 : l.speed = newSections[i - 1].lanesByDir[k.first][j].speed;
2262 : } else {
2263 801 : tc.getEdgeTypeSpeed(l.type);
2264 : }
2265 : }
2266 1633 : if (l.permission == 0) {
2267 803 : if (i > 0) {
2268 2 : l.permission = newSections[i - 1].lanesByDir[k.first][j].permission;
2269 2 : l.type = newSections[i - 1].lanesByDir[k.first][j].type;
2270 : } else {
2271 801 : tc.getEdgeTypePermissions(l.type);
2272 : }
2273 : }
2274 : }
2275 : }
2276 : }
2277 : return true;
2278 : }
2279 :
2280 :
2281 :
2282 : // ---------------------------------------------------------------------------
2283 : // edge
2284 : // ---------------------------------------------------------------------------
2285 : int
2286 504 : NIImporter_OpenDrive::OpenDriveEdge::getPriority(OpenDriveXMLTag dir) const {
2287 : // for signal interpretations see https://de.wikipedia.org/wiki/Bildtafel_der_Verkehrszeichen_in_der_Bundesrepublik_Deutschland_seit_2013
2288 : int prio = 1;
2289 564 : for (std::vector<OpenDriveSignal>::const_iterator i = signals.begin(); i != signals.end(); ++i) {
2290 : int tmp = 1;
2291 60 : if ((*i).type == "301" || (*i).type == "306") { // priority road or local priority
2292 : tmp = 2;
2293 : }
2294 60 : if ((*i).type == "205" /*|| (*i).type == "206"*/) { // yield or stop
2295 : tmp = 0;
2296 : }
2297 60 : if (tmp != 1 && dir == OPENDRIVE_TAG_RIGHT && (*i).orientation > 0) {
2298 : prio = tmp;
2299 : }
2300 60 : if (tmp != 1 && dir == OPENDRIVE_TAG_LEFT && (*i).orientation < 0) {
2301 : prio = tmp;
2302 : }
2303 :
2304 : }
2305 504 : return prio;
2306 : }
2307 :
2308 :
2309 :
2310 : // ---------------------------------------------------------------------------
2311 : // loader methods
2312 : // ---------------------------------------------------------------------------
2313 26 : NIImporter_OpenDrive::NIImporter_OpenDrive(const NBTypeCont& tc, std::map<std::string, OpenDriveEdge*>& edges)
2314 : : GenericSAXHandler(openDriveTags, OPENDRIVE_TAG_NOTHING, openDriveAttrs, OPENDRIVE_ATTR_NOTHING, "opendrive"),
2315 78 : myTypeContainer(tc), myCurrentEdge("", "", "", -1), myCurrentController("", ""), myEdges(edges), myOffset(0, 0),
2316 52 : myUseCurrentNode(false) {
2317 26 : }
2318 :
2319 :
2320 26 : NIImporter_OpenDrive::~NIImporter_OpenDrive() {
2321 26 : }
2322 :
2323 :
2324 : void
2325 29437 : NIImporter_OpenDrive::myStartElement(int element,
2326 : const SUMOSAXAttributes& attrs) {
2327 29437 : if (myUseCurrentNode) { // skip the parent node repeated in the included file
2328 1 : myUseCurrentNode = false;
2329 1 : myElementStack.push_back(element);
2330 1 : return;
2331 : }
2332 29436 : bool ok = true;
2333 29436 : switch (element) {
2334 26 : case OPENDRIVE_TAG_HEADER: {
2335 26 : int majorVersion = attrs.get<int>(OPENDRIVE_ATTR_REVMAJOR, nullptr, ok);
2336 26 : int minorVersion = attrs.get<int>(OPENDRIVE_ATTR_REVMINOR, nullptr, ok);
2337 26 : if (majorVersion == 1 && minorVersion > 4) { // disable flags only used for old 1.4 standard
2338 2 : NIImporter_OpenDrive::myIgnoreMisplacedSignals = false;
2339 : }
2340 : /*
2341 : if (majorVersion != 1 || minorVersion != 2) {
2342 : // TODO: leave note of exceptions
2343 : WRITE_WARNINGF(TL("Given openDrive file '%' uses version %.%;\n Version 1.2 is supported."), getFileName(), toString(majorVersion), toString(minorVersion));
2344 : }
2345 : */
2346 : }
2347 : break;
2348 3 : case OPENDRIVE_TAG_OFFSET: {
2349 3 : double x = attrs.get<double>(OPENDRIVE_ATTR_X, "offset", ok);
2350 3 : double y = attrs.get<double>(OPENDRIVE_ATTR_Y, "offset", ok);
2351 3 : double z = attrs.get<double>(OPENDRIVE_ATTR_Z, "offset", ok);
2352 3 : myOffset.set(-x, -y, -z);
2353 3 : if (GeoConvHelper::getNumLoaded()) {
2354 2 : GeoConvHelper::getLoaded().moveConvertedBy(-x, -y);
2355 : }
2356 : }
2357 : break;
2358 882 : case OPENDRIVE_TAG_ROAD: {
2359 882 : std::string id = attrs.get<std::string>(OPENDRIVE_ATTR_ID, nullptr, ok);
2360 1764 : std::string streetName = attrs.getOpt<std::string>(OPENDRIVE_ATTR_NAME, nullptr, ok, "", false);
2361 882 : std::string junction = attrs.get<std::string>(OPENDRIVE_ATTR_JUNCTION, id.c_str(), ok);
2362 882 : double length = attrs.get<double>(OPENDRIVE_ATTR_LENGTH, id.c_str(), ok);
2363 882 : myCurrentEdge = OpenDriveEdge(id, streetName, junction, length);
2364 : }
2365 882 : break;
2366 1682 : case OPENDRIVE_TAG_PREDECESSOR: {
2367 1682 : if (myElementStack.size() >= 2 && myElementStack[myElementStack.size() - 2] == OPENDRIVE_TAG_ROAD) {
2368 837 : std::string elementType = attrs.get<std::string>(OPENDRIVE_ATTR_ELEMENTTYPE, myCurrentEdge.id.c_str(), ok);
2369 837 : std::string elementID = attrs.get<std::string>(OPENDRIVE_ATTR_ELEMENTID, myCurrentEdge.id.c_str(), ok);
2370 837 : std::string contactPoint = attrs.hasAttribute(OPENDRIVE_ATTR_CONTACTPOINT)
2371 837 : ? attrs.get<std::string>(OPENDRIVE_ATTR_CONTACTPOINT, myCurrentEdge.id.c_str(), ok)
2372 837 : : "end";
2373 837 : addLink(OPENDRIVE_LT_PREDECESSOR, elementType, elementID, contactPoint);
2374 : }
2375 1682 : if (myElementStack.size() >= 2 && myElementStack[myElementStack.size() - 2] == OPENDRIVE_TAG_LANE) {
2376 845 : int no = attrs.get<int>(OPENDRIVE_ATTR_ID, myCurrentEdge.id.c_str(), ok);
2377 845 : OpenDriveLane& l = myCurrentEdge.laneSections.back().lanesByDir[myCurrentLaneDirection].back();
2378 845 : l.predecessor = no;
2379 : }
2380 : }
2381 : break;
2382 1688 : case OPENDRIVE_TAG_SUCCESSOR: {
2383 1688 : if (myElementStack.size() >= 2 && myElementStack[myElementStack.size() - 2] == OPENDRIVE_TAG_ROAD) {
2384 849 : std::string elementType = attrs.get<std::string>(OPENDRIVE_ATTR_ELEMENTTYPE, myCurrentEdge.id.c_str(), ok);
2385 849 : std::string elementID = attrs.get<std::string>(OPENDRIVE_ATTR_ELEMENTID, myCurrentEdge.id.c_str(), ok);
2386 849 : std::string contactPoint = attrs.hasAttribute(OPENDRIVE_ATTR_CONTACTPOINT)
2387 849 : ? attrs.get<std::string>(OPENDRIVE_ATTR_CONTACTPOINT, myCurrentEdge.id.c_str(), ok)
2388 849 : : "start";
2389 849 : addLink(OPENDRIVE_LT_SUCCESSOR, elementType, elementID, contactPoint);
2390 : }
2391 1688 : if (myElementStack.size() >= 2 && myElementStack[myElementStack.size() - 2] == OPENDRIVE_TAG_LANE) {
2392 839 : int no = attrs.get<int>(OPENDRIVE_ATTR_ID, myCurrentEdge.id.c_str(), ok);
2393 839 : OpenDriveLane& l = myCurrentEdge.laneSections.back().lanesByDir[myCurrentLaneDirection].back();
2394 839 : l.successor = no;
2395 : }
2396 : }
2397 : break;
2398 1326 : case OPENDRIVE_TAG_GEOMETRY: {
2399 1326 : double length = attrs.get<double>(OPENDRIVE_ATTR_LENGTH, myCurrentEdge.id.c_str(), ok);
2400 1326 : double s = attrs.get<double>(OPENDRIVE_ATTR_S, myCurrentEdge.id.c_str(), ok);
2401 1326 : double x = attrs.get<double>(OPENDRIVE_ATTR_X, myCurrentEdge.id.c_str(), ok);
2402 1326 : double y = attrs.get<double>(OPENDRIVE_ATTR_Y, myCurrentEdge.id.c_str(), ok);
2403 1326 : double hdg = attrs.get<double>(OPENDRIVE_ATTR_HDG, myCurrentEdge.id.c_str(), ok);
2404 1326 : myCurrentEdge.geometries.push_back(OpenDriveGeometry(length, s, x, y, hdg));
2405 : }
2406 1326 : break;
2407 868 : case OPENDRIVE_TAG_ELEVATION: {
2408 868 : double s = attrs.get<double>(OPENDRIVE_ATTR_S, myCurrentEdge.id.c_str(), ok);
2409 868 : double a = attrs.get<double>(OPENDRIVE_ATTR_A, myCurrentEdge.id.c_str(), ok);
2410 868 : double b = attrs.get<double>(OPENDRIVE_ATTR_B, myCurrentEdge.id.c_str(), ok);
2411 868 : double c = attrs.get<double>(OPENDRIVE_ATTR_C, myCurrentEdge.id.c_str(), ok);
2412 868 : double d = attrs.get<double>(OPENDRIVE_ATTR_D, myCurrentEdge.id.c_str(), ok);
2413 868 : myCurrentEdge.elevations.push_back(OpenDriveElevation(s, a, b, c, d));
2414 : }
2415 868 : break;
2416 433 : case OPENDRIVE_TAG_LINE: {
2417 433 : if (myElementStack.size() > 0 && myElementStack.back() == OPENDRIVE_TAG_GEOMETRY) {
2418 : std::vector<double> vals;
2419 424 : addGeometryShape(OPENDRIVE_GT_LINE, vals);
2420 424 : }
2421 : }
2422 : break;
2423 : case OPENDRIVE_TAG_SPIRAL: {
2424 : std::vector<double> vals;
2425 0 : vals.push_back(attrs.get<double>(OPENDRIVE_ATTR_CURVSTART, myCurrentEdge.id.c_str(), ok));
2426 0 : vals.push_back(attrs.get<double>(OPENDRIVE_ATTR_CURVEND, myCurrentEdge.id.c_str(), ok));
2427 0 : addGeometryShape(OPENDRIVE_GT_SPIRAL, vals);
2428 0 : }
2429 0 : break;
2430 : case OPENDRIVE_TAG_ARC: {
2431 : std::vector<double> vals;
2432 29 : vals.push_back(attrs.get<double>(OPENDRIVE_ATTR_CURVATURE, myCurrentEdge.id.c_str(), ok));
2433 29 : addGeometryShape(OPENDRIVE_GT_ARC, vals);
2434 29 : }
2435 29 : break;
2436 : case OPENDRIVE_TAG_POLY3: {
2437 : std::vector<double> vals;
2438 0 : vals.push_back(attrs.get<double>(OPENDRIVE_ATTR_A, myCurrentEdge.id.c_str(), ok));
2439 0 : vals.push_back(attrs.get<double>(OPENDRIVE_ATTR_B, myCurrentEdge.id.c_str(), ok));
2440 0 : vals.push_back(attrs.get<double>(OPENDRIVE_ATTR_C, myCurrentEdge.id.c_str(), ok));
2441 0 : vals.push_back(attrs.get<double>(OPENDRIVE_ATTR_D, myCurrentEdge.id.c_str(), ok));
2442 0 : addGeometryShape(OPENDRIVE_GT_POLY3, vals);
2443 0 : }
2444 0 : break;
2445 : case OPENDRIVE_TAG_PARAMPOLY3: {
2446 : std::vector<double> vals;
2447 873 : vals.push_back(attrs.get<double>(OPENDRIVE_ATTR_AU, myCurrentEdge.id.c_str(), ok));
2448 873 : vals.push_back(attrs.get<double>(OPENDRIVE_ATTR_BU, myCurrentEdge.id.c_str(), ok));
2449 873 : vals.push_back(attrs.get<double>(OPENDRIVE_ATTR_CU, myCurrentEdge.id.c_str(), ok));
2450 873 : vals.push_back(attrs.get<double>(OPENDRIVE_ATTR_DU, myCurrentEdge.id.c_str(), ok));
2451 873 : vals.push_back(attrs.get<double>(OPENDRIVE_ATTR_AV, myCurrentEdge.id.c_str(), ok));
2452 873 : vals.push_back(attrs.get<double>(OPENDRIVE_ATTR_BV, myCurrentEdge.id.c_str(), ok));
2453 873 : vals.push_back(attrs.get<double>(OPENDRIVE_ATTR_CV, myCurrentEdge.id.c_str(), ok));
2454 873 : vals.push_back(attrs.get<double>(OPENDRIVE_ATTR_DV, myCurrentEdge.id.c_str(), ok));
2455 873 : const std::string pRange = attrs.getOpt<std::string>(OPENDRIVE_ATTR_PRANGE, myCurrentEdge.id.c_str(), ok, "normalized", false);
2456 873 : if (pRange == "normalized") {
2457 873 : vals.push_back(1.0);
2458 0 : } else if (pRange == "arcLength") {
2459 0 : vals.push_back(-1.0);
2460 : } else {
2461 0 : WRITE_WARNINGF(TL("Ignoring invalid pRange value '%' for road '%'."), pRange, myCurrentEdge.id);
2462 0 : vals.push_back(1.0);
2463 : }
2464 873 : addGeometryShape(OPENDRIVE_GT_PARAMPOLY3, vals);
2465 873 : }
2466 873 : break;
2467 962 : case OPENDRIVE_TAG_LANESECTION: {
2468 962 : double s = attrs.get<double>(OPENDRIVE_ATTR_S, myCurrentEdge.id.c_str(), ok);
2469 962 : if (myCurrentEdge.laneSections.size() > 0) {
2470 81 : myCurrentEdge.laneSections.back().length = s - myCurrentEdge.laneSections.back().s;
2471 : }
2472 1924 : myCurrentEdge.laneSections.push_back(OpenDriveLaneSection(s));
2473 :
2474 : // possibly updated by the next laneSection
2475 962 : myCurrentEdge.laneSections.back().length = myCurrentEdge.length - s;
2476 : }
2477 962 : break;
2478 179 : case OPENDRIVE_TAG_LANEOFFSET: {
2479 179 : double s = attrs.get<double>(OPENDRIVE_ATTR_S, myCurrentEdge.id.c_str(), ok);
2480 179 : double a = attrs.get<double>(OPENDRIVE_ATTR_A, myCurrentEdge.id.c_str(), ok);
2481 179 : double b = attrs.get<double>(OPENDRIVE_ATTR_B, myCurrentEdge.id.c_str(), ok);
2482 179 : double c = attrs.get<double>(OPENDRIVE_ATTR_C, myCurrentEdge.id.c_str(), ok);
2483 179 : double d = attrs.get<double>(OPENDRIVE_ATTR_D, myCurrentEdge.id.c_str(), ok);
2484 179 : myCurrentEdge.offsets.push_back(OpenDriveLaneOffset(s, a, b, c, d));
2485 : }
2486 179 : break;
2487 137 : case OPENDRIVE_TAG_LEFT:
2488 137 : myCurrentLaneDirection = OPENDRIVE_TAG_LEFT;
2489 137 : break;
2490 962 : case OPENDRIVE_TAG_CENTER:
2491 962 : myCurrentLaneDirection = OPENDRIVE_TAG_CENTER;
2492 962 : break;
2493 957 : case OPENDRIVE_TAG_RIGHT:
2494 957 : myCurrentLaneDirection = OPENDRIVE_TAG_RIGHT;
2495 957 : break;
2496 2721 : case OPENDRIVE_TAG_LANE: {
2497 2721 : std::string type = attrs.get<std::string>(OPENDRIVE_ATTR_TYPE, myCurrentEdge.id.c_str(), ok);
2498 2721 : int id = attrs.get<int>(OPENDRIVE_ATTR_ID, myCurrentEdge.id.c_str(), ok);
2499 2721 : std::string level = attrs.hasAttribute(OPENDRIVE_ATTR_LEVEL)
2500 2721 : ? attrs.get<std::string>(OPENDRIVE_ATTR_LEVEL, myCurrentEdge.id.c_str(), ok)
2501 2721 : : "";
2502 : OpenDriveLaneSection& ls = myCurrentEdge.laneSections.back();
2503 5442 : ls.lanesByDir[myCurrentLaneDirection].push_back(OpenDriveLane(id, level, type));
2504 : }
2505 2721 : break;
2506 30 : case OPENDRIVE_TAG_SIGNAL: {
2507 30 : std::string id = attrs.get<std::string>(OPENDRIVE_ATTR_ID, myCurrentEdge.id.c_str(), ok);
2508 30 : std::string type = attrs.get<std::string>(OPENDRIVE_ATTR_TYPE, myCurrentEdge.id.c_str(), ok);
2509 60 : std::string name = attrs.getOpt<std::string>(OPENDRIVE_ATTR_NAME, myCurrentEdge.id.c_str(), ok, "", false);
2510 30 : const std::string orientation = attrs.get<std::string>(OPENDRIVE_ATTR_ORIENTATION, id.c_str(), ok);
2511 30 : int orientationCode = orientation == "-" ? -1 : orientation == "+" ? 1 : 0;
2512 30 : double s = attrs.get<double>(OPENDRIVE_ATTR_S, myCurrentEdge.id.c_str(), ok);
2513 30 : bool dynamic = attrs.get<std::string>(OPENDRIVE_ATTR_DYNAMIC, myCurrentEdge.id.c_str(), ok) == "no" ? false : true;
2514 90 : OpenDriveSignal signal = OpenDriveSignal(id, type, name, orientationCode, dynamic, s);
2515 30 : myCurrentEdge.signals.push_back(signal);
2516 30 : mySignals[id] = signal;
2517 30 : }
2518 30 : break;
2519 0 : case OPENDRIVE_TAG_SIGNALREFERENCE: {
2520 0 : std::string id = attrs.get<std::string>(OPENDRIVE_ATTR_ID, myCurrentEdge.id.c_str(), ok);
2521 0 : const std::string orientation = attrs.get<std::string>(OPENDRIVE_ATTR_ORIENTATION, id.c_str(), ok);
2522 0 : int orientationCode = orientation == "-" ? -1 : orientation == "+" ? 1 : 0;
2523 0 : double s = attrs.get<double>(OPENDRIVE_ATTR_S, myCurrentEdge.id.c_str(), ok);
2524 0 : OpenDriveSignal signal = OpenDriveSignal(id, "", "", orientationCode, false, s);
2525 0 : myCurrentEdge.signals.push_back(signal);
2526 0 : }
2527 0 : break;
2528 3 : case OPENDRIVE_TAG_CONTROLLER: {
2529 3 : std::string id = attrs.get<std::string>(OPENDRIVE_ATTR_ID, nullptr, ok);
2530 6 : std::string name = attrs.getOpt<std::string>(OPENDRIVE_ATTR_NAME, nullptr, ok, "", false);
2531 6 : myCurrentController = OpenDriveController(id, name);
2532 : }
2533 3 : break;
2534 25 : case OPENDRIVE_TAG_CONTROL: {
2535 25 : std::string signalID = attrs.get<std::string>(OPENDRIVE_ATTR_SIGNALID, myCurrentController.id.c_str(), ok);
2536 25 : myCurrentController.signalIDs.push_back(signalID);
2537 25 : if (mySignals.find(signalID) != mySignals.end()) {
2538 25 : mySignals[signalID].controller = myCurrentController.id;
2539 : } else {
2540 0 : WRITE_WARNINGF(TL("Ignoring missing signal '%' in controller '%'."), signalID, myCurrentController.id);
2541 : }
2542 : }
2543 25 : break;
2544 30 : case OPENDRIVE_TAG_VALIDITY: {
2545 30 : int fromLane = attrs.get<int>(OPENDRIVE_ATTR_FROMLANE, myCurrentEdge.id.c_str(), ok);
2546 30 : int toLane = attrs.get<int>(OPENDRIVE_ATTR_TOLANE, myCurrentEdge.id.c_str(), ok);
2547 30 : if (myElementStack.size() >= 1 && (myElementStack.back() == OPENDRIVE_TAG_SIGNAL
2548 0 : || myElementStack.back() == OPENDRIVE_TAG_SIGNALREFERENCE)) {
2549 30 : myCurrentEdge.signals.back().minLane = fromLane;
2550 30 : myCurrentEdge.signals.back().maxLane = toLane;
2551 : }
2552 : }
2553 : break;
2554 5 : case OPENDRIVE_TAG_PRIORITY: {
2555 : if (myElementStack.size() >= 2
2556 5 : && myElementStack.back() == OPENDRIVE_TAG_SEMANTICS
2557 10 : && myElementStack[myElementStack.size() - 2] == OPENDRIVE_TAG_SIGNAL) {
2558 : auto& signal = myCurrentEdge.signals.back();
2559 10 : signal.priority = attrs.get<std::string>(OPENDRIVE_ATTR_TYPE, signal.id.c_str(), ok);
2560 : }
2561 : }
2562 : break;
2563 86 : case OPENDRIVE_TAG_JUNCTION:
2564 86 : myCurrentJunctionID = attrs.get<std::string>(OPENDRIVE_ATTR_ID, myCurrentJunctionID.c_str(), ok);
2565 86 : break;
2566 630 : case OPENDRIVE_TAG_CONNECTION: {
2567 630 : std::string id = attrs.get<std::string>(OPENDRIVE_ATTR_ID, myCurrentJunctionID.c_str(), ok);
2568 1260 : myCurrentIncomingRoad = attrs.get<std::string>(OPENDRIVE_ATTR_INCOMINGROAD, myCurrentJunctionID.c_str(), ok);
2569 1260 : myCurrentConnectingRoad = attrs.get<std::string>(OPENDRIVE_ATTR_CONNECTINGROAD, myCurrentJunctionID.c_str(), ok);
2570 630 : std::string cp = attrs.get<std::string>(OPENDRIVE_ATTR_CONTACTPOINT, myCurrentJunctionID.c_str(), ok);
2571 630 : myCurrentContactPoint = cp == "start" ? OPENDRIVE_CP_START : OPENDRIVE_CP_END;
2572 630 : myConnectionWasEmpty = true;
2573 : }
2574 630 : break;
2575 659 : case OPENDRIVE_TAG_LANELINK: {
2576 659 : int from = attrs.get<int>(OPENDRIVE_ATTR_FROM, myCurrentJunctionID.c_str(), ok);
2577 659 : int to = attrs.get<int>(OPENDRIVE_ATTR_TO, myCurrentJunctionID.c_str(), ok);
2578 659 : Connection c;
2579 659 : c.fromEdge = myCurrentIncomingRoad;
2580 659 : c.toEdge = myCurrentConnectingRoad;
2581 659 : c.fromLane = from;
2582 659 : c.toLane = to;
2583 659 : c.fromCP = OPENDRIVE_CP_END;
2584 659 : c.toCP = myCurrentContactPoint;
2585 659 : c.all = false;
2586 659 : if (myEdges.find(c.fromEdge) == myEdges.end()) {
2587 0 : WRITE_ERRORF(TL("In laneLink-element: incoming road '%' is not known."), c.fromEdge);
2588 : } else {
2589 659 : OpenDriveEdge* e = myEdges.find(c.fromEdge)->second;
2590 : e->connections.insert(c);
2591 659 : myConnectionWasEmpty = false;
2592 : }
2593 659 : }
2594 659 : break;
2595 1909 : case OPENDRIVE_TAG_WIDTH: {
2596 1909 : if (myElementStack.size() >= 2 && myElementStack[myElementStack.size() - 1] == OPENDRIVE_TAG_LANE) {
2597 1909 : const double s = attrs.get<double>(OPENDRIVE_ATTR_SOFFSET, myCurrentEdge.id.c_str(), ok);
2598 1909 : const double a = attrs.get<double>(OPENDRIVE_ATTR_A, myCurrentEdge.id.c_str(), ok);
2599 1909 : const double b = attrs.get<double>(OPENDRIVE_ATTR_B, myCurrentEdge.id.c_str(), ok);
2600 1909 : const double c = attrs.get<double>(OPENDRIVE_ATTR_C, myCurrentEdge.id.c_str(), ok);
2601 1909 : const double d = attrs.get<double>(OPENDRIVE_ATTR_D, myCurrentEdge.id.c_str(), ok);
2602 1909 : OpenDriveLane& l = myCurrentEdge.laneSections.back().lanesByDir[myCurrentLaneDirection].back();
2603 1909 : l.width = MAX2(l.width, a);
2604 1909 : l.widthData.push_back(OpenDriveWidth(s, a, b, c, d));
2605 : #ifdef DEBUG_VARIABLE_WIDTHS
2606 : if (DEBUG_COND(&myCurrentEdge)) {
2607 : std::cout << " road=" << myCurrentEdge.id
2608 : << std::setprecision(gPrecision)
2609 : << " junction=" << myCurrentEdge.junction
2610 : << " section=" << myCurrentEdge.laneSections.size() - 1
2611 : << " dir=" << myCurrentLaneDirection << " lane=" << l.id
2612 : << " type=" << l.type
2613 : << " width=" << l.width
2614 : << " a=" << a
2615 : << " b=" << b
2616 : << " c=" << c
2617 : << " d=" << d
2618 : << " s=" << s
2619 : << " entries=" << l.widthData.size()
2620 : << "\n";
2621 : }
2622 : #endif
2623 : }
2624 : }
2625 : break;
2626 4 : case OPENDRIVE_TAG_ACCESS: {
2627 4 : if (myElementStack.size() >= 2 && myElementStack[myElementStack.size() - 1] == OPENDRIVE_TAG_LANE) {
2628 4 : const double pos = attrs.get<double>(OPENDRIVE_ATTR_SOFFSET, myCurrentEdge.id.c_str(), ok);
2629 8 : std::string rule = attrs.getOpt<std::string>(OPENDRIVE_ATTR_RULE, nullptr, ok, "allow", false); // OpenDRIVE 1.4 without rule value
2630 4 : std::string vClass = attrs.get<std::string>(OPENDRIVE_ATTR_RESTRICTION, myCurrentEdge.id.c_str(), ok);
2631 :
2632 4 : std::vector < std::pair<double, LaneAttributeChange >>& attributeChanges = myCurrentEdge.laneSections.back().lanesByDir[myCurrentLaneDirection].back().attributeChanges;
2633 4 : std::vector<std::pair<double, LaneAttributeChange> >::iterator i = std::find_if(attributeChanges.begin(), attributeChanges.end(), same_position_finder(pos));
2634 4 : if (i != attributeChanges.end()) {
2635 0 : if (rule == "allow") {
2636 0 : (*i).second.allowed.push_back(vClass);
2637 0 : } else if (rule == "deny") {
2638 0 : (*i).second.denied.push_back(vClass);
2639 : }
2640 : } else {
2641 : LaneAttributeChange lac = LaneAttributeChange(0);
2642 4 : if (rule == "allow") {
2643 2 : lac.allowed.push_back(vClass);
2644 2 : } else if (rule == "deny") {
2645 2 : lac.denied.push_back(vClass);
2646 : }
2647 4 : attributeChanges.push_back(std::make_pair(pos, lac));
2648 : }
2649 : }
2650 : }
2651 : break;
2652 829 : case OPENDRIVE_TAG_SPEED: {
2653 829 : if (myElementStack.size() >= 2 && myElementStack[myElementStack.size() - 1] == OPENDRIVE_TAG_LANE) {
2654 826 : double speed = attrs.get<double>(OPENDRIVE_ATTR_MAX, myCurrentEdge.id.c_str(), ok);
2655 826 : double pos = attrs.get<double>(OPENDRIVE_ATTR_SOFFSET, myCurrentEdge.id.c_str(), ok);
2656 : // required for xodr v1.4
2657 1652 : const std::string unit = attrs.getOpt<std::string>(OPENDRIVE_ATTR_UNIT, myCurrentEdge.id.c_str(), ok, "", false);
2658 : // now convert the speed to reasonable default SI [m/s]
2659 826 : if (!unit.empty()) {
2660 : // something to be done at all ?
2661 0 : if (unit == "km/h") {
2662 0 : speed /= 3.6;
2663 : }
2664 0 : if (unit == "mph") {
2665 0 : speed *= 1.609344 / 3.6;
2666 : }
2667 : // IGNORING unknown units.
2668 : }
2669 826 : std::vector < std::pair<double, LaneAttributeChange >>& attributeChanges = myCurrentEdge.laneSections.back().lanesByDir[myCurrentLaneDirection].back().attributeChanges;
2670 826 : std::vector<std::pair<double, LaneAttributeChange> >::iterator i = std::find_if(attributeChanges.begin(), attributeChanges.end(), same_position_finder(pos));
2671 826 : if (i != attributeChanges.end()) {
2672 0 : (*i).second.speed = speed;
2673 : } else {
2674 : LaneAttributeChange lac = LaneAttributeChange(speed);
2675 826 : attributeChanges.push_back(std::make_pair(pos, lac));
2676 : }
2677 : }
2678 : }
2679 : break;
2680 0 : case OPENDRIVE_TAG_OBJECT: {
2681 0 : if (!attrs.hasAttribute(OPENDRIVE_ATTR_ID)) {
2682 0 : WRITE_WARNINGF(TL("Ignoring object without id at edge '%'."), toString(myCurrentEdge.id));
2683 0 : break;
2684 : }
2685 : OpenDriveObject o;
2686 0 : o.id = attrs.get<std::string>(OPENDRIVE_ATTR_ID, 0, ok);
2687 0 : o.type = attrs.getOpt<std::string>(OPENDRIVE_ATTR_TYPE, o.id.c_str(), ok, "", false);
2688 0 : o.name = attrs.getOpt<std::string>(OPENDRIVE_ATTR_NAME, o.id.c_str(), ok, "", false);
2689 0 : o.s = attrs.get<double>(OPENDRIVE_ATTR_S, o.id.c_str(), ok);
2690 0 : o.t = attrs.get<double>(OPENDRIVE_ATTR_T, o.id.c_str(), ok);
2691 0 : o.width = attrs.getOpt<double>(OPENDRIVE_ATTR_WIDTH, o.id.c_str(), ok, -1);
2692 0 : o.length = attrs.getOpt<double>(OPENDRIVE_ATTR_LENGTH, o.id.c_str(), ok, -1);
2693 0 : o.radius = attrs.getOpt<double>(OPENDRIVE_ATTR_RADIUS, o.id.c_str(), ok, -1);
2694 0 : o.hdg = attrs.getOpt<double>(OPENDRIVE_ATTR_HDG, o.id.c_str(), ok, 0);
2695 0 : myCurrentEdge.objects.push_back(o);
2696 0 : }
2697 0 : break;
2698 0 : case OPENDRIVE_TAG_REPEAT: {
2699 0 : if (myCurrentEdge.objects.empty()) {
2700 0 : WRITE_ERRORF(TL("Repeat without object at edge '%'."), toString(myCurrentEdge.id));
2701 : ok = false;
2702 : } else {
2703 0 : OpenDriveObject o = myCurrentEdge.objects.back();
2704 : const std::string baseID = o.id;
2705 0 : double dist = attrs.get<double>(OPENDRIVE_ATTR_DISTANCE, o.id.c_str(), ok);
2706 0 : if (dist == 0) {
2707 : // continuous feature. Split into parts (XXX exmport as a single polygon #5235)
2708 0 : dist = OptionsCont::getOptions().getFloat("opendrive.curve-resolution");
2709 : }
2710 :
2711 : myCurrentEdge.objects.pop_back();
2712 0 : const double length = attrs.get<double>(OPENDRIVE_ATTR_LENGTH, o.id.c_str(), ok);
2713 0 : o.s = attrs.getOpt<double>(OPENDRIVE_ATTR_S, o.id.c_str(), ok, o.s);
2714 0 : double wStart = attrs.getOpt<double>(OPENDRIVE_ATTR_WIDTHSTART, o.id.c_str(), ok, o.width);
2715 0 : double wEnd = attrs.getOpt<double>(OPENDRIVE_ATTR_WIDTHEND, o.id.c_str(), ok, o.width);
2716 0 : double tStart = attrs.getOpt<double>(OPENDRIVE_ATTR_TSTART, o.id.c_str(), ok, o.t);
2717 0 : double tEnd = attrs.getOpt<double>(OPENDRIVE_ATTR_TEND, o.id.c_str(), ok, o.t);
2718 : int index = 0;
2719 0 : for (double x = 0; x <= length + NUMERICAL_EPS; x += dist) {
2720 0 : o.id = baseID + "#" + toString(index++);
2721 0 : const double a = x / length;
2722 0 : o.width = wStart * (1 - a) + wEnd * a;
2723 0 : o.t = tStart * (1 - a) + tEnd * a;
2724 0 : myCurrentEdge.objects.push_back(o);
2725 0 : o.s += dist;
2726 : }
2727 0 : }
2728 : }
2729 : break;
2730 1 : case OPENDRIVE_TAG_INCLUDE: {
2731 1 : std::string includedFile = attrs.get<std::string>(OPENDRIVE_ATTR_FILE, 0, ok);
2732 1 : if (!FileHelpers::isAbsolute(includedFile)) {
2733 2 : includedFile = FileHelpers::getConfigurationRelative(getFileName(), includedFile);
2734 : }
2735 3 : PROGRESS_BEGIN_MESSAGE("Parsing included opendrive from '" + includedFile + "'");
2736 1 : myUseCurrentNode = true;
2737 1 : XMLSubSys::runParser(*this, includedFile);
2738 1 : PROGRESS_DONE_MESSAGE();
2739 : }
2740 1 : break;
2741 : default:
2742 : break;
2743 : }
2744 29436 : myElementStack.push_back(element);
2745 : }
2746 :
2747 :
2748 : void
2749 3379 : NIImporter_OpenDrive::myCharacters(int element, const std::string& cdata) {
2750 3379 : if (element == OPENDRIVE_TAG_GEOREFERENCE) {
2751 : size_t i = cdata.find("+proj");
2752 4 : if (i != std::string::npos) {
2753 3 : const std::string proj = cdata.substr(i);
2754 3 : if (proj != "") {
2755 3 : Boundary convBoundary;
2756 3 : Boundary origBoundary;
2757 : // XXX read values from the header
2758 3 : convBoundary.add(Position(0, 0));
2759 3 : origBoundary.add(Position(0, 0));
2760 : try {
2761 3 : GeoConvHelper::setLoaded(GeoConvHelper(proj, myOffset, origBoundary, convBoundary));
2762 0 : } catch (ProcessError& e) {
2763 0 : WRITE_ERRORF(TL("Could not set projection (%). This can be ignored with --ignore-errors."), std::string(e.what()));
2764 0 : }
2765 : }
2766 : } else {
2767 3 : WRITE_WARNINGF(TL("geoReference format '%' currently not supported"), cdata);
2768 : }
2769 : needsCharacterData(false);
2770 : }
2771 3379 : }
2772 :
2773 :
2774 : void
2775 29437 : NIImporter_OpenDrive::myEndElement(int element) {
2776 : myElementStack.pop_back();
2777 29437 : switch (element) {
2778 882 : case OPENDRIVE_TAG_ROAD:
2779 882 : myEdges[myCurrentEdge.id] = new OpenDriveEdge(myCurrentEdge);
2780 882 : break;
2781 630 : case OPENDRIVE_TAG_CONNECTION:
2782 630 : if (myConnectionWasEmpty) {
2783 0 : Connection c;
2784 0 : c.fromEdge = myCurrentIncomingRoad;
2785 0 : c.toEdge = myCurrentConnectingRoad;
2786 0 : c.fromLane = 0;
2787 0 : c.toLane = 0;
2788 0 : c.fromCP = OPENDRIVE_CP_END;
2789 0 : c.toCP = myCurrentContactPoint;
2790 0 : c.all = true;
2791 0 : if (myEdges.find(c.fromEdge) == myEdges.end()) {
2792 0 : WRITE_ERRORF(TL("In laneLink-element: incoming road '%' is not known."), c.fromEdge);
2793 : } else {
2794 0 : OpenDriveEdge* e = myEdges.find(c.fromEdge)->second;
2795 : e->connections.insert(c);
2796 : }
2797 0 : }
2798 : break;
2799 3 : case OPENDRIVE_TAG_CONTROLLER: {
2800 3 : myControllers.insert({ myCurrentController.id, myCurrentController });
2801 : }
2802 3 : break;
2803 963 : case OPENDRIVE_TAG_LANESECTION: {
2804 963 : myCurrentEdge.laneSections.back().buildLaneMapping(myTypeContainer);
2805 : }
2806 963 : break;
2807 30 : case OPENDRIVE_TAG_SIGNAL:
2808 : case OPENDRIVE_TAG_SIGNALREFERENCE: {
2809 30 : if (NIImporter_OpenDrive::myIgnoreMisplacedSignals) {
2810 : int intType = -1;
2811 : try {
2812 0 : intType = StringUtils::toInt(myCurrentEdge.signals.back().type);
2813 0 : } catch (NumberFormatException&) {
2814 : break;
2815 0 : } catch (EmptyData&) {
2816 : break;
2817 0 : }
2818 0 : if (intType < 1000001 || (intType > 1000013 && intType != 1000020) || intType == 1000008) {
2819 : // not a traffic_light (Section 6.11)
2820 : break;
2821 : }
2822 0 : double s = myCurrentEdge.signals.back().s;
2823 0 : int minLane = myCurrentEdge.signals.back().minLane;
2824 0 : int maxLane = myCurrentEdge.signals.back().maxLane;
2825 : bool foundDrivingType = false;
2826 0 : for (OpenDriveLaneSection ls : myCurrentEdge.laneSections) {
2827 0 : if (ls.s <= s && ls.s + ls.length > s) {
2828 0 : if (myCurrentEdge.signals.back().orientation < 0) {
2829 0 : for (OpenDriveLane l : ls.lanesByDir[OPENDRIVE_TAG_LEFT]) {
2830 0 : if ((minLane < 0 && l.id >= minLane && l.id <= maxLane) && l.type == "driving") {
2831 : foundDrivingType = true;
2832 : }
2833 0 : }
2834 0 : } else if (myCurrentEdge.signals.back().orientation > 0) { // 0 = center is never used for driving
2835 0 : for (OpenDriveLane l : ls.lanesByDir[OPENDRIVE_TAG_RIGHT]) {
2836 0 : if ((minLane > 0 && l.id >= minLane && l.id <= maxLane) && l.type == "driving") {
2837 : foundDrivingType = true;
2838 : }
2839 0 : }
2840 : }
2841 : }
2842 0 : }
2843 0 : if (!foundDrivingType) { // reject signal / signal reference if not on driving lane
2844 : myCurrentEdge.signals.pop_back();
2845 : }
2846 : }
2847 : }
2848 : break;
2849 : default:
2850 : break;
2851 : }
2852 29437 : }
2853 :
2854 :
2855 :
2856 : void
2857 1686 : NIImporter_OpenDrive::addLink(LinkType lt, const std::string& elementType,
2858 : const std::string& elementID,
2859 : const std::string& contactPoint) {
2860 : OpenDriveLink l(lt, elementID);
2861 : // elementType
2862 1686 : if (elementType == "road") {
2863 1260 : l.elementType = OPENDRIVE_ET_ROAD;
2864 426 : } else if (elementType == "junction") {
2865 426 : l.elementType = OPENDRIVE_ET_JUNCTION;
2866 : }
2867 : // contact point
2868 1686 : if (contactPoint == "start") {
2869 824 : l.contactPoint = OPENDRIVE_CP_START;
2870 862 : } else if (contactPoint == "end") {
2871 862 : l.contactPoint = OPENDRIVE_CP_END;
2872 : }
2873 : // add
2874 1686 : myCurrentEdge.links.push_back(l);
2875 1686 : }
2876 :
2877 :
2878 : void
2879 1326 : NIImporter_OpenDrive::addGeometryShape(GeometryType type, const std::vector<double>& vals) {
2880 : // checks
2881 1326 : if (myCurrentEdge.geometries.size() == 0) {
2882 0 : throw ProcessError(TLF("Mismatching parenthesis in geometry definition for road '%'", myCurrentEdge.id));
2883 : }
2884 : OpenDriveGeometry& last = myCurrentEdge.geometries.back();
2885 1326 : if (last.type != OPENDRIVE_GT_UNKNOWN) {
2886 0 : throw ProcessError(TLF("Double geometry information for road '%'", myCurrentEdge.id));
2887 : }
2888 : // set
2889 1326 : last.type = type;
2890 1326 : last.params = vals;
2891 1326 : }
2892 :
2893 :
2894 : bool
2895 3955 : operator<(const NIImporter_OpenDrive::Connection& c1, const NIImporter_OpenDrive::Connection& c2) {
2896 3955 : if (c1.fromEdge != c2.fromEdge) {
2897 0 : return c1.fromEdge < c2.fromEdge;
2898 : }
2899 3955 : if (c1.toEdge != c2.toEdge) {
2900 2334 : return c1.toEdge < c2.toEdge;
2901 : }
2902 1621 : if (c1.fromLane != c2.fromLane) {
2903 280 : return c1.fromLane < c2.fromLane;
2904 : }
2905 1341 : return c1.toLane < c2.toLane;
2906 : }
2907 :
2908 : void
2909 252 : NIImporter_OpenDrive::sanitizeWidths(OpenDriveEdge* e) {
2910 : #ifdef DEBUG_VARIABLE_WIDTHS
2911 : if (DEBUG_COND(e)) {
2912 : gDebugFlag1 = true;
2913 : std::cout << "sanitizeWidths e=" << e->id << " sections=" << e->laneSections.size() << "\n";
2914 : }
2915 : #endif
2916 575 : for (OpenDriveLaneSection& sec : e->laneSections) {
2917 : // filter widths within the current section (#5888).
2918 : // @note, Short laneSections could also be worth filtering alltogether
2919 323 : if (sec.rightLaneNumber > 0) {
2920 320 : sanitizeWidths(sec.lanesByDir[OPENDRIVE_TAG_RIGHT], sec.length);
2921 : }
2922 323 : if (sec.leftLaneNumber > 0) {
2923 102 : sanitizeWidths(sec.lanesByDir[OPENDRIVE_TAG_LEFT], sec.length);
2924 : }
2925 : }
2926 252 : }
2927 :
2928 : void
2929 422 : NIImporter_OpenDrive::sanitizeWidths(std::vector<OpenDriveLane>& lanes, double length) {
2930 1334 : for (OpenDriveLane& l : lanes) {
2931 912 : if (l.widthData.size() > 0) {
2932 : auto& wd = l.widthData;
2933 : const double threshold = POSITION_EPS;
2934 : double maxNoShort = -std::numeric_limits<double>::max();
2935 : double seen = 0;
2936 1844 : for (int i = 0; i < (int)wd.size(); i++) {
2937 932 : const double wdLength = i < (int)wd.size() - 1 ? wd[i + 1].s - wd[i].s : length - seen;
2938 932 : seen += wdLength;
2939 932 : if (wdLength > threshold) {
2940 932 : maxNoShort = MAX2(maxNoShort, wd[i].a);
2941 : }
2942 : }
2943 912 : if (maxNoShort > 0) {
2944 825 : l.width = maxNoShort;
2945 : #ifdef DEBUG_VARIABLE_WIDTHS
2946 : if (gDebugFlag1) {
2947 : std::cout << " lane=" << l.id << " width=" << l.width << "\n";
2948 : }
2949 : #endif
2950 : }
2951 : }
2952 : }
2953 422 : }
2954 :
2955 :
2956 : void
2957 252 : NIImporter_OpenDrive::splitMinWidths(OpenDriveEdge* e, const NBTypeCont& tc, double minDist) {
2958 : std::vector<OpenDriveLaneSection> newSections;
2959 : #ifdef DEBUG_VARIABLE_WIDTHS
2960 : if (DEBUG_COND(e)) {
2961 : gDebugFlag1 = true;
2962 : std::cout << "splitMinWidths e=" << e->id << " sections=" << e->laneSections.size() << "\n";
2963 : }
2964 : #endif
2965 575 : for (std::vector<OpenDriveLaneSection>::iterator j = e->laneSections.begin(); j != e->laneSections.end(); ++j) {
2966 : OpenDriveLaneSection& sec = *j;
2967 : std::vector<double> splitPositions;
2968 323 : const double sectionEnd = (j + 1) == e->laneSections.end() ? e->length : (*(j + 1)).s;
2969 323 : const int section = (int)(j - e->laneSections.begin());
2970 : #ifdef DEBUG_VARIABLE_WIDTHS
2971 : if (DEBUG_COND(e)) {
2972 : std::cout << " findWidthSplit section=" << section << " sectionStart=" << sec.s << " sectionOrigStart=" << sec.sOrig << " sectionEnd=" << sectionEnd << "\n";
2973 : }
2974 : #endif
2975 323 : if (sec.rightLaneNumber > 0) {
2976 320 : findWidthSplit(tc, sec.lanesByDir[OPENDRIVE_TAG_RIGHT], section, sec.sOrig, sectionEnd, splitPositions);
2977 : }
2978 323 : if (sec.leftLaneNumber > 0) {
2979 102 : findWidthSplit(tc, sec.lanesByDir[OPENDRIVE_TAG_LEFT], section, sec.sOrig, sectionEnd, splitPositions);
2980 : }
2981 323 : newSections.push_back(sec);
2982 323 : std::sort(splitPositions.begin(), splitPositions.end());
2983 : // filter out tiny splits
2984 323 : double prevSplit = sec.s;
2985 334 : for (std::vector<double>::iterator it = splitPositions.begin(); it != splitPositions.end();) {
2986 11 : if ((*it) - prevSplit < minDist || sectionEnd - (*it) < minDist) {
2987 : // avoid tiny (or duplicate) splits
2988 : #ifdef DEBUG_VARIABLE_WIDTHS
2989 : if (DEBUG_COND(e)) {
2990 : std::cout << " skip close split=" << (*it) << " prevSplit=" << prevSplit << "\n";
2991 : }
2992 : #endif
2993 : it = splitPositions.erase(it);
2994 7 : } else if ((*it) < sec.s) {
2995 : // avoid splits for another section
2996 : #ifdef DEBUG_VARIABLE_WIDTHS
2997 : if (DEBUG_COND(e)) {
2998 : std::cout << " skip early split=" << (*it) << " s=" << sec.s << "\n";
2999 : }
3000 : #endif
3001 : it = splitPositions.erase(it);
3002 : } else {
3003 : prevSplit = *it;
3004 : it++;
3005 : }
3006 : }
3007 :
3008 323 : if (splitPositions.size() > 0) {
3009 : #ifdef DEBUG_VARIABLE_WIDTHS
3010 : if (DEBUG_COND(e)) {
3011 : std::cout << " road=" << e->id << " splitMinWidths section=" << section
3012 : << " start=" << sec.s
3013 : << " origStart=" << sec.sOrig
3014 : << " end=" << sectionEnd << " minDist=" << minDist
3015 : << " splitPositions=" << toString(splitPositions) << "\n";
3016 : }
3017 : #endif
3018 : #ifdef DEBUG_VARIABLE_WIDTHS
3019 : if (DEBUG_COND(e)) {
3020 : std::cout << "first section...\n";
3021 : }
3022 : #endif
3023 7 : recomputeWidths(newSections.back(), sec.sOrig, splitPositions.front(), sec.sOrig, sectionEnd);
3024 14 : for (std::vector<double>::iterator it = splitPositions.begin(); it != splitPositions.end(); ++it) {
3025 7 : OpenDriveLaneSection secNew = sec;
3026 7 : secNew.s = *it;
3027 : #ifdef DEBUG_VARIABLE_WIDTHS
3028 : if (DEBUG_COND(e)) {
3029 : std::cout << "splitAt " << secNew.s << "\n";
3030 : }
3031 : #endif
3032 7 : newSections.push_back(secNew);
3033 7 : if (secNew.rightLaneNumber > 0) {
3034 7 : setStraightConnections(newSections.back().lanesByDir[OPENDRIVE_TAG_RIGHT]);
3035 : }
3036 7 : if (secNew.leftLaneNumber > 0) {
3037 7 : setStraightConnections(newSections.back().lanesByDir[OPENDRIVE_TAG_LEFT]);
3038 : }
3039 7 : double end = (it + 1) == splitPositions.end() ? sectionEnd : *(it + 1);
3040 7 : recomputeWidths(newSections.back(), secNew.s, end, sec.sOrig, sectionEnd);
3041 7 : }
3042 : }
3043 323 : }
3044 252 : gDebugFlag1 = false;
3045 252 : e->laneSections = newSections;
3046 252 : }
3047 :
3048 :
3049 : void
3050 422 : NIImporter_OpenDrive::findWidthSplit(const NBTypeCont& tc, std::vector<OpenDriveLane>& lanes,
3051 : int section, double sectionStart, double sectionEnd,
3052 : std::vector<double>& splitPositions) {
3053 : UNUSED_PARAMETER(section);
3054 1334 : for (const OpenDriveLane& l : lanes) {
3055 912 : const SVCPermissions permissions = tc.getEdgeTypePermissions(l.type) & ~SVC_VULNERABLE;
3056 912 : if (l.widthData.size() > 0 && tc.knows(l.type) && !tc.getEdgeTypeShallBeDiscarded(l.type) && permissions != 0) {
3057 398 : double sPrev = l.widthData.front().s;
3058 : double wPrev = l.widthData.front().computeAt(sPrev);
3059 : #ifdef DEBUG_VARIABLE_WIDTHS
3060 : if (gDebugFlag1) std::cout
3061 : << "findWidthSplit section=" << section
3062 : << " sectionStart=" << sectionStart
3063 : << " sectionEnd=" << sectionEnd
3064 : << " lane=" << l.id
3065 : << " type=" << l.type
3066 : << " widthEntries=" << l.widthData.size() << "\n"
3067 : << " s=" << sPrev
3068 : << " w=" << wPrev
3069 : << "\n";
3070 : #endif
3071 814 : for (std::vector<OpenDriveWidth>::const_iterator it_w = l.widthData.begin(); it_w != l.widthData.end(); ++it_w) {
3072 416 : double sEnd = (it_w + 1) != l.widthData.end() ? (*(it_w + 1)).s : sectionEnd - sectionStart;
3073 : double w = (*it_w).computeAt(sEnd);
3074 : #ifdef DEBUG_VARIABLE_WIDTHS
3075 : if (gDebugFlag1) std::cout
3076 : << " sEnd=" << sEnd
3077 : << " s=" << (*it_w).s
3078 : << " a=" << (*it_w).a << " b=" << (*it_w).b << " c=" << (*it_w).c << " d=" << (*it_w).d
3079 : << " w=" << w
3080 : << "\n";
3081 : #endif
3082 416 : const double changeDist = fabs(myMinWidth - wPrev);
3083 416 : if (((wPrev < myMinWidth) && (w > myMinWidth))
3084 405 : || ((wPrev > myMinWidth) && (w < myMinWidth))) {
3085 11 : double splitPos = sPrev + (sEnd - sPrev) / fabs(w - wPrev) * changeDist;
3086 : double wSplit = (*it_w).computeAt(splitPos);
3087 : #ifdef DEBUG_VARIABLE_WIDTHS
3088 : if (gDebugFlag1) {
3089 : std::cout << " candidate splitPos=" << splitPos << " w=" << wSplit << "\n";
3090 : }
3091 : #endif
3092 : // ensure that the thin part is actually thin enough
3093 46 : while (wSplit > myMinWidth) {
3094 35 : if (wPrev < myMinWidth) {
3095 : // getting wider
3096 35 : splitPos -= POSITION_EPS;
3097 35 : if (splitPos < sPrev) {
3098 : #ifdef DEBUG_VARIABLE_WIDTHS
3099 : if (gDebugFlag1) {
3100 : std::cout << " aborting search splitPos=" << splitPos << " wSplit=" << wSplit << " sPrev=" << sPrev << " wPrev=" << wPrev << "\n";
3101 : }
3102 : #endif
3103 : splitPos = sPrev;
3104 : break;
3105 : }
3106 : } else {
3107 : // getting thinner
3108 0 : splitPos += POSITION_EPS;
3109 0 : if (splitPos > sEnd) {
3110 : #ifdef DEBUG_VARIABLE_WIDTHS
3111 : if (gDebugFlag1) {
3112 : std::cout << " aborting search splitPos=" << splitPos << " wSplit=" << wSplit << " sEnd=" << sEnd << " w=" << w << "\n";
3113 : }
3114 : #endif
3115 : splitPos = sEnd;
3116 : break;
3117 : }
3118 : }
3119 : wSplit = (*it_w).computeAt(splitPos);
3120 : #ifdef DEBUG_VARIABLE_WIDTHS
3121 : if (gDebugFlag1) {
3122 : std::cout << " refined splitPos=" << splitPos << " w=" << wSplit << "\n";
3123 : }
3124 : #endif
3125 : }
3126 11 : splitPositions.push_back(sectionStart + splitPos);
3127 : }
3128 : // //wPrev = wSplit;
3129 : //} else if ((fabs(wPrev) < NUMERICAL_EPS && w > POSITION_EPS)
3130 : // || (wPrev > POSITION_EPS && fabs(w) < NUMERICAL_EPS)) {
3131 : // splitPositions.push_back(sectionStart + sPrev);
3132 : // if (gDebugFlag1) std::cout << " laneDisappears candidate splitPos=" << sPrev << " wPrev=" << wPrev << " w=" << w<< "\n";
3133 : //}
3134 : wPrev = w;
3135 : sPrev = sEnd;
3136 : }
3137 : }
3138 : }
3139 422 : }
3140 :
3141 :
3142 : void
3143 14 : NIImporter_OpenDrive::setStraightConnections(std::vector<OpenDriveLane>& lanes) {
3144 53 : for (std::vector<OpenDriveLane>::iterator k = lanes.begin(); k != lanes.end(); ++k) {
3145 39 : (*k).predecessor = (*k).id;
3146 : }
3147 14 : }
3148 :
3149 :
3150 : void
3151 14 : NIImporter_OpenDrive::recomputeWidths(OpenDriveLaneSection& sec, double start, double end, double sectionStart, double sectionEnd) {
3152 14 : if (sec.rightLaneNumber > 0) {
3153 14 : recomputeWidths(sec.lanesByDir[OPENDRIVE_TAG_RIGHT], start, end, sectionStart, sectionEnd);
3154 : }
3155 14 : if (sec.leftLaneNumber > 0) {
3156 14 : recomputeWidths(sec.lanesByDir[OPENDRIVE_TAG_LEFT], start, end, sectionStart, sectionEnd);
3157 : }
3158 14 : }
3159 :
3160 :
3161 : void
3162 28 : NIImporter_OpenDrive::recomputeWidths(std::vector<OpenDriveLane>& lanes, double start, double end, double sectionStart, double sectionEnd) {
3163 106 : for (std::vector<OpenDriveLane>::iterator k = lanes.begin(); k != lanes.end(); ++k) {
3164 : OpenDriveLane& l = *k;
3165 78 : if (l.widthData.size() > 0) {
3166 : #ifdef DEBUG_VARIABLE_WIDTHS
3167 : if (gDebugFlag1) std::cout
3168 : << "recomputeWidths lane=" << l.id
3169 : << " type=" << l.type
3170 : << " start=" << start
3171 : << " end=" << end
3172 : << " sectionStart=" << sectionStart
3173 : << " sectionEnd=" << sectionEnd
3174 : << " widthEntries=" << l.widthData.size() << "\n"
3175 : << "\n";
3176 : #endif
3177 78 : l.width = 0;
3178 78 : double sPrev = l.widthData.front().s;
3179 78 : double sPrevAbs = sPrev + sectionStart;
3180 156 : for (std::vector<OpenDriveWidth>::iterator it_w = l.widthData.begin(); it_w != l.widthData.end(); ++it_w) {
3181 78 : double sEnd = (it_w + 1) != l.widthData.end() ? (*(it_w + 1)).s : sectionEnd - sectionStart;
3182 78 : double sEndAbs = sEnd + sectionStart;
3183 : #ifdef DEBUG_VARIABLE_WIDTHS
3184 : if (gDebugFlag1) std::cout
3185 : << " sPrev=" << sPrev << " sPrevAbs=" << sPrevAbs
3186 : << " sEnd=" << sEnd << " sEndAbs=" << sEndAbs
3187 : << " widthData s=" << (*it_w).s
3188 : << " a=" << (*it_w).a
3189 : << " b=" << (*it_w).b
3190 : << " c=" << (*it_w).c
3191 : << " d=" << (*it_w).d
3192 : << "\n";
3193 : #endif
3194 78 : if (sPrevAbs <= start && sEndAbs >= start) {
3195 : #ifdef DEBUG_VARIABLE_WIDTHS
3196 : if (gDebugFlag1) {
3197 : std::cout << " atStart=" << start << " pos=" << start - sectionStart << " w=" << (*it_w).computeAt(start - sectionStart) << "\n";
3198 : }
3199 : #endif
3200 156 : l.width = MAX2(l.width, (*it_w).computeAt(start - sectionStart));
3201 : }
3202 78 : if (sPrevAbs <= end && sEndAbs >= end) {
3203 : #ifdef DEBUG_VARIABLE_WIDTHS
3204 : if (gDebugFlag1) {
3205 : std::cout << " atEnd=" << end << " pos=" << end - sectionStart << " w=" << (*it_w).computeAt(end - sectionStart) << "\n";
3206 : }
3207 : #endif
3208 156 : l.width = MAX2(l.width, (*it_w).computeAt(end - sectionStart));
3209 : }
3210 78 : if (start <= sPrevAbs && end >= sPrevAbs) {
3211 : #ifdef DEBUG_VARIABLE_WIDTHS
3212 : if (gDebugFlag1) {
3213 : std::cout << " atSPrev=" << sPrev << " w=" << (*it_w).computeAt(sPrev) << "\n";
3214 : }
3215 : #endif
3216 67 : l.width = MAX2(l.width, (*it_w).computeAt(sPrev));
3217 : }
3218 78 : if (start <= sEndAbs && end >= sEndAbs) {
3219 : #ifdef DEBUG_VARIABLE_WIDTHS
3220 : if (gDebugFlag1) {
3221 : std::cout << " atSEnd=" << sEnd << " w=" << (*it_w).computeAt(sEnd) << "\n";
3222 : }
3223 : #endif
3224 78 : l.width = MAX2(l.width, (*it_w).computeAt(sEnd));
3225 : }
3226 : #ifdef DEBUG_VARIABLE_WIDTHS
3227 : if (gDebugFlag1) {
3228 : std::cout << " sPrev=" << sPrev << " sEnd=" << sEnd << " l.width=" << l.width << "\n";
3229 : }
3230 : #endif
3231 : sPrev = sEnd;
3232 : sPrevAbs = sEndAbs;
3233 : }
3234 : }
3235 : }
3236 28 : }
3237 :
3238 : /****************************************************************************/
|
