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