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