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)
198 bool customLaneShapes = oc.
getBool(
"opendrive.lane-shapes");
202 std::map<std::string, OpenDriveEdge*> edges;
206 for (
const std::string& file : oc.
getStringVector(
"opendrive-files")) {
213 for (
auto& item : edges) {
215 if (signal.type ==
"") {
216 if (handler.
getSignals().count(signal.id) == 0) {
220 signal.type = ref.
type;
221 signal.name = ref.
name;
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;
235 outerEdges[(*i).first] = (*i).second;
250 std::map<std::string, Boundary> posMap;
251 std::map<std::string, std::string> edge2junction;
252 std::vector<NodeSet> joinedNodeIDs;
254 for (std::map<std::string, OpenDriveEdge*>::iterator i = innerEdges.begin(); i != innerEdges.end(); ++i) {
258 if (posMap.find(e->
junction) == posMap.end()) {
264 for (std::map<std::string, Boundary>::iterator i = posMap.begin(); i != posMap.end(); ++i) {
271 for (std::map<std::string, OpenDriveEdge*>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
273 for (std::vector<OpenDriveLink>::iterator j = e->
links.begin(); j != e->
links.end(); ++j) {
288 if (edge2junction.find(l.
elementID) != edge2junction.end()) {
300 for (std::map<std::string, OpenDriveEdge*>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
302 for (std::vector<OpenDriveLink>::iterator j = e->
links.begin(); j != e->
links.end(); ++j) {
309 std::string id1 = e->
id;
314 std::string nid = id1 +
"." + id2;
337 for (std::map<std::string, OpenDriveEdge*>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
339 if (e->
to !=
nullptr && e->
from !=
nullptr) {
342 for (std::map<std::string, OpenDriveEdge*>::iterator j = innerEdges.begin(); j != innerEdges.end(); ++j) {
344 for (std::vector<OpenDriveLink>::iterator k = ie->
links.begin(); k != ie->
links.end(); ++k) {
350 std::string nid = edge2junction[ie->
id];
362 for (std::map<std::string, OpenDriveEdge*>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
364 if ((e->
from ==
nullptr || e->
to ==
nullptr) && e->
geom.size() == 0) {
367 if (e->
from ==
nullptr) {
368 const std::string nid = e->
id +
".begin";
371 if (e->
to ==
nullptr) {
372 const std::string nid = e->
id +
".end";
377 std::map<NBNode*, NBNode*> joinedNodes;
378 for (
NodeSet& joined : joinedNodeIDs) {
380 for (
NBNode* j : joined) {
381 joinedPos.
add(j->getPosition());
383 joinedPos.
mul(1. / (
double)joined.size());
385 if (!nc.
insert(joinedID, joinedPos)) {
389 for (
NBNode* j : joined) {
393 for (std::map<std::string, OpenDriveEdge*>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
395 if (joinedNodes.count(e->
from) != 0) {
399 if (joinedNodes.count(e->
to) != 0) {
401 e->
to = joinedNodes[e->
to];
413 std::map<std::pair<NBEdge*, int>,
int> laneIndexMap;
415 for (std::map<std::string, OpenDriveEdge*>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
417 if (e->
geom.size() < 2) {
421 bool lanesBuilt =
false;
447 std::cout <<
" geomWithOffset=" << geomWithOffset <<
"\n";
450 const double length2D = geomWithOffset.
length2D();
451 double cF = length2D == 0 ? 1 : e->
length / length2D;
452 NBEdge* prevRight =
nullptr;
453 NBEdge* prevLeft =
nullptr;
461 WRITE_WARNING(
"Edge '" + e->
id +
"' has to be split as it connects same junctions.")
465 const double minDist = oc.
getFloat(
"opendrive.curve-resolution");
470 int sectionIndex = 0;
477 double nextS = (j + 1)->s;
478 const std::string nodeID = e->
id + (positionIDs ?
"." +
toString(nextS) :
"#" +
toString(sectionIndex + 1));
486 std::string
id = e->
id;
488 if (sFrom != e->
from || sTo != e->
to) {
494 id =
id +
"#" +
toString(sectionIndex++);
496#ifdef DEBUG_VARIABLE_WIDTHS
498 std::cout <<
" id=" <<
id <<
" sB=" << sB <<
" sE=" << sE <<
" geom=" << geom <<
"\n";
503 NBEdge* currRight =
nullptr;
504 if ((*j).rightLaneNumber > 0) {
505 std::vector<double> offsets(geom.size(), 0);
506 bool useOffsets =
false;
513 rightGeom.
move2side((*j).discardedInnerWidthRight);
516 std::cout <<
" -" <<
id <<
"_geom=" << geom <<
" -" <<
id <<
"_rightGeom=" << rightGeom <<
"\n";
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;
531 laneIndexMap[std::make_pair(currRight, sumoLaneIndex)] = (*k).id;
537 }
else if (customLaneShapes) {
540 if (customLaneShapes) {
551 if (prevRight !=
nullptr) {
553 for (std::map<int, int>::const_iterator k = connections.begin(); k != connections.end(); ++k) {
554#ifdef DEBUG_CONNECTIONS
556 std::cout <<
"addCon1 from=" << prevRight->
getID() <<
"_" << (*k).first <<
" to=" << currRight->
getID() <<
"_" << (*k).second <<
"\n";
562 prevRight = currRight;
567 NBEdge* currLeft =
nullptr;
568 if ((*j).leftLaneNumber > 0) {
569 std::vector<double> offsets(geom.size(), 0);
570 bool useOffsets =
false;
572 leftGeom.
move2side(-(*j).discardedInnerWidthLeft);
576 std::cout <<
" " <<
id <<
"_geom=" << geom <<
" " <<
id <<
"_leftGeom=" << leftGeom <<
"\n";
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;
589 laneIndexMap[std::make_pair(currLeft, sumoLaneIndex)] = (*k).id;
595 }
else if (customLaneShapes) {
598 if (customLaneShapes) {
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
614 std::cout <<
"addCon2 from=" << currLeft->
getID() <<
"_" << (*k).first <<
" to=" << prevLeft->
getID() <<
"_" << (*k).second <<
"\n";
629 if (oc.
isSet(
"polygon-output")) {
636 if (oc.
isSet(
"polygon-output")) {
637 for (
auto item : innerEdges) {
646 for (std::map<std::string, OpenDriveEdge*>::iterator i = edges.begin(); i != edges.end(); ++i) {
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;
654 for (std::set<Connection>::const_iterator i = conns.begin(); i != conns.end(); ++i) {
655 if (innerEdges.find((*i).fromEdge) != innerEdges.end()) {
659 if (innerEdges.find((*i).toEdge) != innerEdges.end()) {
660 std::set<Connection> seen;
663 connections2.push_back(*i);
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";
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);
678 int fromLane = (*i).fromLane;
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);
689 int toLane = (*i).toLane;
709 if (from ==
nullptr) {
710 WRITE_WARNINGF(
TL(
"Could not find fromEdge representation of '%' in connection '%'."), fromEdge, (*i).origID);
713 WRITE_WARNINGF(
TL(
"Could not find fromEdge representation of '%' in connection '%'."), toEdge, (*i).origID);
715 if (from ==
nullptr || to ==
nullptr) {
719#ifdef DEBUG_CONNECTIONS
721 std::cout <<
"addCon3 from=" << from->
getID() <<
"_" << fromLane <<
" to=" << to->
getID() <<
"_" << toLane <<
"\n";
733 if ((*i).origID !=
"" && saveOrigIDs) {
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) {
749 std::map<std::string, std::string> signal2junction;
750 std::map<std::string, OpenDriveController>& controllers = handler.
getControllers();
752 for (
const auto& it : edges) {
755 if (signal.controller.size() == 0) {
758 std::string junctionID;
760 if ((connection.fromLane < 0 && signal.orientation < 0) || (connection.fromLane > 0 && signal.orientation > 0)) {
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);
768 controllers[signal.controller].junction = connectedEdge->
junction;
774 const bool importSignalGroups = oc.
getBool(
"opendrive.signal-groups");
775 for (std::map<std::string, OpenDriveEdge*>::iterator i = edges.begin(); i != edges.end(); ++i) {
782 if (intType < 1000001 || (intType > 1000013 && intType != 1000020) || intType == 1000008) {
790 std::vector<OpenDriveLaneSection>::iterator k = e->
laneSections.begin();
793 if (signal.s > (*k).s && signal.s <= (*(k + 1)).s) {
800 std::string
id = (*k).sumoID;
805 std::string fromID, toID;
806 for (std::vector<OpenDriveLink>::const_iterator l = e->
links.begin(); l != e->
links.end(); ++l) {
814 if (signal.orientation < 0) {
815 fromID =
"-" + fromID;
819 if (signal.orientation > 0) {
820 fromID =
"-" + fromID;
838 if (from ==
nullptr) {
839 WRITE_WARNINGF(
TL(
"Could not find edge '%' for signal '%'."), fromID, signal.id);
844 if (signal.maxLane != 0) {
845 bool fromForward = from->
getID()[0] ==
'-';
846 bool lanesForward = signal.maxLane < 0;
847 if (fromForward != lanesForward) {
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);
860 if (c.toEdge == to) {
861 int odLane = laneIndexMap[std::make_pair(from, c.fromLane)];
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);
867 c.setParameter(
"signalID", signal.id);
871 if (importSignalGroups) {
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);
879 c.tlLinkIndex = tlIndex;
880 c.setParameter(
"controllerID", controller.
id);
890 WRITE_WARNINGF(
TL(
"Found a traffic light signal on an unknown edge (original edge id='%')."), e->
id);
895 if (signal.orientation == 1) {
900 if (edge ==
nullptr) {
901 WRITE_WARNINGF(
TL(
"Could not find edge '%' for signal '%'."),
id, signal.id);
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);
912 c.setParameter(
"signalID", signal.id);
917 if (importSignalGroups) {
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);
925 c.tlLinkIndex = tlIndex;
926 c.setParameter(
"controllerID", controller.
id);
942 for (std::map<std::string, OpenDriveEdge*>::iterator i = edges.begin(); i != edges.end(); ++i) {
969 centerLine.push_back(
Position(-o.length / 2, 0));
970 centerLine.push_back(
Position(o.length / 2, 0));
972 centerLine.
rotate2D(roadHdg + o.hdg);
996std::pair<NBEdge*, NBEdge*>
1008 return std::make_pair(from, to);
1046 if (sumoLane.
width >= 0 && widthResolution > 0) {
1047 sumoLane.
width = floor(sumoLane.
width / widthResolution + 0.5) * widthResolution;
1049 sumoLane.
width -= widthResolution;
1050 if (sumoLane.
width <= 0) {
1053 }
else if (sumoLane.
width == 0) {
1055 sumoLane.
width = widthResolution;
1061 if (forbiddenNarrow) {
1069 const std::map<std::string, OpenDriveEdge*>& innerEdges,
1070 const std::map<std::string, OpenDriveEdge*>& edges,
1072 std::vector<Connection>& into, std::set<Connection>& seen) {
1075#ifdef DEBUG_CONNECTIONS
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 <<
" ";
1085 if (dest ==
nullptr) {
1091 auto innerEdgesIt = innerEdges.find(destCon.toEdge);
1092#ifdef DEBUG_CONNECTIONS
1094 std::cout <<
" toInner=" << (innerEdgesIt != innerEdges.end()) <<
" destCon " << destCon.getDescription() <<
"\n";
1097 if (innerEdgesIt != innerEdges.end()) {
1098 std::vector<Connection> t;
1099 if (seen.count(destCon) == 0) {
1101 for (std::vector<Connection>::const_iterator j = t.begin(); j != t.end(); ++j) {
1109 cn.shape = innerEdgesIt->second->geom + c.
shape;
1118 int out = destCon.fromLane;
1123#ifdef DEBUG_CONNECTIONS
1125 std::cout <<
" laneSectionsConnected dest=" << dest->
id <<
" in=" << in <<
" out=" << out
1142 int referenceLane = 0;
1143 int offsetFactor = 1;
1147 for (
const auto& destLane : dest->
laneSections.front().lanesByDir[lanesDir]) {
1148 if (destLane.successor == c.
fromLane) {
1149 referenceLane = destLane.id;
1155 for (
const auto& destLane : dest->
laneSections.front().lanesByDir[lanesDir]) {
1156 if (destLane.predecessor == c.
fromLane) {
1157 referenceLane = destLane.id;
1166 std::vector<double> offsets(dest->
geom.size(), 0);
1170#ifdef DEBUG_INTERNALSHAPES
1171 std::string destPred;
1175 for (
int laneSectionIndex = 0; laneSectionIndex < (int)dest->
laneSections.size(); laneSectionIndex++) {
1177 const double nextS = laneSectionIndex + 1 < (int)dest->
laneSections.size() ? dest->
laneSections[laneSectionIndex + 1].s : std::numeric_limits<double>::max();
1180 int finalI = iShape;
1183 double sectionS = 0;
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";
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";
1199 int widthDataIndex = 0;
1200 while (s < nextS && i < (
int)
cn.shape.size()) {
1202 const double dist =
cn.shape[i - 1].distanceTo2D(
cn.shape[i]);
1207 while (widthDataIndex + 1 < (
int)destLane.widthData.size()
1208 && sectionS >= destLane.widthData[widthDataIndex + 1].s) {
1212 if (destLane.widthData.size() > 0) {
1213 width = destLane.widthData[widthDataIndex].computeAt(sectionS);
1215#ifdef DEBUG_INTERNALSHAPES
1216 std::cout <<
" missing width data at inner edge " << dest->
id <<
" to=" <<
cn.toEdge <<
"_" <<
cn.toLane <<
" cp=" <<
cn.toCP <<
"\n";
1223 if (outerToLane.id ==
cn.toLane && outerToLane.width > 0) {
1224#ifdef DEBUG_INTERNALSHAPES
1225 std::cout <<
" using toLane width " << width <<
"\n";
1231 offsets[i] += width * multiplier;
1239 }
else if (finalS == s) {
1241 while (s < nextS && i < (
int)
cn.shape.size()) {
1243 const double dist =
cn.shape[i - 1].distanceTo2D(
cn.shape[i]);
1259 cn.shape.move2sideCustom(offsets);
1264#ifdef DEBUG_INTERNALSHAPES
1265 std::cout <<
"internalShape "
1267 <<
" dest=" << dest->
id
1268 <<
" refLane=" << referenceLane
1269 <<
" destPred\n" << destPred
1270 <<
" offsets=" << offsets
1271 <<
"\n shape=" << dest->
geom
1272 <<
"\n shape2=" <<
cn.shape
1279#ifdef DEBUG_CONNECTIONS
1281 std::cout <<
" added connection\n";
1301 if (lane.id == in) {
1302 in = lane.successor;
1309 if (lane.id == in) {
1310 in = lane.successor;
1323 for (std::vector<OpenDriveLink>::iterator i = e.
links.begin(); i != e.
links.end(); ++i) {
1330 std::string connectedEdge = l.
elementID;
1331 std::string edgeID = e.
id;
1334 const std::map<int, int>& laneMap = laneSection.
laneMap;
1335#ifdef DEBUG_CONNECTIONS
1337 std::cout <<
"edge=" << e.
id <<
" eType=" << l.
elementType <<
" lType=" << l.
linkType <<
" connectedEdge=" << connectedEdge <<
" laneSection=" << laneSection.
s <<
" map:\n";
1343 for (std::vector<OpenDriveLane>::const_iterator j = lanes.begin(); j != lanes.end(); ++j) {
1352 c.
toEdge = connectedEdge;
1360 if (edges.find(c.
fromEdge) == edges.end()) {
1365#ifdef DEBUG_CONNECTIONS
1367 std::cout <<
"insertConRight from=" << src->
id <<
"_" << c.
fromLane <<
" to=" << c.
toEdge <<
"_" << c.
toLane <<
"\n";
1375 for (std::vector<OpenDriveLane>::const_iterator j = lanes.begin(); j != lanes.end(); ++j) {
1392 if (edges.find(c.
fromEdge) == edges.end()) {
1397#ifdef DEBUG_CONNECTIONS
1399 std::cout <<
"insertConLeft from=" << src->
id <<
"_" << c.
fromLane <<
" to=" << c.
toEdge <<
"_" << c.
toLane <<
"\n";
1411 return id.substr(1);
1422 if (!nc.
insert(
id, pos)) {
1438 NBNode* toJoin =
nullptr;
1440 if (e.
to !=
nullptr && e.
to != n) {
1445 if (e.
from !=
nullptr && e.
from != n) {
1450 if (toJoin !=
nullptr) {
1454 for (
NodeSet& joined : joinedNodeIDs) {
1455 if (joined.count(toJoin) != 0) {
1458 if (joined.count(n) != 0) {
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) {
1471 set1->insert(set2->begin(), set2->end());
1472 joinedNodeIDs.erase(std::find(joinedNodeIDs.begin(), joinedNodeIDs.end(), *set2));
1483 if (el.c != 0 || el.d != 0) {
1493 const double res = oc.
getFloat(
"opendrive.curve-resolution");
1494 for (std::map<std::string, OpenDriveEdge*>::iterator i = edges.begin(); i != edges.end(); ++i) {
1499 for (std::vector<OpenDriveGeometry>::iterator j = e.
geometries.begin(); j != e.
geometries.end(); ++j) {
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);
1534 for (PositionVector::iterator k = geom.begin(); k != geom.end(); ++k) {
1540 if (e.
geom.size() == 1 && e.
geom.front() != last) {
1542 e.
geom.push_back(last);
1546 std::cout <<
" initialGeom=" << e.
geom <<
"\n";
1549 if (oc.
exists(
"geometry.min-dist") && !oc.
isDefault(
"geometry.min-dist")) {
1552 if (e.
geom.size() > 4) {
1558 std::cout <<
" reducedGeom=" << e.
geom <<
"\n";
1569 for (std::vector<OpenDriveElevation>::iterator j = e.
elevations.begin(); j != e.
elevations.end(); ++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) {
1577 if (k < (
int)e.
geom.size()) {
1580 pos += e.
geom[k - 1].distanceTo2D(e.
geom[k]);
1597 std::vector<double> laneOffsets;
1613 for (
auto j = offsets.begin(); j != offsets.end(); ++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);
1620 if (kk < (
int)geom.size()) {
1623 ppos += geom[kk - 1].distanceTo2D(geom[kk]);
1643 double interpolatedOffset = 0;
1645 interpolatedOffset = result.front();
1646 }
else if (at == (
int)geom.size() - 1) {
1647 interpolatedOffset = result.back();
1649 interpolatedOffset = (result[at - 1] + result[at]) / 2;
1651 result.insert(result.begin() + at, interpolatedOffset);
1657 const int sign = left ? -1 : 1;
1658 for (
auto j = offsets.begin(); j != offsets.end(); ++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;
1665 if (kk < (
int)geom.size()) {
1668 ppos += geom[kk - 1].distanceTo2D(geom[kk]);
1677 for (std::map<std::string, OpenDriveEdge*>::iterator i = edges.begin(); i != edges.end(); ++i) {
1679#ifdef DEBUG_VARIABLE_SPEED
1682 std::cout <<
"revisitLaneSections e=" << e.
id <<
"\n";
1685 std::vector<OpenDriveLaneSection>& laneSections = e.
laneSections;
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);
1694 std::copy(splitSections.begin(), splitSections.end(), back_inserter(newSections));
1703 for (std::vector<OpenDriveLaneSection>::const_iterator j = laneSections.begin(); j != laneSections.end() && sorted; ++j) {
1704 if ((*j).s <= lastS) {
1716 for (std::vector<OpenDriveLaneSection>::iterator j = laneSections.begin(); j != laneSections.end();) {
1717 bool simlarToLast = fabs((*j).s - lastS) < POSITION_EPS;
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);
1728#ifdef DEBUG_VARIABLE_SPEED
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));
1749 ret.push_back(start);
1760 double curveStart = g.
params[0];
1761 double curveEnd = g.
params[1];
1763 double cDot = (curveEnd - curveStart) / g.
length;
1764 if (cDot == 0 || g.
length == 0) {
1769 double sStart = curveStart / cDot;
1770 double sEnd = curveEnd / cDot;
1776 odrSpiral(sStart, cDot, &x, &y, &tStart);
1777 for (s = sStart; s <= sEnd; s += resolution) {
1788 assert(ret.size() >= 2);
1789 assert(ret[0] != ret[1]);
1792 ret.
add(ret.front() * -1);
1798 << std::setprecision(4)
1799 <<
"edge=" << e.
id <<
" s=" << g.
s
1800 <<
" cStart=" << curveStart
1801 <<
" cEnd=" << curveEnd
1803 <<
" sStart=" << sStart
1807 <<
"\n beforeShift=" << ret1
1808 <<
"\n beforeRot=" << ret2
1812 ret.
add(g.
x, g.
y, 0);
1813 }
catch (
const std::runtime_error&
error) {
1825 double centerX = g.
x;
1826 double centerY = g.
y;
1828 double curvature = g.
params[0];
1829 double radius = 1. / curvature;
1834 double startX = g.
x;
1835 double startY = g.
y;
1836 double geo_posS = g.
s;
1837 double geo_posE = g.
s;
1840 geo_posE += resolution;
1841 if (geo_posE - g.
s > g.
length) {
1844 if (geo_posE - g.
s > g.
length) {
1847 calcPointOnCurve(&endX, &endY, centerX, centerY, radius, geo_posE - geo_posS);
1848 ret.push_back(
Position(startX, startY));
1852 geo_posS = geo_posE;
1854 if (geo_posE - (g.
s + g.
length) < 0.001 && geo_posE - (g.
s + g.
length) > -0.001) {
1858 ret.push_back(
Position(startX, startY));
1866 const double s = sin(g.
hdg);
1867 const double c = cos(g.
hdg);
1869 for (
double off = 0; off < g.
length + 2.; off += resolution) {
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));
1883 const double s = sin(g.
hdg);
1884 const double c = cos(g.
hdg);
1886 const double pStep = pMax / ceil(g.
length / resolution);
1888 for (
double p = 0; p <= pMax + pStep; p += pStep) {
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));
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);
1917 if (ad_radius > 0) {
1924 normX = normX * cos(ad_hdg) + normY * sin(ad_hdg);
1925 normY = tmpX * sin(ad_hdg) + normY * cos(ad_hdg);
1928 normX = turn * normY;
1929 normY = -turn * tmpX;
1931 normX = fabs(ad_radius) * normX;
1932 normY = fabs(ad_radius) * normY;
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;
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;
1973 discardedInnerWidthRight = 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) {
1980 discardedInnerWidthRight = 0;
1981 laneMap[(*i).id] = sumoLane++;
1982 types.push_back((*i).type);
1983 if (types.front() != types.back()) {
1987 discardedInnerWidthRight += (*i).width;
1990 discardedInnerWidthLeft = 0;
1991 rightLaneNumber = sumoLane;
1992 rightType = sumoLane > 0 ? (singleType ? types.front() :
joinToString(types,
"|")) :
"";
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) {
1999 discardedInnerWidthLeft = 0;
2000 laneMap[(*i).id] = sumoLane++;
2001 types.push_back((*i).type);
2002 if (types.front() != types.back()) {
2006 discardedInnerWidthLeft += (*i).width;
2009 leftLaneNumber = sumoLane;
2010 leftType = sumoLane > 0 ? (singleType ? types.front() :
joinToString(types,
"|")) :
"";
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()) {
2024 int to = (*toP).second;
2027 from = (*i).predecessor;
2030 std::map<int, int>::const_iterator fromP = prev.
laneMap.find(from);
2031 if (fromP != prev.
laneMap.end()) {
2032 from = (*fromP).second;
2038 if (ret.find(from) != ret.end()) {
2063 l.
speed = (*it).second.speed;
2073 l.
speed = (*it).second.speed;
2083 const std::vector<std::string>& denied)
const {
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) {
2089 for (
const std::string& allow : allowed) {
2090 if (allow ==
"simulator") {
2093 }
else if (allow ==
"autonomousTraffic" || allow ==
"autonomous traffic" || allow ==
"throughTraffic") {
2096 }
else if (allow ==
"pedestrian") {
2098 }
else if (allow ==
"passengerCar") {
2100 }
else if (allow ==
"bus") {
2102 }
else if (allow ==
"delivery") {
2104 }
else if (allow ==
"emergency") {
2106 }
else if (allow ==
"taxi") {
2108 }
else if (allow ==
"bicycle") {
2110 }
else if (allow ==
"motorcycle") {
2112 }
else if (allow ==
"truck" || allow ==
"trucks") {
2117 }
else if (denied.size() > 0) {
2118 for (
const std::string& deny : denied) {
2119 if (deny ==
"none") {
2122 }
else if (deny ==
"autonomousTraffic" || deny ==
"autonomous traffic" || deny ==
"throughTraffic") {
2125 }
else if (deny ==
"pedestrian") {
2126 perms &= ~SVC_PEDESTRIAN;
2127 }
else if (deny ==
"passengerCar") {
2128 perms &= ~SVC_PASSENGER;
2129 }
else if (deny ==
"bus") {
2131 }
else if (deny ==
"delivery") {
2132 perms &= ~SVC_DELIVERY;
2133 }
else if (deny ==
"emergency") {
2134 perms &= ~SVC_EMERGENCY;
2135 }
else if (deny ==
"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;
2153 std::set<double> attributeChangePositions;
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);
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);
2175 if (attributeChangePositions.size() == 0) {
2179 if (*attributeChangePositions.begin() > 0) {
2180 attributeChangePositions.insert(0);
2182#ifdef DEBUG_VARIABLE_SPEED
2184 <<
" buildSpeedChanges sectionStart=" << s
2185 <<
" speedChangePositions=" <<
joinToString(speedChangePositions,
", ")
2188 for (std::set<double>::iterator i = attributeChangePositions.begin(); i != attributeChangePositions.end(); ++i) {
2189 if (i == attributeChangePositions.begin()) {
2190 newSections.push_back(*
this);
2192 newSections.push_back(buildLaneSection(tc, *i));
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) {
2202 l.
speed = newSections[i - 1].lanesByDir[k.first][j].speed;
2209 l.
permission = newSections[i - 1].lanesByDir[k.first][j].permission;
2210 l.
type = newSections[i - 1].lanesByDir[k.first][j].type;
2230 for (std::vector<OpenDriveSignal>::const_iterator i = signals.begin(); i != signals.end(); ++i) {
2232 if ((*i).type ==
"301" || (*i).type ==
"306") {
2235 if ((*i).type ==
"205" ) {
2349 std::vector<double> vals;
2355 std::vector<double> vals;
2362 std::vector<double> vals;
2368 std::vector<double> vals;
2377 std::vector<double> vals;
2387 if (pRange ==
"normalized") {
2388 vals.push_back(1.0);
2389 }
else if (pRange ==
"arcLength") {
2390 vals.push_back(-1.0);
2393 vals.push_back(1.0);
2441 int orientationCode = orientation ==
"-" ? -1 : orientation ==
"+" ? 1 : 0;
2452 int orientationCode = orientation ==
"-" ? -1 : orientation ==
"+" ? 1 : 0;
2508 WRITE_ERRORF(
TL(
"In laneLink-element: incoming road '%' is not known."), c.fromEdge);
2524 l.width =
MAX2(l.width, a);
2526#ifdef DEBUG_VARIABLE_WIDTHS
2533 <<
" type=" << l.type
2534 <<
" width=" << l.width
2540 <<
" entries=" << l.widthData.size()
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);
2563 if (rule ==
"allow") {
2564 lac.
allowed.push_back(vClass);
2565 }
else if (rule ==
"deny") {
2566 lac.
denied.push_back(vClass);
2568 attributeChanges.push_back(std::make_pair(pos, lac));
2580 if (!unit.empty()) {
2582 if (unit ==
"km/h") {
2585 if (unit ==
"mph") {
2586 speed *= 1.609344 / 3.6;
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;
2596 attributeChanges.push_back(std::make_pair(pos, lac));
2625 const std::string baseID = o.
id;
2640 for (
double x = 0; x <= length + NUMERICAL_EPS; x += dist) {
2642 const double a = x / length;
2643 o.
width = wStart * (1 - a) + wEnd * a;
2644 o.
t = tStart * (1 - a) + tEnd * a;
2661 size_t i = cdata.find(
"+proj");
2662 if (i != std::string::npos) {
2663 const std::string proj = cdata.substr(i);
2675 WRITE_ERRORF(
TL(
"Could not set projection (%). This can be ignored with --ignore-errors."), std::string(e.what()));
2679 WRITE_WARNINGF(
TL(
"geoReference format '%' currently not supported"), cdata);
2728 const std::string& elementID,
2729 const std::string& contactPoint) {
2732 if (elementType ==
"road") {
2734 }
else if (elementType ==
"junction") {
2738 if (contactPoint ==
"start") {
2740 }
else if (contactPoint ==
"end") {
2780#ifdef DEBUG_VARIABLE_WIDTHS
2783 std::cout <<
"sanitizeWidths e=" << e->
id <<
" sections=" << e->
laneSections.size() <<
"\n";
2789 if (sec.rightLaneNumber > 0) {
2792 if (sec.leftLaneNumber > 0) {
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();
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;
2809 if (wdLength > threshold) {
2810 maxNoShort =
MAX2(maxNoShort, wd[i].a);
2813 if (maxNoShort > 0) {
2814 l.width = maxNoShort;
2823 std::vector<OpenDriveLaneSection> newSections;
2824#ifdef DEBUG_VARIABLE_WIDTHS
2827 std::cout <<
"splitMinWidths e=" << e->
id <<
" sections=" << e->
laneSections.size() <<
"\n";
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
2837 std::cout <<
" findWidthSplit section=" << section <<
" sectionStart=" << sec.
s <<
" sectionOrigStart=" << sec.
sOrig <<
" sectionEnd=" << sectionEnd <<
"\n";
2846 newSections.push_back(sec);
2847 std::sort(splitPositions.begin(), splitPositions.end());
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) {
2853#ifdef DEBUG_VARIABLE_WIDTHS
2855 std::cout <<
" skip close split=" << (*it) <<
" prevSplit=" << prevSplit <<
"\n";
2858 it = splitPositions.erase(it);
2859 }
else if ((*it) < sec.
s) {
2861#ifdef DEBUG_VARIABLE_WIDTHS
2863 std::cout <<
" skip early split=" << (*it) <<
" s=" << sec.
s <<
"\n";
2866 it = splitPositions.erase(it);
2873 if (splitPositions.size() > 0) {
2874#ifdef DEBUG_VARIABLE_WIDTHS
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";
2883#ifdef DEBUG_VARIABLE_WIDTHS
2885 std::cout <<
"first section...\n";
2889 for (std::vector<double>::iterator it = splitPositions.begin(); it != splitPositions.end(); ++it) {
2892#ifdef DEBUG_VARIABLE_WIDTHS
2894 std::cout <<
"splitAt " << secNew.
s <<
"\n";
2897 newSections.push_back(secNew);
2904 double end = (it + 1) == splitPositions.end() ? sectionEnd : *(it + 1);
2916 int section,
double sectionStart,
double sectionEnd,
2917 std::vector<double>& splitPositions) {
2919 for (std::vector<OpenDriveLane>::iterator k = lanes.begin(); k != lanes.end(); ++k) {
2924 double wPrev = l.
widthData.front().computeAt(sPrev);
2926 <<
"findWidthSplit section=" << section
2927 <<
" sectionStart=" << sectionStart
2928 <<
" sectionEnd=" << sectionEnd
2930 <<
" type=" << l.
type
2931 <<
" widthEntries=" << l.
widthData.size() <<
"\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);
2940 <<
" s=" << (*it_w).s
2941 <<
" a=" << (*it_w).a <<
" b=" << (*it_w).b <<
" c=" << (*it_w).c <<
" d=" << (*it_w).d
2944 const double changeDist = fabs(
myMinWidth - wPrev);
2947 double splitPos = sPrev + (sEnd - sPrev) / fabs(w - wPrev) * changeDist;
2948 double wSplit = (*it_w).computeAt(splitPos);
2950 std::cout <<
" candidate splitPos=" << splitPos <<
" w=" << wSplit <<
"\n";
2956 splitPos -= POSITION_EPS;
2957 if (splitPos < sPrev) {
2959 std::cout <<
" aborting search splitPos=" << splitPos <<
" wSplit=" << wSplit <<
" sPrev=" << sPrev <<
" wPrev=" << wPrev <<
"\n";
2966 splitPos += POSITION_EPS;
2967 if (splitPos > sEnd) {
2969 std::cout <<
" aborting search splitPos=" << splitPos <<
" wSplit=" << wSplit <<
" sEnd=" << sEnd <<
" w=" << w <<
"\n";
2975 wSplit = (*it_w).computeAt(splitPos);
2977 std::cout <<
" refined splitPos=" << splitPos <<
" w=" << wSplit <<
"\n";
2980 splitPositions.push_back(sectionStart + splitPos);
2998 for (std::vector<OpenDriveLane>::iterator k = lanes.begin(); k != lanes.end(); ++k) {
2999 (*k).predecessor = (*k).id;
3017 for (std::vector<OpenDriveLane>::iterator k = lanes.begin(); k != lanes.end(); ++k) {
3020#ifdef DEBUG_VARIABLE_WIDTHS
3022 <<
"recomputeWidths lane=" << l.
id
3023 <<
" type=" << l.
type
3024 <<
" start=" << start
3026 <<
" sectionStart=" << sectionStart
3027 <<
" sectionEnd=" << sectionEnd
3028 <<
" widthEntries=" << l.
widthData.size() <<
"\n"
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
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
3048 if (sPrevAbs <= start && sEndAbs >= start) {
3049#ifdef DEBUG_VARIABLE_WIDTHS
3051 std::cout <<
" atStart=" << start <<
" pos=" << start - sectionStart <<
" w=" << (*it_w).computeAt(start - sectionStart) <<
"\n";
3054 l.
width =
MAX2(l.
width, (*it_w).computeAt(start - sectionStart));
3056 if (sPrevAbs <= end && sEndAbs >= end) {
3057#ifdef DEBUG_VARIABLE_WIDTHS
3059 std::cout <<
" atEnd=" << end <<
" pos=" << end - sectionStart <<
" w=" << (*it_w).computeAt(end - sectionStart) <<
"\n";
3064 if (start <= sPrevAbs && end >= sPrevAbs) {
3065#ifdef DEBUG_VARIABLE_WIDTHS
3067 std::cout <<
" atSPrev=" << sPrev <<
" w=" << (*it_w).computeAt(sPrev) <<
"\n";
3072 if (start <= sEndAbs && end >= sEndAbs) {
3073#ifdef DEBUG_VARIABLE_WIDTHS
3075 std::cout <<
" atSEnd=" << sEnd <<
" w=" << (*it_w).computeAt(sEnd) <<
"\n";
3080#ifdef DEBUG_VARIABLE_WIDTHS
3082 std::cout <<
" sPrev=" << sPrev <<
" sEnd=" << sEnd <<
" l.width=" << l.
width <<
"\n";
#define WRITE_WARNINGF(...)
#define WRITE_ERRORF(...)
#define WRITE_WARNING(msg)
#define PROGRESS_DONE_MESSAGE()
#define PROGRESS_BEGIN_MESSAGE(msg)
std::set< NBNode *, ComparatorIdLess > NodeSet
#define DEBUG_COND3(roadID)
bool operator<(const NIImporter_OpenDrive::Connection &c1, const NIImporter_OpenDrive::Connection &c2)
@ 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
bool gDebugFlag1
global utility flags for debugging
#define UNUSED_PARAMETER(x)
std::string joinToString(const std::vector< T > &v, const T_BETWEEN &between, std::streamsize accuracy=gPrecision)
std::string toString(const T &t, std::streamsize accuracy=gPrecision)
A class that stores a 2D geometrical boundary.
void add(double x, double y, double z=0)
Makes the boundary include the given coordinate.
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.
bool insert(NBEdge *edge, bool ignorePrunning=false)
Adds an edge to the dictionary.
The representation of a single edge during network building.
const std::vector< Connection > & getConnections() const
Returns the connections.
NBNode * getToNode() const
Returns the destination node of the edge.
static const double UNSPECIFIED_FRICTION
unspecified lane friction
Lane & getLaneStruct(int lane)
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.
const std::string & getID() const
static const double UNSPECIFIED_LOADED_LENGTH
no length override given
static const double UNSPECIFIED_CONTPOS
unspecified internal junction position
static const double UNSPECIFIED_VISIBILITY_DISTANCE
unspecified foe visibility for connections
static const double UNSPECIFIED_SPEED
unspecified lane speed
@ 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.
static const double UNSPECIFIED_WIDTH
unspecified lane width
static const double UNSPECIFIED_OFFSET
unspecified lane offset
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.
std::string createClusterId(const NodeSet &cluster, const std::string &prefix="cluster_")
generate id from cluster node ids
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.
const std::set< NBTrafficLightDefinition * > & getControllingTLS() const
Returns the traffic lights that were assigned to this node (The set of tls that control this node)
void addTrafficLight(NBTrafficLightDefinition *tlDef)
Adds a traffic light to the list of traffic lights that control this node.
bool isTLControlled() const
Returns whether this node is controlled by any tls.
A traffic light logics which must be computed (only nodes/edges are given)
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.
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)
static bool myImportInternalShapes
OpenDriveXMLTag
Numbers representing openDrive-XML - element names.
@ OPENDRIVE_TAG_ELEVATION
@ OPENDRIVE_TAG_CONTROLLER
@ OPENDRIVE_TAG_PARAMPOLY3
@ OPENDRIVE_TAG_LANEOFFSET
@ OPENDRIVE_TAG_SIGNALREFERENCE
@ OPENDRIVE_TAG_GEOREFERENCE
@ OPENDRIVE_TAG_SUCCESSOR
@ OPENDRIVE_TAG_PREDECESSOR
@ OPENDRIVE_TAG_LANESECTION
@ OPENDRIVE_TAG_CONNECTION
void addGeometryShape(GeometryType type, const std::vector< double > &vals)
static bool myImportWidths
static void setStraightConnections(std::vector< OpenDriveLane > &lanes)
std::string myCurrentConnectingRoad
OpenDriveController myCurrentController
static void setLaneAttributes(const OpenDriveEdge *e, NBEdge::Lane &sumoLane, const OpenDriveLane &odLane, bool saveOrigIDs, const NBTypeCont &tc)
std::vector< int > myElementStack
~NIImporter_OpenDrive()
Destructor.
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
@ OPENDRIVE_ATTR_REVMAJOR
@ OPENDRIVE_ATTR_SIGNALID
@ OPENDRIVE_ATTR_CURVSTART
@ OPENDRIVE_ATTR_CONTACTPOINT
@ OPENDRIVE_ATTR_REVMINOR
@ OPENDRIVE_ATTR_ORIENTATION
@ OPENDRIVE_ATTR_INCOMINGROAD
@ OPENDRIVE_ATTR_CURVATURE
@ OPENDRIVE_ATTR_ELEMENTTYPE
@ OPENDRIVE_ATTR_JUNCTION
@ OPENDRIVE_ATTR_CONNECTINGROAD
@ OPENDRIVE_ATTR_WIDTHEND
@ OPENDRIVE_ATTR_FROMLANE
@ OPENDRIVE_ATTR_RESTRICTION
@ OPENDRIVE_ATTR_DISTANCE
@ OPENDRIVE_ATTR_ELEMENTID
@ OPENDRIVE_ATTR_WIDTHSTART
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.
Poly3 OpenDriveLaneOffset
OpenDriveEdge myCurrentEdge
static void sanitizeWidths(OpenDriveEdge *e)
GeometryType
OpenDrive geometry type enumeration.
@ OPENDRIVE_GT_PARAMPOLY3
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)
std::string myCurrentJunctionID
static void setEdgeLinks2(OpenDriveEdge &e, const std::map< std::string, OpenDriveEdge * > &edges)
std::string myCurrentIncomingRoad
static void splitMinWidths(OpenDriveEdge *e, const NBTypeCont &tc, double minDist)
bool myConnectionWasEmpty
static bool myImportAllTypes
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.
ContactPoint myCurrentContactPoint
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.
@ OPENDRIVE_LT_PREDECESSOR
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.
A storage for options typed value containers)
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.
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.
void set(double x, double y)
set positions x and y
double distanceTo2D(const Position &p2) const
returns the euclidean distance in the x-y-plane
double x() const
Returns the x-position.
void add(const Position &pos)
Adds the given position to this one.
void mul(double val)
Multiplies both positions with the given value.
double y() const
Returns the y-position.
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
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.
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
void odrSpiral(double s, double cDot, double *x, double *y, double *t)
A structure which describes a connection between edges or lanes.
An (internal) definition of a single lane of an edge.
double width
This lane's width.
PositionVector customShape
A custom shape for this lane set by the user.
std::string type
the type of this lane
double speed
The speed allowed on this lane.
SVCPermissions permissions
List of vehicle types that are allowed on this lane.
A connection between two roads.
std::string getDescription() const
Attribute set applied at a certain position along a lane.
std::vector< std::string > allowed
std::vector< std::string > denied
Representation of a signal group.
std::vector< std::string > signalIDs
Representation of an openDrive "link".
double length
The length of the edge.
std::vector< double > laneOffsets
std::string id
The id of the edge.
std::set< Connection > connections
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< OpenDriveLink > links
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< double > params
Representation of a lane.
std::vector< OpenDriveWidth > widthData
std::vector< std::pair< double, LaneAttributeChange > > attributeChanges
List of permission and speed changes.
std::string type
The lane's type.
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)
Representation of a lane section.
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)
OpenDriveLaneSection(double sArg)
Constructor.
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.
Representation of an OpenDrive link.
ContactPoint contactPoint
Representation of a signal.
std::string controller
the controller ID
double computeAt(double pos) const