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)
202 bool customLaneShapes = oc.
getBool(
"opendrive.lane-shapes");
206 std::map<std::string, OpenDriveEdge*> edges;
210 for (
const std::string& file : oc.
getStringVector(
"opendrive-files")) {
217 for (
auto& item : edges) {
219 if (signal.
type ==
"") {
234 std::map<std::string, OpenDriveEdge*> innerEdges, outerEdges;
235 for (std::map<std::string, OpenDriveEdge*>::iterator i = edges.begin(); i != edges.end(); ++i) {
236 if ((*i).second->isInner) {
237 innerEdges[(*i).first] = (*i).second;
239 outerEdges[(*i).first] = (*i).second;
254 std::map<std::string, Boundary> posMap;
255 std::map<std::string, std::string> edge2junction;
256 std::vector<NodeSet> joinedNodeIDs;
258 for (std::map<std::string, OpenDriveEdge*>::iterator i = innerEdges.begin(); i != innerEdges.end(); ++i) {
262 if (posMap.find(e->
junction) == posMap.end()) {
268 for (std::map<std::string, Boundary>::iterator i = posMap.begin(); i != posMap.end(); ++i) {
275 for (std::map<std::string, OpenDriveEdge*>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
277 for (std::vector<OpenDriveLink>::iterator j = e->
links.begin(); j != e->
links.end(); ++j) {
292 if (edge2junction.find(l.
elementID) != edge2junction.end()) {
304 for (std::map<std::string, OpenDriveEdge*>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
306 for (std::vector<OpenDriveLink>::iterator j = e->
links.begin(); j != e->
links.end(); ++j) {
313 std::string id1 = e->
id;
318 std::string nid = id1 +
"." + id2;
341 for (std::map<std::string, OpenDriveEdge*>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
343 if (e->
to !=
nullptr && e->
from !=
nullptr) {
346 for (std::map<std::string, OpenDriveEdge*>::iterator j = innerEdges.begin(); j != innerEdges.end(); ++j) {
348 for (std::vector<OpenDriveLink>::iterator k = ie->
links.begin(); k != ie->
links.end(); ++k) {
354 std::string nid = edge2junction[ie->
id];
366 for (std::map<std::string, OpenDriveEdge*>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
368 if ((e->
from ==
nullptr || e->
to ==
nullptr) && e->
geom.size() == 0) {
371 if (e->
from ==
nullptr) {
372 const std::string nid = e->
id +
".begin";
375 if (e->
to ==
nullptr) {
376 const std::string nid = e->
id +
".end";
381 std::map<NBNode*, NBNode*> joinedNodes;
382 for (
NodeSet& joined : joinedNodeIDs) {
384 for (
NBNode* j : joined) {
385 joinedPos.
add(j->getPosition());
387 joinedPos.
mul(1. / (
double)joined.size());
389 if (!nc.
insert(joinedID, joinedPos)) {
393 for (
NBNode* j : joined) {
397 for (std::map<std::string, OpenDriveEdge*>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
399 if (joinedNodes.count(e->
from) != 0) {
403 if (joinedNodes.count(e->
to) != 0) {
405 e->
to = joinedNodes[e->
to];
417 std::map<std::pair<NBEdge*, int>,
int> laneIndexMap;
419 for (std::map<std::string, OpenDriveEdge*>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
421 if (e->
geom.size() < 2) {
425 bool lanesBuilt =
false;
451 std::cout <<
" geomWithOffset=" << geomWithOffset <<
"\n";
454 const double length2D = geomWithOffset.
length2D();
455 double cF = length2D == 0 ? 1 : e->
length / length2D;
456 NBEdge* prevRight =
nullptr;
457 NBEdge* prevLeft =
nullptr;
465 WRITE_WARNING(
"Edge '" + e->
id +
"' has to be split as it connects same junctions.")
469 const double minDist = oc.
getFloat(
"opendrive.curve-resolution");
474 int sectionIndex = 0;
481 double nextS = (j + 1)->s;
482 const std::string nodeID = e->
id + (positionIDs ?
"." +
toString(nextS) :
"#" +
toString(sectionIndex + 1));
490 std::string
id = e->
id;
492 if (sFrom != e->
from || sTo != e->
to) {
498 id =
id +
"#" +
toString(sectionIndex++);
500 #ifdef DEBUG_VARIABLE_WIDTHS
502 std::cout <<
" id=" <<
id <<
" sB=" << sB <<
" sE=" << sE <<
" geom=" << geom <<
"\n";
507 NBEdge* currRight =
nullptr;
508 if ((*j).rightLaneNumber > 0) {
509 std::vector<double> offsets(geom.size(), 0);
510 bool useOffsets =
false;
517 rightGeom.
move2side((*j).discardedInnerWidthRight);
520 std::cout <<
" -" <<
id <<
"_geom=" << geom <<
" -" <<
id <<
"_rightGeom=" << rightGeom <<
"\n";
529 std::sort(lanes.begin(), lanes.end(),
LaneSorter());
530 for (std::vector<OpenDriveLane>::const_iterator k = lanes.begin(); k != lanes.end(); ++k) {
531 std::map<int, int>::const_iterator lp = (*j).laneMap.find((*k).id);
532 if (lp != (*j).laneMap.end()) {
533 int sumoLaneIndex = lp->second;
535 laneIndexMap[std::make_pair(currRight, sumoLaneIndex)] = (*k).id;
541 }
else if (customLaneShapes) {
544 if (customLaneShapes) {
555 if (prevRight !=
nullptr) {
557 for (std::map<int, int>::const_iterator k = connections.begin(); k != connections.end(); ++k) {
558 #ifdef DEBUG_CONNECTIONS
560 std::cout <<
"addCon1 from=" << prevRight->
getID() <<
"_" << (*k).first <<
" to=" << currRight->
getID() <<
"_" << (*k).second <<
"\n";
566 prevRight = currRight;
571 NBEdge* currLeft =
nullptr;
572 if ((*j).leftLaneNumber > 0) {
573 std::vector<double> offsets(geom.size(), 0);
574 bool useOffsets =
false;
576 leftGeom.
move2side(-(*j).discardedInnerWidthLeft);
580 std::cout <<
" " <<
id <<
"_geom=" << geom <<
" " <<
id <<
"_leftGeom=" << leftGeom <<
"\n";
587 std::sort(lanes.begin(), lanes.end(),
LaneSorter());
588 for (std::vector<OpenDriveLane>::const_iterator k = lanes.begin(); k != lanes.end(); ++k) {
589 std::map<int, int>::const_iterator lp = (*j).laneMap.find((*k).id);
590 if (lp != (*j).laneMap.end()) {
591 int sumoLaneIndex = lp->second;
593 laneIndexMap[std::make_pair(currLeft, sumoLaneIndex)] = (*k).id;
599 }
else if (customLaneShapes) {
602 if (customLaneShapes) {
613 if (prevLeft !=
nullptr) {
614 std::map<int, int> connections = (*j).getInnerConnections(
OPENDRIVE_TAG_LEFT, *(j - 1));
615 for (std::map<int, int>::const_iterator k = connections.begin(); k != connections.end(); ++k) {
616 #ifdef DEBUG_CONNECTIONS
618 std::cout <<
"addCon2 from=" << currLeft->
getID() <<
"_" << (*k).first <<
" to=" << prevLeft->
getID() <<
"_" << (*k).second <<
"\n";
633 if (oc.
isSet(
"polygon-output")) {
640 if (oc.
isSet(
"polygon-output")) {
641 for (
auto item : innerEdges) {
650 for (std::map<std::string, OpenDriveEdge*>::iterator i = edges.begin(); i != edges.end(); ++i) {
654 std::vector<Connection> connections2;
655 for (std::map<std::string, OpenDriveEdge*>::iterator j = edges.begin(); j != edges.end(); ++j) {
656 const std::set<Connection>& conns = (*j).second->connections;
658 for (std::set<Connection>::const_iterator i = conns.begin(); i != conns.end(); ++i) {
659 if (innerEdges.find((*i).fromEdge) != innerEdges.end()) {
663 if (innerEdges.find((*i).toEdge) != innerEdges.end()) {
664 std::set<Connection> seen;
667 connections2.push_back(*i);
672 for (std::vector<Connection>::const_iterator i = connections2.begin(); i != connections2.end(); ++i) {
673 #ifdef DEBUG_CONNECTIONS
674 std::cout <<
"connections2 " << (*i).getDescription() <<
"\n";
676 std::string fromEdge = (*i).fromEdge;
677 if (edges.find(fromEdge) == edges.end()) {
678 WRITE_WARNINGF(
TL(
"While setting connections: from-edge '%' is not known."), fromEdge);
682 int fromLane = (*i).fromLane;
686 std::string toEdge = (*i).toEdge;
687 if (edges.find(toEdge) == edges.end()) {
688 WRITE_WARNINGF(
TL(
"While setting connections: to-edge '%' is not known."), toEdge);
693 int toLane = (*i).toLane;
713 if (from ==
nullptr) {
714 WRITE_WARNINGF(
TL(
"Could not find fromEdge representation of '%' in connection '%'."), fromEdge, (*i).origID);
717 WRITE_WARNINGF(
TL(
"Could not find fromEdge representation of '%' in connection '%'."), toEdge, (*i).origID);
719 if (from ==
nullptr || to ==
nullptr) {
723 #ifdef DEBUG_CONNECTIONS
725 std::cout <<
"addCon3 from=" << from->
getID() <<
"_" << fromLane <<
" to=" << to->
getID() <<
"_" << toLane <<
"\n";
737 if ((*i).origID !=
"" && saveOrigIDs) {
740 for (std::vector<NBEdge::Connection>::iterator k = cons.begin(); k != cons.end(); ++k) {
741 if ((*k).fromLane == fromLane && (*k).toEdge == to && (*k).toLane == toLane) {
753 std::map<std::string, std::string> signal2junction;
754 std::map<std::string, OpenDriveController>& controllers = handler.
getControllers();
756 for (
const auto& it : edges) {
762 std::string junctionID;
764 if ((connection.fromLane < 0 && signal.
orientation < 0) || (connection.fromLane > 0 && signal.
orientation > 0)) {
768 const OpenDriveEdge* connectedEdge = edges[connection.toEdge];
770 WRITE_WARNINGF(
TL(
"Controlling multiple junctions by the same controller '%' is currently not implemented."), signal.
controller);
778 const bool importSignalGroups = oc.
getBool(
"opendrive.signal-groups");
779 for (std::map<std::string, OpenDriveEdge*>::iterator i = edges.begin(); i != edges.end(); ++i) {
786 if (intType < 1000001 || (intType > 1000013 && intType != 1000020) || intType == 1000008) {
794 std::vector<OpenDriveLaneSection>::iterator k = e->
laneSections.begin();
797 if (signal.
s > (*k).s && signal.
s <= (*(k + 1)).s) {
804 std::string
id = (*k).sumoID;
809 std::string fromID, toID;
810 for (std::vector<OpenDriveLink>::const_iterator l = e->
links.begin(); l != e->
links.end(); ++l) {
819 fromID =
"-" + fromID;
824 fromID =
"-" + fromID;
842 if (from ==
nullptr) {
849 bool fromForward = from->
getID()[0] ==
'-';
850 bool lanesForward = signal.
maxLane < 0;
851 if (fromForward != lanesForward) {
855 from = signalFromTo.first;
856 to = signalFromTo.second;
857 if (from ==
nullptr) {
864 if (c.toEdge == to) {
865 int odLane = laneIndexMap[std::make_pair(from, c.fromLane)];
868 if (c.hasParameter(
"signalID")) {
869 c.setParameter(
"signalID", c.getParameter(
"signalID") +
" " + signal.
id);
871 c.setParameter(
"signalID", signal.
id);
875 if (importSignalGroups) {
877 if (controller.
id !=
"") {
878 if (c.getParameter(
"controllerID") !=
"") {
879 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);
883 c.tlLinkIndex = tlIndex;
884 c.setParameter(
"controllerID", controller.
id);
894 WRITE_WARNINGF(
TL(
"Found a traffic light signal on an unknown edge (original edge id='%')."), e->
id);
904 if (edge ==
nullptr) {
911 int odLane = laneIndexMap[std::make_pair(edge, c.fromLane)];
913 if (c.hasParameter(
"signalID")) {
914 c.setParameter(
"signalID", c.getParameter(
"signalID") +
" " + signal.
id);
916 c.setParameter(
"signalID", signal.
id);
921 if (importSignalGroups) {
923 if (controller.
id !=
"") {
924 if (c.getParameter(
"controllerID") !=
"") {
925 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);
929 c.tlLinkIndex = tlIndex;
930 c.setParameter(
"controllerID", controller.
id);
946 for (std::map<std::string, OpenDriveEdge*>::iterator i = edges.begin(); i != edges.end(); ++i) {
968 POI.setParameter(
"name", o.name);
969 POI.writeXML(dev, writeGeo);
973 centerLine.push_back(
Position(-o.length / 2, 0));
974 centerLine.push_back(
Position(o.length / 2, 0));
976 centerLine.
rotate2D(roadHdg + o.hdg);
1000 std::pair<NBEdge*, NBEdge*>
1012 return std::make_pair(from, to);
1050 if (sumoLane.
width >= 0 && widthResolution > 0) {
1051 sumoLane.
width = floor(sumoLane.
width / widthResolution + 0.5) * widthResolution;
1053 sumoLane.
width -= widthResolution;
1054 if (sumoLane.
width <= 0) {
1057 }
else if (sumoLane.
width == 0) {
1059 sumoLane.
width = widthResolution;
1065 if (forbiddenNarrow) {
1073 const std::map<std::string, OpenDriveEdge*>& innerEdges,
1074 const std::map<std::string, OpenDriveEdge*>& edges,
1076 std::vector<Connection>& into, std::set<Connection>& seen) {
1079 #ifdef DEBUG_CONNECTIONS
1081 std::cout <<
" buildConnectionsToOuter " << c.
getDescription() <<
"\n";
1082 std::cout <<
" dest=" << (dest ==
nullptr ?
"NULL" : dest->
id) <<
" seenlist=";
1083 for (std::set<Connection>::const_iterator i = seen.begin(); i != seen.end(); ++i) {
1084 std::cout <<
" " << (*i).fromEdge <<
"," << (*i).toEdge <<
" ";
1089 if (dest ==
nullptr) {
1095 auto innerEdgesIt = innerEdges.find(destCon.
toEdge);
1096 #ifdef DEBUG_CONNECTIONS
1098 std::cout <<
" toInner=" << (innerEdgesIt != innerEdges.end()) <<
" destCon " << destCon.
getDescription() <<
"\n";
1101 if (innerEdgesIt != innerEdges.end()) {
1102 std::vector<Connection> t;
1103 if (seen.count(destCon) == 0) {
1105 for (std::vector<Connection>::const_iterator j = t.begin(); j != t.end(); ++j) {
1113 cn.shape = innerEdgesIt->second->geom + c.
shape;
1127 #ifdef DEBUG_CONNECTIONS
1129 std::cout <<
" laneSectionsConnected dest=" << dest->
id <<
" in=" << in <<
" out=" << out
1146 int referenceLane = 0;
1147 int offsetFactor = 1;
1151 for (
const auto& destLane : dest->
laneSections.front().lanesByDir[lanesDir]) {
1152 if (destLane.successor == c.
fromLane) {
1153 referenceLane = destLane.id;
1159 for (
const auto& destLane : dest->
laneSections.front().lanesByDir[lanesDir]) {
1160 if (destLane.predecessor == c.
fromLane) {
1161 referenceLane = destLane.id;
1170 std::vector<double> offsets(dest->
geom.size(), 0);
1174 #ifdef DEBUG_INTERNALSHAPES
1175 std::string destPred;
1179 for (
int laneSectionIndex = 0; laneSectionIndex < (int)dest->
laneSections.size(); laneSectionIndex++) {
1181 const double nextS = laneSectionIndex + 1 < (int)dest->
laneSections.size() ? dest->
laneSections[laneSectionIndex + 1].s : std::numeric_limits<double>::max();
1184 int finalI = iShape;
1187 double sectionS = 0;
1190 #ifdef DEBUG_INTERNALSHAPES
1191 destPred +=
" lane=" +
toString(destLane.id)
1192 +
" pred=" +
toString(destLane.predecessor)
1193 +
" succ=" +
toString(destLane.successor)
1194 +
" wStart=" + (destLane.widthData.empty() ?
"?" :
toString(destLane.widthData.front().computeAt(0)))
1195 +
" wEnd=" + (destLane.widthData.empty() ?
"?" :
toString(destLane.widthData.front().computeAt(
cn.shape.length2D())))
1196 +
" width=" +
toString(destLane.width) +
"\n";
1198 if (abs(destLane.id) <= abs(referenceLane)) {
1199 const double multiplier = offsetFactor * (destLane.id == referenceLane ? 0.5 : 1);
1200 #ifdef DEBUG_INTERNALSHAPES
1201 destPred +=
" multiplier=" +
toString(multiplier) +
"\n";
1203 int widthDataIndex = 0;
1204 while (s < nextS && i < (
int)
cn.shape.size()) {
1206 const double dist =
cn.shape[i - 1].distanceTo2D(
cn.shape[i]);
1211 while (widthDataIndex + 1 < (
int)destLane.widthData.size()
1212 && sectionS >= destLane.widthData[widthDataIndex + 1].s) {
1216 if (destLane.widthData.size() > 0) {
1217 width = destLane.widthData[widthDataIndex].computeAt(sectionS);
1219 #ifdef DEBUG_INTERNALSHAPES
1220 std::cout <<
" missing width data at inner edge " << dest->
id <<
" to=" <<
cn.toEdge <<
"_" <<
cn.toLane <<
" cp=" <<
cn.toCP <<
"\n";
1227 if (outerToLane.id ==
cn.toLane && outerToLane.width > 0) {
1228 #ifdef DEBUG_INTERNALSHAPES
1229 std::cout <<
" using toLane width " << width <<
"\n";
1235 offsets[i] += width * multiplier;
1243 }
else if (finalS == s) {
1245 while (s < nextS && i < (
int)
cn.shape.size()) {
1247 const double dist =
cn.shape[i - 1].distanceTo2D(
cn.shape[i]);
1263 cn.shape.move2sideCustom(offsets);
1268 #ifdef DEBUG_INTERNALSHAPES
1269 std::cout <<
"internalShape "
1271 <<
" dest=" << dest->
id
1272 <<
" refLane=" << referenceLane
1273 <<
" destPred\n" << destPred
1274 <<
" offsets=" << offsets
1275 <<
"\n shape=" << dest->
geom
1276 <<
"\n shape2=" <<
cn.shape
1280 cn.shape =
cn.shape.reverse();
1283 #ifdef DEBUG_CONNECTIONS
1285 std::cout <<
" added connection\n";
1305 if (lane.id == in) {
1306 in = lane.successor;
1313 if (lane.id == in) {
1314 in = lane.successor;
1327 for (std::vector<OpenDriveLink>::iterator i = e.
links.begin(); i != e.
links.end(); ++i) {
1334 std::string connectedEdge = l.
elementID;
1335 std::string edgeID = e.
id;
1338 const std::map<int, int>& laneMap = laneSection.
laneMap;
1339 #ifdef DEBUG_CONNECTIONS
1341 std::cout <<
"edge=" << e.
id <<
" eType=" << l.
elementType <<
" lType=" << l.
linkType <<
" connectedEdge=" << connectedEdge <<
" laneSection=" << laneSection.
s <<
" map:\n";
1347 for (std::vector<OpenDriveLane>::const_iterator j = lanes.begin(); j != lanes.end(); ++j) {
1356 c.
toEdge = connectedEdge;
1364 if (edges.find(c.
fromEdge) == edges.end()) {
1369 #ifdef DEBUG_CONNECTIONS
1371 std::cout <<
"insertConRight from=" << src->
id <<
"_" << c.
fromLane <<
" to=" << c.
toEdge <<
"_" << c.
toLane <<
"\n";
1379 for (std::vector<OpenDriveLane>::const_iterator j = lanes.begin(); j != lanes.end(); ++j) {
1396 if (edges.find(c.
fromEdge) == edges.end()) {
1401 #ifdef DEBUG_CONNECTIONS
1403 std::cout <<
"insertConLeft from=" << src->
id <<
"_" << c.
fromLane <<
" to=" << c.
toEdge <<
"_" << c.
toLane <<
"\n";
1415 return id.substr(1);
1426 if (!nc.
insert(
id, pos)) {
1442 NBNode* toJoin =
nullptr;
1444 if (e.
to !=
nullptr && e.
to != n) {
1449 if (e.
from !=
nullptr && e.
from != n) {
1454 if (toJoin !=
nullptr) {
1458 for (
NodeSet& joined : joinedNodeIDs) {
1459 if (joined.count(toJoin) != 0) {
1462 if (joined.count(n) != 0) {
1466 if (set1 ==
nullptr && set2 ==
nullptr) {
1467 joinedNodeIDs.push_back(
NodeSet());
1468 joinedNodeIDs.back().insert(n);
1469 joinedNodeIDs.back().insert(toJoin);
1470 }
else if (set1 ==
nullptr && set2 !=
nullptr) {
1471 set2->insert(toJoin);
1472 }
else if (set1 !=
nullptr && set2 ==
nullptr) {
1475 set1->insert(set2->begin(), set2->end());
1476 joinedNodeIDs.erase(std::find(joinedNodeIDs.begin(), joinedNodeIDs.end(), *set2));
1487 if (el.
c != 0 || el.
d != 0) {
1497 const double res = oc.
getFloat(
"opendrive.curve-resolution");
1498 for (std::map<std::string, OpenDriveEdge*>::iterator i = edges.begin(); i != edges.end(); ++i) {
1503 for (std::vector<OpenDriveGeometry>::iterator j = e.
geometries.begin(); j != e.
geometries.end(); ++j) {
1532 const int index = (int)(j - e.
geometries.begin());
1533 WRITE_WARNINGF(
TL(
"Mismatched geometry for edge '%' between geometry segments % and %."), e.
id, index - 1, index);
1538 for (PositionVector::iterator k = geom.begin(); k != geom.end(); ++k) {
1544 if (e.
geom.size() == 1 && e.
geom.front() != last) {
1546 e.
geom.push_back(last);
1550 std::cout <<
" initialGeom=" << e.
geom <<
"\n";
1553 if (oc.
exists(
"geometry.min-dist") && !oc.
isDefault(
"geometry.min-dist")) {
1556 if (e.
geom.size() > 4) {
1562 std::cout <<
" reducedGeom=" << e.
geom <<
"\n";
1573 for (std::vector<OpenDriveElevation>::iterator j = e.
elevations.begin(); j != e.
elevations.end(); ++j) {
1575 const double sNext = (j + 1) == e.
elevations.end() ? std::numeric_limits<double>::max() : (*(j + 1)).s;
1576 while (k < (
int)e.
geom.size() && pos < sNext) {
1581 if (k < (
int)e.
geom.size()) {
1584 pos += e.
geom[k - 1].distanceTo2D(e.
geom[k]);
1601 std::vector<double> laneOffsets;
1617 for (
auto j = offsets.begin(); j != offsets.end(); ++j) {
1619 const double sNext = (j + 1) == offsets.end() ? std::numeric_limits<double>::max() : (*(j + 1)).s;
1620 while (kk < (
int)geom.size() && ppos < sNext) {
1621 const double offset = el.
computeAt(ppos);
1622 laneOffsets.push_back(fabs(offset) > POSITION_EPS ? -offset : 0);
1624 if (kk < (
int)geom.size()) {
1627 ppos += geom[kk - 1].distanceTo2D(geom[kk]);
1647 double interpolatedOffset = 0;
1649 interpolatedOffset = result.front();
1650 }
else if (at == (
int)geom.size() - 1) {
1651 interpolatedOffset = result.back();
1653 interpolatedOffset = (result[at - 1] + result[at]) / 2;
1655 result.insert(result.begin() + at, interpolatedOffset);
1661 const int sign = left ? -1 : 1;
1662 for (
auto j = offsets.begin(); j != offsets.end(); ++j) {
1664 const double sNext = (j + 1) == offsets.end() ? std::numeric_limits<double>::max() : (*(j + 1)).s;
1665 while (kk < (
int)geom.size() && ppos < sNext) {
1666 const double offset = el.
computeAt(ppos);
1667 result[kk] += fabs(offset) > POSITION_EPS ? sign * offset : 0;
1669 if (kk < (
int)geom.size()) {
1672 ppos += geom[kk - 1].distanceTo2D(geom[kk]);
1681 for (std::map<std::string, OpenDriveEdge*>::iterator i = edges.begin(); i != edges.end(); ++i) {
1683 #ifdef DEBUG_VARIABLE_SPEED
1686 std::cout <<
"revisitLaneSections e=" << e.
id <<
"\n";
1689 std::vector<OpenDriveLaneSection>& laneSections = e.
laneSections;
1691 std::vector<OpenDriveLaneSection> newSections;
1692 for (std::vector<OpenDriveLaneSection>::iterator j = laneSections.begin(); j != laneSections.end(); ++j) {
1693 std::vector<OpenDriveLaneSection> splitSections;
1694 bool splitByAttrChange = (*j).buildAttributeChanges(tc, splitSections);
1695 if (!splitByAttrChange) {
1696 newSections.push_back(*j);
1698 std::copy(splitSections.begin(), splitSections.end(), back_inserter(newSections));
1707 for (std::vector<OpenDriveLaneSection>::const_iterator j = laneSections.begin(); j != laneSections.end() && sorted; ++j) {
1708 if ((*j).s <= lastS) {
1720 for (std::vector<OpenDriveLaneSection>::iterator j = laneSections.begin(); j != laneSections.end();) {
1721 bool simlarToLast = fabs((*j).s - lastS) < POSITION_EPS;
1725 if (simlarToLast && !e.
isInner) {
1726 WRITE_WARNINGF(
TL(
"Almost duplicate s-value '%' for lane sections occurred at edge '%'; second entry was removed."),
toString(lastS), e.
id);
1727 j = laneSections.erase(j);
1732 #ifdef DEBUG_VARIABLE_SPEED
1745 if (resolution > 0 && g.
length > 0) {
1746 const int numPoints = (int)ceil(g.
length / resolution) + 1;
1747 double dx = (end.
x() - start.
x()) / (numPoints - 1);
1748 double dy = (end.
y() - start.
y()) / (numPoints - 1);
1749 for (
int i = 0; i < numPoints; i++) {
1750 ret.push_back(
Position(g.
x + i * dx, g.
y + i * dy));
1753 ret.push_back(start);
1764 double curveStart = g.
params[0];
1765 double curveEnd = g.
params[1];
1767 double cDot = (curveEnd - curveStart) / g.
length;
1768 if (cDot == 0 || g.
length == 0) {
1773 double sStart = curveStart / cDot;
1774 double sEnd = curveEnd / cDot;
1780 odrSpiral(sStart, cDot, &x, &y, &tStart);
1781 for (s = sStart; s <= sEnd; s += resolution) {
1792 assert(ret.size() >= 2);
1793 assert(ret[0] != ret[1]);
1796 ret.
add(ret.front() * -1);
1802 << std::setprecision(4)
1803 <<
"edge=" << e.
id <<
" s=" << g.
s
1804 <<
" cStart=" << curveStart
1805 <<
" cEnd=" << curveEnd
1807 <<
" sStart=" << sStart
1811 <<
"\n beforeShift=" << ret1
1812 <<
"\n beforeRot=" << ret2
1816 ret.
add(g.
x, g.
y, 0);
1817 }
catch (
const std::runtime_error&
error) {
1829 double centerX = g.
x;
1830 double centerY = g.
y;
1832 double curvature = g.
params[0];
1833 double radius = 1. / curvature;
1838 double startX = g.
x;
1839 double startY = g.
y;
1840 double geo_posS = g.
s;
1841 double geo_posE = g.
s;
1844 geo_posE += resolution;
1845 if (geo_posE - g.
s > g.
length) {
1848 if (geo_posE - g.
s > g.
length) {
1851 calcPointOnCurve(&endX, &endY, centerX, centerY, radius, geo_posE - geo_posS);
1852 ret.push_back(
Position(startX, startY));
1856 geo_posS = geo_posE;
1858 if (geo_posE - (g.
s + g.
length) < 0.001 && geo_posE - (g.
s + g.
length) > -0.001) {
1862 ret.push_back(
Position(startX, startY));
1870 const double s = sin(g.
hdg);
1871 const double c = cos(g.
hdg);
1873 for (
double off = 0; off < g.
length + 2.; off += resolution) {
1876 double xnew = x * c - y * s;
1877 double ynew = x * s + y * c;
1878 ret.push_back(
Position(g.
x + xnew, g.
y + ynew));
1887 const double s = sin(g.
hdg);
1888 const double c = cos(g.
hdg);
1890 const double pStep = pMax / ceil(g.
length / resolution);
1892 for (
double p = 0; p <= pMax + pStep; p += pStep) {
1895 double xnew = x * c - y * s;
1896 double ynew = x * s + y * c;
1897 ret.push_back(
Position(g.
x + xnew, g.
y + ynew));
1905 double normx = 1.0f;
1906 double normy = 0.0f;
1907 double x2 = normx * cos(hdg) - normy * sin(hdg);
1908 double y2 = normx * sin(hdg) + normy * cos(hdg);
1909 normx = x2 * length;
1910 normy = y2 * length;
1911 return Position(start.
x() + normx, start.
y() + normy);
1921 if (ad_radius > 0) {
1928 normX = normX * cos(ad_hdg) + normY * sin(ad_hdg);
1929 normY = tmpX * sin(ad_hdg) + normY * cos(ad_hdg);
1932 normX = turn * normY;
1933 normY = -turn * tmpX;
1935 normX = fabs(ad_radius) * normX;
1936 normY = fabs(ad_radius) * normY;
1945 double ad_r,
double ad_length) {
1946 double rotAngle = ad_length / fabs(ad_r);
1947 double vx = *ad_x - ad_centerX;
1948 double vy = *ad_y - ad_centerY;
1958 vx = vx * cos(rotAngle) + turn * vy * sin(rotAngle);
1959 vy = -1 * turn * tmpx * sin(rotAngle) + vy * cos(rotAngle);
1960 *ad_x = vx + ad_centerX;
1961 *ad_y = vy + ad_centerY;
1977 discardedInnerWidthRight = 0;
1979 bool singleType =
true;
1980 std::vector<std::string> types;
1981 const std::vector<OpenDriveLane>& dirLanesR = lanesByDir.find(
OPENDRIVE_TAG_RIGHT)->second;
1982 for (std::vector<OpenDriveLane>::const_reverse_iterator i = dirLanesR.rbegin(); i != dirLanesR.rend(); ++i) {
1984 discardedInnerWidthRight = 0;
1985 laneMap[(*i).id] = sumoLane++;
1986 types.push_back((*i).type);
1987 if (types.front() != types.back()) {
1991 discardedInnerWidthRight += (*i).width;
1994 discardedInnerWidthLeft = 0;
1995 rightLaneNumber = sumoLane;
1996 rightType = sumoLane > 0 ? (singleType ? types.front() :
joinToString(types,
"|")) :
"";
2000 const std::vector<OpenDriveLane>& dirLanesL = lanesByDir.find(
OPENDRIVE_TAG_LEFT)->second;
2001 for (std::vector<OpenDriveLane>::const_iterator i = dirLanesL.begin(); i != dirLanesL.end(); ++i) {
2003 discardedInnerWidthLeft = 0;
2004 laneMap[(*i).id] = sumoLane++;
2005 types.push_back((*i).type);
2006 if (types.front() != types.back()) {
2010 discardedInnerWidthLeft += (*i).width;
2013 leftLaneNumber = sumoLane;
2014 leftType = sumoLane > 0 ? (singleType ? types.front() :
joinToString(types,
"|")) :
"";
2020 std::map<int, int> ret;
2021 const std::vector<OpenDriveLane>& dirLanes = lanesByDir.find(dir)->second;
2022 for (std::vector<OpenDriveLane>::const_reverse_iterator i = dirLanes.rbegin(); i != dirLanes.rend(); ++i) {
2023 std::map<int, int>::const_iterator toP = laneMap.find((*i).id);
2024 if (toP == laneMap.end()) {
2028 int to = (*toP).second;
2031 from = (*i).predecessor;
2034 std::map<int, int>::const_iterator fromP = prev.
laneMap.find(from);
2035 if (fromP != prev.
laneMap.end()) {
2036 from = (*fromP).second;
2042 if (ret.find(from) != ret.end()) {
2067 l.
speed = (*it).second.speed;
2077 l.
speed = (*it).second.speed;
2087 const std::vector<std::string>& denied)
const {
2089 if (allowed.size() > 0 && denied.size() > 0) {
2090 WRITE_WARNING(
TL(
"Will discard access settings as both denied and allowed classes have been specified."));
2091 }
else if (allowed.size() > 0) {
2093 for (
const std::string& allow : allowed) {
2094 if (allow ==
"simulator") {
2097 }
else if (allow ==
"autonomousTraffic" || allow ==
"autonomous traffic" || allow ==
"throughTraffic") {
2100 }
else if (allow ==
"pedestrian") {
2102 }
else if (allow ==
"passengerCar") {
2104 }
else if (allow ==
"bus") {
2106 }
else if (allow ==
"delivery") {
2108 }
else if (allow ==
"emergency") {
2110 }
else if (allow ==
"taxi") {
2112 }
else if (allow ==
"bicycle") {
2114 }
else if (allow ==
"motorcycle") {
2116 }
else if (allow ==
"truck" || allow ==
"trucks") {
2121 }
else if (denied.size() > 0) {
2122 for (
const std::string& deny : denied) {
2123 if (deny ==
"none") {
2126 }
else if (deny ==
"autonomousTraffic" || deny ==
"autonomous traffic" || deny ==
"throughTraffic") {
2129 }
else if (deny ==
"pedestrian") {
2131 }
else if (deny ==
"passengerCar") {
2133 }
else if (deny ==
"bus") {
2135 }
else if (deny ==
"delivery") {
2137 }
else if (deny ==
"emergency") {
2139 }
else if (deny ==
"taxi") {
2141 }
else if (deny ==
"bicycle") {
2143 }
else if (deny ==
"motorcycle") {
2145 }
else if (deny ==
"truck" || deny ==
"trucks") {
2157 std::set<double> attributeChangePositions;
2160 for (std::vector<std::pair<double, LaneAttributeChange> >::const_iterator l = (*k).attributeChanges.begin(); l != (*k).attributeChanges.end(); ++l) {
2161 attributeChangePositions.insert((*l).first);
2162 if ((*l).first == 0) {
2163 (*k).speed = (*l).second.speed;
2164 (*k).permission = (*k).computePermission(tc, (*l).second.allowed, (*l).second.denied);
2169 for (std::vector<std::pair<double, LaneAttributeChange> >::const_iterator l = (*k).attributeChanges.begin(); l != (*k).attributeChanges.end(); ++l) {
2170 attributeChangePositions.insert((*l).first);
2171 if ((*l).first == 0) {
2172 (*k).speed = (*l).second.speed;
2173 (*k).permission = (*k).computePermission(tc, (*l).second.allowed, (*l).second.denied);
2179 if (attributeChangePositions.size() == 0) {
2183 if (*attributeChangePositions.begin() > 0) {
2184 attributeChangePositions.insert(0);
2186 #ifdef DEBUG_VARIABLE_SPEED
2188 <<
" buildSpeedChanges sectionStart=" << s
2189 <<
" speedChangePositions=" <<
joinToString(speedChangePositions,
", ")
2192 for (std::set<double>::iterator i = attributeChangePositions.begin(); i != attributeChangePositions.end(); ++i) {
2193 if (i == attributeChangePositions.begin()) {
2194 newSections.push_back(*
this);
2196 newSections.push_back(buildLaneSection(tc, *i));
2200 for (
int i = 0; i != (int)newSections.size(); ++i) {
2201 for (
auto& k : newSections[i].lanesByDir) {
2202 for (
int j = 0; j != (int)k.second.size(); ++j) {
2206 l.
speed = newSections[i - 1].lanesByDir[k.first][j].speed;
2213 l.
permission = newSections[i - 1].lanesByDir[k.first][j].permission;
2214 l.
type = newSections[i - 1].lanesByDir[k.first][j].type;
2234 for (std::vector<OpenDriveSignal>::const_iterator i = signals.begin(); i != signals.end(); ++i) {
2236 if ((*i).type ==
"301" || (*i).type ==
"306") {
2239 if ((*i).type ==
"205" ) {
2260 myTypeContainer(tc),
myCurrentEdge(
"",
"",
"", -1),
myCurrentController(
"",
""),
myEdges(edges),
myOffset(0, 0),
2282 if (majorVersion == 1 && minorVersion > 4) {
2362 std::vector<double> vals;
2368 std::vector<double> vals;
2375 std::vector<double> vals;
2381 std::vector<double> vals;
2390 std::vector<double> vals;
2400 if (pRange ==
"normalized") {
2401 vals.push_back(1.0);
2402 }
else if (pRange ==
"arcLength") {
2403 vals.push_back(-1.0);
2406 vals.push_back(1.0);
2455 int orientationCode = orientation ==
"-" ? -1 : orientation ==
"+" ? 1 : 0;
2466 int orientationCode = orientation ==
"-" ? -1 : orientation ==
"+" ? 1 : 0;
2522 WRITE_ERRORF(
TL(
"In laneLink-element: incoming road '%' is not known."), c.fromEdge);
2538 l.width =
MAX2(l.width, a);
2540 #ifdef DEBUG_VARIABLE_WIDTHS
2547 <<
" type=" << l.type
2548 <<
" width=" << l.width
2554 <<
" entries=" << l.widthData.size()
2568 std::vector<std::pair<double, LaneAttributeChange> >::iterator i = std::find_if(attributeChanges.begin(), attributeChanges.end(),
same_position_finder(pos));
2569 if (i != attributeChanges.end()) {
2570 if (rule ==
"allow") {
2571 (*i).second.allowed.push_back(vClass);
2572 }
else if (rule ==
"deny") {
2573 (*i).second.denied.push_back(vClass);
2577 if (rule ==
"allow") {
2578 lac.
allowed.push_back(vClass);
2579 }
else if (rule ==
"deny") {
2580 lac.
denied.push_back(vClass);
2582 attributeChanges.push_back(std::make_pair(pos, lac));
2594 if (!unit.empty()) {
2596 if (unit ==
"km/h") {
2599 if (unit ==
"mph") {
2600 speed *= 1.609344 / 3.6;
2605 std::vector<std::pair<double, LaneAttributeChange> >::iterator i = std::find_if(attributeChanges.begin(), attributeChanges.end(),
same_position_finder(pos));
2606 if (i != attributeChanges.end()) {
2607 (*i).second.speed = speed;
2610 attributeChanges.push_back(std::make_pair(pos, lac));
2639 const std::string baseID = o.
id;
2654 for (
double x = 0; x <= length + NUMERICAL_EPS; x += dist) {
2656 const double a = x / length;
2657 o.
width = wStart * (1 - a) + wEnd * a;
2658 o.
t = tStart * (1 - a) + tEnd * a;
2686 size_t i = cdata.find(
"+proj");
2687 if (i != std::string::npos) {
2688 const std::string proj = cdata.substr(i);
2700 WRITE_ERRORF(
TL(
"Could not set projection (%). This can be ignored with --ignore-errors."), std::string(e.what()));
2704 WRITE_WARNINGF(
TL(
"geoReference format '%' currently not supported"), cdata);
2755 if (intType < 1000001 || (intType > 1000013 && intType != 1000020) || intType == 1000008) {
2762 bool foundDrivingType =
false;
2764 if (ls.
s <= s && ls.
s + ls.
length > s) {
2767 if ((minLane < 0 && l.id >= minLane && l.id <= maxLane) && l.type ==
"driving") {
2768 foundDrivingType =
true;
2773 if ((minLane > 0 && l.id >= minLane && l.id <= maxLane) && l.type ==
"driving") {
2774 foundDrivingType =
true;
2780 if (!foundDrivingType) {
2795 const std::string& elementID,
2796 const std::string& contactPoint) {
2799 if (elementType ==
"road") {
2801 }
else if (elementType ==
"junction") {
2805 if (contactPoint ==
"start") {
2807 }
else if (contactPoint ==
"end") {
2847 #ifdef DEBUG_VARIABLE_WIDTHS
2850 std::cout <<
"sanitizeWidths e=" << e->
id <<
" sections=" << e->
laneSections.size() <<
"\n";
2868 if (l.widthData.size() > 0) {
2869 auto& wd = l.widthData;
2870 const double threshold = POSITION_EPS;
2871 double maxNoShort = -std::numeric_limits<double>::max();
2873 for (
int i = 0; i < (int)wd.size(); i++) {
2874 const double wdLength = i < (int)wd.size() - 1 ? wd[i + 1].s - wd[i].s : length - seen;
2876 if (wdLength > threshold) {
2877 maxNoShort =
MAX2(maxNoShort, wd[i].a);
2880 if (maxNoShort > 0) {
2881 l.width = maxNoShort;
2890 std::vector<OpenDriveLaneSection> newSections;
2891 #ifdef DEBUG_VARIABLE_WIDTHS
2894 std::cout <<
"splitMinWidths e=" << e->
id <<
" sections=" << e->
laneSections.size() <<
"\n";
2899 std::vector<double> splitPositions;
2900 const double sectionEnd = (j + 1) == e->
laneSections.end() ? e->
length : (*(j + 1)).s;
2901 const int section = (int)(j - e->
laneSections.begin());
2902 #ifdef DEBUG_VARIABLE_WIDTHS
2904 std::cout <<
" findWidthSplit section=" << section <<
" sectionStart=" << sec.
s <<
" sectionOrigStart=" << sec.
sOrig <<
" sectionEnd=" << sectionEnd <<
"\n";
2913 newSections.push_back(sec);
2914 std::sort(splitPositions.begin(), splitPositions.end());
2916 double prevSplit = sec.
s;
2917 for (std::vector<double>::iterator it = splitPositions.begin(); it != splitPositions.end();) {
2918 if ((*it) - prevSplit < minDist || sectionEnd - (*it) < minDist) {
2920 #ifdef DEBUG_VARIABLE_WIDTHS
2922 std::cout <<
" skip close split=" << (*it) <<
" prevSplit=" << prevSplit <<
"\n";
2925 it = splitPositions.erase(it);
2926 }
else if ((*it) < sec.
s) {
2928 #ifdef DEBUG_VARIABLE_WIDTHS
2930 std::cout <<
" skip early split=" << (*it) <<
" s=" << sec.
s <<
"\n";
2933 it = splitPositions.erase(it);
2940 if (splitPositions.size() > 0) {
2941 #ifdef DEBUG_VARIABLE_WIDTHS
2943 std::cout <<
" road=" << e->
id <<
" splitMinWidths section=" << section
2944 <<
" start=" << sec.
s
2945 <<
" origStart=" << sec.
sOrig
2946 <<
" end=" << sectionEnd <<
" minDist=" << minDist
2947 <<
" splitPositions=" <<
toString(splitPositions) <<
"\n";
2950 #ifdef DEBUG_VARIABLE_WIDTHS
2952 std::cout <<
"first section...\n";
2956 for (std::vector<double>::iterator it = splitPositions.begin(); it != splitPositions.end(); ++it) {
2959 #ifdef DEBUG_VARIABLE_WIDTHS
2961 std::cout <<
"splitAt " << secNew.
s <<
"\n";
2964 newSections.push_back(secNew);
2971 double end = (it + 1) == splitPositions.end() ? sectionEnd : *(it + 1);
2983 int section,
double sectionStart,
double sectionEnd,
2984 std::vector<double>& splitPositions) {
2986 for (std::vector<OpenDriveLane>::iterator k = lanes.begin(); k != lanes.end(); ++k) {
2991 double wPrev = l.
widthData.front().computeAt(sPrev);
2993 <<
"findWidthSplit section=" << section
2994 <<
" sectionStart=" << sectionStart
2995 <<
" sectionEnd=" << sectionEnd
2997 <<
" type=" << l.
type
2998 <<
" widthEntries=" << l.
widthData.size() <<
"\n"
3002 for (std::vector<OpenDriveWidth>::iterator it_w = l.
widthData.begin(); it_w != l.
widthData.end(); ++it_w) {
3003 double sEnd = (it_w + 1) != l.
widthData.end() ? (*(it_w + 1)).s : sectionEnd - sectionStart;
3004 double w = (*it_w).computeAt(sEnd);
3007 <<
" s=" << (*it_w).s
3008 <<
" a=" << (*it_w).a <<
" b=" << (*it_w).b <<
" c=" << (*it_w).c <<
" d=" << (*it_w).d
3011 const double changeDist = fabs(
myMinWidth - wPrev);
3014 double splitPos = sPrev + (sEnd - sPrev) / fabs(w - wPrev) * changeDist;
3015 double wSplit = (*it_w).computeAt(splitPos);
3017 std::cout <<
" candidate splitPos=" << splitPos <<
" w=" << wSplit <<
"\n";
3023 splitPos -= POSITION_EPS;
3024 if (splitPos < sPrev) {
3026 std::cout <<
" aborting search splitPos=" << splitPos <<
" wSplit=" << wSplit <<
" sPrev=" << sPrev <<
" wPrev=" << wPrev <<
"\n";
3033 splitPos += POSITION_EPS;
3034 if (splitPos > sEnd) {
3036 std::cout <<
" aborting search splitPos=" << splitPos <<
" wSplit=" << wSplit <<
" sEnd=" << sEnd <<
" w=" << w <<
"\n";
3042 wSplit = (*it_w).computeAt(splitPos);
3044 std::cout <<
" refined splitPos=" << splitPos <<
" w=" << wSplit <<
"\n";
3047 splitPositions.push_back(sectionStart + splitPos);
3065 for (std::vector<OpenDriveLane>::iterator k = lanes.begin(); k != lanes.end(); ++k) {
3066 (*k).predecessor = (*k).id;
3084 for (std::vector<OpenDriveLane>::iterator k = lanes.begin(); k != lanes.end(); ++k) {
3087 #ifdef DEBUG_VARIABLE_WIDTHS
3089 <<
"recomputeWidths lane=" << l.
id
3090 <<
" type=" << l.
type
3091 <<
" start=" << start
3093 <<
" sectionStart=" << sectionStart
3094 <<
" sectionEnd=" << sectionEnd
3095 <<
" widthEntries=" << l.
widthData.size() <<
"\n"
3100 double sPrevAbs = sPrev + sectionStart;
3101 for (std::vector<OpenDriveWidth>::iterator it_w = l.
widthData.begin(); it_w != l.
widthData.end(); ++it_w) {
3102 double sEnd = (it_w + 1) != l.
widthData.end() ? (*(it_w + 1)).s : sectionEnd - sectionStart;
3103 double sEndAbs = sEnd + sectionStart;
3104 #ifdef DEBUG_VARIABLE_WIDTHS
3106 <<
" sPrev=" << sPrev <<
" sPrevAbs=" << sPrevAbs
3107 <<
" sEnd=" << sEnd <<
" sEndAbs=" << sEndAbs
3108 <<
" widthData s=" << (*it_w).s
3109 <<
" a=" << (*it_w).a
3110 <<
" b=" << (*it_w).b
3111 <<
" c=" << (*it_w).c
3112 <<
" d=" << (*it_w).d
3115 if (sPrevAbs <= start && sEndAbs >= start) {
3116 #ifdef DEBUG_VARIABLE_WIDTHS
3118 std::cout <<
" atStart=" << start <<
" pos=" << start - sectionStart <<
" w=" << (*it_w).computeAt(start - sectionStart) <<
"\n";
3121 l.
width =
MAX2(l.
width, (*it_w).computeAt(start - sectionStart));
3123 if (sPrevAbs <= end && sEndAbs >= end) {
3124 #ifdef DEBUG_VARIABLE_WIDTHS
3126 std::cout <<
" atEnd=" << end <<
" pos=" << end - sectionStart <<
" w=" << (*it_w).computeAt(end - sectionStart) <<
"\n";
3131 if (start <= sPrevAbs && end >= sPrevAbs) {
3132 #ifdef DEBUG_VARIABLE_WIDTHS
3134 std::cout <<
" atSPrev=" << sPrev <<
" w=" << (*it_w).computeAt(sPrev) <<
"\n";
3139 if (start <= sEndAbs && end >= sEndAbs) {
3140 #ifdef DEBUG_VARIABLE_WIDTHS
3142 std::cout <<
" atSEnd=" << sEnd <<
" w=" << (*it_w).computeAt(sEnd) <<
"\n";
3147 #ifdef DEBUG_VARIABLE_WIDTHS
3149 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)
#define DEBUG_COND2(edgeID)
long long int SVCPermissions
bitset where each bit declares whether a certain SVC may use this edge/lane
@ 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
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.
static bool isAbsolute(const std::string &path)
Returns the information whether the given path is absolute.
static std::string getConfigurationRelative(const std::string &configPath, const std::string &path)
Returns the second path as a relative path to the first file.
A handler which converts occurring 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.
const std::string & getFileName() const
returns 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 GeoConvHelper & getLoaded()
the coordinate transformation that was loaded fron an input file
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 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.
static const double UNSPECIFIED_FRICTION
unspecified lane friction
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
NBNode * getToNode() const
Returns the destination node of the edge.
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.
static const double UNSPECIFIED_WIDTH
unspecified lane width
const std::vector< Connection > & getConnections() const
Returns the connections.
static const double UNSPECIFIED_OFFSET
unspecified lane offset
Lane & getLaneStruct(int lane)
NBNode * getFromNode() const
Returns the origin node of the edge.
Instance responsible for building networks.
static bool transformCoordinates(PositionVector &from, bool includeInBoundary=true, GeoConvHelper *from_srs=nullptr)
NBTypeCont & getTypeCont()
Returns a reference to the type container.
NBEdgeCont & getEdgeCont()
NBNodeCont & getNodeCont()
Returns a reference to the node 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)
std::map< std::string, OpenDriveSignal > & getSignals()
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 SequentialStringBijection::Entry openDriveAttrs[]
The names of openDrive-XML attributes (for passing to GenericSAXHandler)
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
const OpenDriveController & getController(std::string signalID)
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;.
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
static bool myIgnoreMisplacedSignals
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)
Poly3 OpenDriveElevation
LaneOffset has the same fields as Elevation.
std::map< std::string, OpenDriveController > & getControllers()
ContactPoint myCurrentContactPoint
static void findWidthSplit(const NBTypeCont &tc, std::vector< OpenDriveLane > &lanes, int section, double sectionStart, double sectionEnd, std::vector< double > &splitPositions)
static SequentialStringBijection::Entry openDriveTags[]
The names of openDrive-XML elements (for passing to GenericSAXHandler)
static PositionVector geomFromArc(const OpenDriveEdge &e, const OpenDriveGeometry &g, double resolution)
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 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.
C++ TraCI client API implementation.
virtual void setParameter(const std::string &key, const std::string &value)
Sets a parameter.
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 position 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 amount
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
static StringBijection< POIIcon > POIIcons
POI icon values.
const std::string & getString(const T key) const
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)
double length
The length of this lane section.
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
int minLane
signal validity range
double computeAt(double pos) const