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