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)
203 const bool customLaneShapes = oc.
getBool(
"opendrive.lane-shapes");
207 std::map<std::string, OpenDriveEdge*> edges;
211 for (
const std::string& file : oc.
getStringVector(
"opendrive-files")) {
218 for (
auto& item : edges) {
220 if (signal.type ==
"") {
221 if (handler.
getSignals().count(signal.id) == 0) {
225 signal.type = ref.
type;
226 signal.name = ref.
name;
235 std::map<std::string, OpenDriveEdge*> innerEdges, outerEdges;
236 for (std::map<std::string, OpenDriveEdge*>::iterator i = edges.begin(); i != edges.end(); ++i) {
237 if ((*i).second->isInner) {
238 innerEdges[(*i).first] = (*i).second;
240 outerEdges[(*i).first] = (*i).second;
255 std::map<std::string, Boundary> posMap;
256 std::map<std::string, std::string> edge2junction;
257 std::vector<NodeSet> joinedNodeIDs;
259 for (std::map<std::string, OpenDriveEdge*>::iterator i = innerEdges.begin(); i != innerEdges.end(); ++i) {
263 if (posMap.find(e->
junction) == posMap.end()) {
269 for (std::map<std::string, Boundary>::iterator i = posMap.begin(); i != posMap.end(); ++i) {
276 for (std::map<std::string, OpenDriveEdge*>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
278 for (std::vector<OpenDriveLink>::iterator j = e->
links.begin(); j != e->
links.end(); ++j) {
293 if (edge2junction.find(l.
elementID) != edge2junction.end()) {
305 for (std::map<std::string, OpenDriveEdge*>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
307 for (std::vector<OpenDriveLink>::iterator j = e->
links.begin(); j != e->
links.end(); ++j) {
314 std::string id1 = e->
id;
319 std::string nid = id1 +
"." + id2;
342 for (std::map<std::string, OpenDriveEdge*>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
344 if (e->
to !=
nullptr && e->
from !=
nullptr) {
347 for (std::map<std::string, OpenDriveEdge*>::iterator j = innerEdges.begin(); j != innerEdges.end(); ++j) {
349 for (std::vector<OpenDriveLink>::iterator k = ie->
links.begin(); k != ie->
links.end(); ++k) {
355 std::string nid = edge2junction[ie->
id];
367 for (std::map<std::string, OpenDriveEdge*>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
369 if ((e->
from ==
nullptr || e->
to ==
nullptr) && e->
geom.size() == 0) {
372 if (e->
from ==
nullptr) {
373 const std::string nid = e->
id +
".begin";
376 if (e->
to ==
nullptr) {
377 const std::string nid = e->
id +
".end";
382 std::map<NBNode*, NBNode*> joinedNodes;
383 for (
NodeSet& joined : joinedNodeIDs) {
385 for (
NBNode* j : joined) {
386 joinedPos.
add(j->getPosition());
388 joinedPos.
mul(1. / (
double)joined.size());
390 if (!nc.
insert(joinedID, joinedPos)) {
394 for (
NBNode* j : joined) {
398 for (std::map<std::string, OpenDriveEdge*>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
400 if (joinedNodes.count(e->
from) != 0) {
404 if (joinedNodes.count(e->
to) != 0) {
406 e->
to = joinedNodes[e->
to];
418 std::map<std::pair<NBEdge*, int>,
int> laneIndexMap;
420 for (std::map<std::string, OpenDriveEdge*>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
422 if (e->
geom.size() < 2) {
426 bool lanesBuilt =
false;
452 std::cout <<
" geomWithOffset=" << geomWithOffset <<
"\n";
455 const double length2D = geomWithOffset.
length2D();
456 double cF = length2D == 0 ? 1 : e->
length / length2D;
457 NBEdge* prevRight =
nullptr;
458 NBEdge* prevLeft =
nullptr;
466 WRITE_WARNING(
"Edge '" + e->
id +
"' has to be split as it connects same junctions.")
470 const double minDist = oc.
getFloat(
"opendrive.curve-resolution");
475 int sectionIndex = 0;
482 double nextS = (j + 1)->s;
483 const std::string nodeID = e->
id + (positionIDs ?
"." +
toString(nextS) :
"#" +
toString(sectionIndex + 1));
491 std::string
id = e->
id;
493 if (sFrom != e->
from || sTo != e->
to) {
499 id =
id +
"#" +
toString(sectionIndex++);
501#ifdef DEBUG_VARIABLE_WIDTHS
503 std::cout <<
" id=" <<
id <<
" sB=" << sB <<
" sE=" << sE <<
" geom=" << geom <<
"\n";
508 NBEdge* currRight =
nullptr;
509 if ((*j).rightLaneNumber > 0) {
510 std::vector<double> offsets(geom.size(), 0);
511 bool useOffsets =
false;
518 rightGeom.
move2side((*j).discardedInnerWidthRight);
521 std::cout <<
" -" <<
id <<
"_geom=" << geom <<
" -" <<
id <<
"_rightGeom=" << rightGeom <<
"\n";
530 std::sort(lanes.begin(), lanes.end(),
LaneSorter());
532 std::map<int, int>::const_iterator lp = (*j).laneMap.find(odl.id);
533 if (lp != (*j).laneMap.end()) {
534 int sumoLaneIndex = lp->second;
536 laneIndexMap[std::make_pair(currRight, sumoLaneIndex)] = odl.id;
542 }
else if (customLaneShapes) {
545 if (customLaneShapes) {
556 if (prevRight !=
nullptr) {
558 for (std::map<int, int>::const_iterator k = connections.begin(); k != connections.end(); ++k) {
559#ifdef DEBUG_CONNECTIONS
561 std::cout <<
"addCon1 from=" << prevRight->
getID() <<
"_" << (*k).first <<
" to=" << currRight->
getID() <<
"_" << (*k).second <<
"\n";
567 prevRight = currRight;
572 NBEdge* currLeft =
nullptr;
573 if ((*j).leftLaneNumber > 0) {
574 std::vector<double> offsets(geom.size(), 0);
575 bool useOffsets =
false;
577 leftGeom.
move2side(-(*j).discardedInnerWidthLeft);
581 std::cout <<
" " <<
id <<
"_geom=" << geom <<
" " <<
id <<
"_leftGeom=" << leftGeom <<
"\n";
588 std::sort(lanes.begin(), lanes.end(),
LaneSorter());
589 for (std::vector<OpenDriveLane>::const_iterator k = lanes.begin(); k != lanes.end(); ++k) {
590 std::map<int, int>::const_iterator lp = (*j).laneMap.find((*k).id);
591 if (lp != (*j).laneMap.end()) {
592 int sumoLaneIndex = lp->second;
594 laneIndexMap[std::make_pair(currLeft, sumoLaneIndex)] = (*k).id;
600 }
else if (customLaneShapes) {
603 if (customLaneShapes) {
614 if (prevLeft !=
nullptr) {
615 std::map<int, int> connections = (*j).getInnerConnections(
OPENDRIVE_TAG_LEFT, *(j - 1));
616 for (std::map<int, int>::const_iterator k = connections.begin(); k != connections.end(); ++k) {
617#ifdef DEBUG_CONNECTIONS
619 std::cout <<
"addCon2 from=" << currLeft->
getID() <<
"_" << (*k).first <<
" to=" << prevLeft->
getID() <<
"_" << (*k).second <<
"\n";
634 if (oc.
isSet(
"polygon-output")) {
641 if (oc.
isSet(
"polygon-output")) {
642 for (
auto item : innerEdges) {
651 for (std::map<std::string, OpenDriveEdge*>::iterator i = edges.begin(); i != edges.end(); ++i) {
655 std::vector<Connection> connections2;
656 for (std::map<std::string, OpenDriveEdge*>::iterator j = edges.begin(); j != edges.end(); ++j) {
657 const std::set<Connection>& conns = (*j).second->connections;
659 for (std::set<Connection>::const_iterator i = conns.begin(); i != conns.end(); ++i) {
660 if (innerEdges.find((*i).fromEdge) != innerEdges.end()) {
664 if (innerEdges.find((*i).toEdge) != innerEdges.end()) {
665 std::set<Connection> seen;
668 connections2.push_back(*i);
673 for (std::vector<Connection>::const_iterator i = connections2.begin(); i != connections2.end(); ++i) {
674#ifdef DEBUG_CONNECTIONS
675 std::cout <<
"connections2 " << (*i).getDescription() <<
"\n";
677 std::string fromEdge = (*i).fromEdge;
678 if (edges.find(fromEdge) == edges.end()) {
679 WRITE_WARNINGF(
TL(
"While setting connections: from-edge '%' is not known."), fromEdge);
683 int fromLane = (*i).fromLane;
687 std::string toEdge = (*i).toEdge;
688 if (edges.find(toEdge) == edges.end()) {
689 WRITE_WARNINGF(
TL(
"While setting connections: to-edge '%' is not known."), toEdge);
694 int toLane = (*i).toLane;
714 if (from ==
nullptr) {
715 WRITE_WARNINGF(
TL(
"Could not find fromEdge representation of '%' in connection '%'."), fromEdge, (*i).origID);
718 WRITE_WARNINGF(
TL(
"Could not find fromEdge representation of '%' in connection '%'."), toEdge, (*i).origID);
720 if (from ==
nullptr || to ==
nullptr) {
724#ifdef DEBUG_CONNECTIONS
726 std::cout <<
"addCon3 from=" << from->
getID() <<
"_" << fromLane <<
" to=" << to->
getID() <<
"_" << toLane <<
"\n";
738 if ((*i).origID !=
"" && saveOrigIDs) {
741 for (std::vector<NBEdge::Connection>::iterator k = cons.begin(); k != cons.end(); ++k) {
742 if ((*k).fromLane == fromLane && (*k).toEdge == to && (*k).toLane == toLane) {
754 std::map<std::string, std::string> signal2junction;
755 std::map<std::string, OpenDriveController>& controllers = handler.
getControllers();
757 for (
const auto& it : edges) {
760 if (signal.controller.size() == 0) {
763 std::string junctionID;
765 if ((connection.fromLane < 0 && signal.orientation < 0) || (connection.fromLane > 0 && signal.orientation > 0)) {
768 if ((signal.minLane == 0 && signal.maxLane == 0) || (signal.maxLane >= connection.fromLane && signal.minLane <= connection.fromLane)) {
769 const OpenDriveEdge* connectedEdge = edges[connection.toEdge];
770 if (controllers[signal.controller].junction.size() > 0 && controllers[signal.controller].junction != connectedEdge->
junction) {
771 WRITE_WARNINGF(
TL(
"Controlling multiple junctions by the same controller '%' is currently not implemented."), signal.controller);
773 controllers[signal.controller].junction = connectedEdge->
junction;
779 const bool importSignalGroups = oc.
getBool(
"opendrive.signal-groups");
780 for (std::map<std::string, OpenDriveEdge*>::iterator i = edges.begin(); i != edges.end(); ++i) {
787 if (intType < 1000001 || (intType > 1000013 && intType != 1000020) || intType == 1000008) {
795 std::vector<OpenDriveLaneSection>::iterator k = e->
laneSections.begin();
798 if (signal.s > (*k).s && signal.s <= (*(k + 1)).s) {
805 std::string
id = (*k).sumoID;
810 std::string fromID, toID;
811 for (std::vector<OpenDriveLink>::const_iterator l = e->
links.begin(); l != e->
links.end(); ++l) {
819 if (signal.orientation < 0) {
820 fromID =
"-" + fromID;
824 if (signal.orientation > 0) {
825 fromID =
"-" + fromID;
843 if (from ==
nullptr) {
844 WRITE_WARNINGF(
TL(
"Could not find edge '%' for signal '%'."), fromID, signal.id);
849 if (signal.maxLane != 0) {
850 bool fromForward = from->
getID()[0] ==
'-';
851 bool lanesForward = signal.maxLane < 0;
852 if (fromForward != lanesForward) {
856 from = signalFromTo.first;
857 to = signalFromTo.second;
858 if (from ==
nullptr) {
859 WRITE_WARNINGF(
TL(
"Could not find edge '%' for signal '%'."), fromID, signal.id);
865 if (c.toEdge == to) {
866 int odLane = laneIndexMap[std::make_pair(from, c.fromLane)];
868 if (signal.minLane == 0 || (signal.minLane <= odLane && signal.maxLane >= odLane)) {
869 if (c.hasParameter(
"signalID")) {
870 c.setParameter(
"signalID", c.getParameter(
"signalID") +
" " + signal.id);
872 c.setParameter(
"signalID", signal.id);
876 if (importSignalGroups) {
878 if (controller.
id !=
"") {
879 if (c.getParameter(
"controllerID") !=
"") {
880 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);
884 c.tlLinkIndex = tlIndex;
885 c.setParameter(
"controllerID", controller.
id);
895 WRITE_WARNINGF(
TL(
"Found a traffic light signal on an unknown edge (original edge id='%')."), e->
id);
900 if (signal.orientation == 1) {
905 if (edge ==
nullptr) {
906 WRITE_WARNINGF(
TL(
"Could not find edge '%' for signal '%'."),
id, signal.id);
912 int odLane = laneIndexMap[std::make_pair(edge, c.fromLane)];
913 if (signal.minLane == 0 || (signal.minLane <= odLane && signal.maxLane >= odLane)) {
914 if (c.hasParameter(
"signalID")) {
915 c.setParameter(
"signalID", c.getParameter(
"signalID") +
" " + signal.id);
917 c.setParameter(
"signalID", signal.id);
922 if (importSignalGroups) {
924 if (controller.
id !=
"") {
925 if (c.getParameter(
"controllerID") !=
"") {
926 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);
930 c.tlLinkIndex = tlIndex;
931 c.setParameter(
"controllerID", controller.
id);
947 for (std::map<std::string, OpenDriveEdge*>::iterator i = edges.begin(); i != edges.end(); ++i) {
969 POI.setParameter(
"name", o.name);
970 POI.writeXML(dev, writeGeo);
974 centerLine.push_back(
Position(-o.length / 2, 0));
975 centerLine.push_back(
Position(o.length / 2, 0));
977 centerLine.
rotate2D(roadHdg + o.hdg);
1001std::pair<NBEdge*, NBEdge*>
1013 return std::make_pair(from, to);
1051 if (sumoLane.
width >= 0 && widthResolution > 0) {
1052 sumoLane.
width = floor(sumoLane.
width / widthResolution + 0.5) * widthResolution;
1054 sumoLane.
width -= widthResolution;
1055 if (sumoLane.
width <= 0) {
1058 }
else if (sumoLane.
width == 0) {
1060 sumoLane.
width = widthResolution;
1066 if (forbiddenNarrow) {
1074 const std::map<std::string, OpenDriveEdge*>& innerEdges,
1075 const std::map<std::string, OpenDriveEdge*>& edges,
1077 std::vector<Connection>& into, std::set<Connection>& seen) {
1080#ifdef DEBUG_CONNECTIONS
1082 std::cout <<
" buildConnectionsToOuter " << c.
getDescription() <<
"\n";
1083 std::cout <<
" dest=" << (dest ==
nullptr ?
"NULL" : dest->
id) <<
" seenlist=";
1084 for (std::set<Connection>::const_iterator i = seen.begin(); i != seen.end(); ++i) {
1085 std::cout <<
" " << (*i).fromEdge <<
"," << (*i).toEdge <<
" ";
1090 if (dest ==
nullptr) {
1096 auto innerEdgesIt = innerEdges.find(destCon.toEdge);
1097#ifdef DEBUG_CONNECTIONS
1099 std::cout <<
" toInner=" << (innerEdgesIt != innerEdges.end()) <<
" destCon " << destCon.getDescription() <<
"\n";
1102 if (innerEdgesIt != innerEdges.end()) {
1103 std::vector<Connection> t;
1104 if (seen.count(destCon) == 0) {
1106 for (std::vector<Connection>::const_iterator j = t.begin(); j != t.end(); ++j) {
1114 cn.shape = innerEdgesIt->second->geom + c.
shape;
1123 int out = destCon.fromLane;
1128#ifdef DEBUG_CONNECTIONS
1130 std::cout <<
" laneSectionsConnected dest=" << dest->
id <<
" in=" << in <<
" out=" << out
1147 int referenceLane = 0;
1148 int offsetFactor = 1;
1152 for (
const auto& destLane : dest->
laneSections.front().lanesByDir[lanesDir]) {
1153 if (destLane.successor == c.
fromLane) {
1154 referenceLane = destLane.id;
1160 for (
const auto& destLane : dest->
laneSections.front().lanesByDir[lanesDir]) {
1161 if (destLane.predecessor == c.
fromLane) {
1162 referenceLane = destLane.id;
1171 std::vector<double> offsets(dest->
geom.size(), 0);
1175#ifdef DEBUG_INTERNALSHAPES
1176 std::string destPred;
1180 for (
int laneSectionIndex = 0; laneSectionIndex < (int)dest->
laneSections.size(); laneSectionIndex++) {
1182 const double nextS = laneSectionIndex + 1 < (int)dest->
laneSections.size() ? dest->
laneSections[laneSectionIndex + 1].s : std::numeric_limits<double>::max();
1185 int finalI = iShape;
1188 double sectionS = 0;
1191#ifdef DEBUG_INTERNALSHAPES
1192 destPred +=
" lane=" +
toString(destLane.id)
1193 +
" pred=" +
toString(destLane.predecessor)
1194 +
" succ=" +
toString(destLane.successor)
1195 +
" wStart=" + (destLane.widthData.empty() ?
"?" :
toString(destLane.widthData.front().computeAt(0)))
1196 +
" wEnd=" + (destLane.widthData.empty() ?
"?" :
toString(destLane.widthData.front().computeAt(
cn.shape.length2D())))
1197 +
" width=" +
toString(destLane.width) +
"\n";
1199 if (abs(destLane.id) <= abs(referenceLane)) {
1200 const double multiplier = offsetFactor * (destLane.id == referenceLane ? 0.5 : 1);
1201#ifdef DEBUG_INTERNALSHAPES
1202 destPred +=
" multiplier=" +
toString(multiplier) +
"\n";
1204 int widthDataIndex = 0;
1205 while (s < nextS && i < (
int)
cn.shape.size()) {
1207 const double dist =
cn.shape[i - 1].distanceTo2D(
cn.shape[i]);
1212 while (widthDataIndex + 1 < (
int)destLane.widthData.size()
1213 && sectionS >= destLane.widthData[widthDataIndex + 1].s) {
1217 if (destLane.widthData.size() > 0) {
1218 width = destLane.widthData[widthDataIndex].computeAt(sectionS);
1220#ifdef DEBUG_INTERNALSHAPES
1221 std::cout <<
" missing width data at inner edge " << dest->
id <<
" to=" <<
cn.toEdge <<
"_" <<
cn.toLane <<
" cp=" <<
cn.toCP <<
"\n";
1228 if (outerToLane.id ==
cn.toLane && outerToLane.width > 0) {
1229#ifdef DEBUG_INTERNALSHAPES
1230 std::cout <<
" using toLane width " << width <<
"\n";
1236 offsets[i] += width * multiplier;
1244 }
else if (finalS == s) {
1246 while (s < nextS && i < (
int)
cn.shape.size()) {
1248 const double dist =
cn.shape[i - 1].distanceTo2D(
cn.shape[i]);
1264 cn.shape.move2sideCustom(offsets);
1269#ifdef DEBUG_INTERNALSHAPES
1270 std::cout <<
"internalShape "
1272 <<
" dest=" << dest->
id
1273 <<
" refLane=" << referenceLane
1274 <<
" destPred\n" << destPred
1275 <<
" offsets=" << offsets
1276 <<
"\n shape=" << dest->
geom
1277 <<
"\n shape2=" <<
cn.shape
1284#ifdef DEBUG_CONNECTIONS
1286 std::cout <<
" added connection\n";
1306 if (lane.id == in) {
1307 in = lane.successor;
1314 if (lane.id == in) {
1315 in = lane.successor;
1328 for (std::vector<OpenDriveLink>::iterator i = e.
links.begin(); i != e.
links.end(); ++i) {
1335 std::string connectedEdge = l.
elementID;
1336 std::string edgeID = e.
id;
1339 const std::map<int, int>& laneMap = laneSection.
laneMap;
1340#ifdef DEBUG_CONNECTIONS
1342 std::cout <<
"edge=" << e.
id <<
" eType=" << l.
elementType <<
" lType=" << l.
linkType <<
" connectedEdge=" << connectedEdge <<
" laneSection=" << laneSection.
s <<
" map:\n";
1348 for (std::vector<OpenDriveLane>::const_iterator j = lanes.begin(); j != lanes.end(); ++j) {
1357 c.
toEdge = connectedEdge;
1365 if (edges.find(c.
fromEdge) == edges.end()) {
1370#ifdef DEBUG_CONNECTIONS
1372 std::cout <<
"insertConRight from=" << src->
id <<
"_" << c.
fromLane <<
" to=" << c.
toEdge <<
"_" << c.
toLane <<
"\n";
1380 for (std::vector<OpenDriveLane>::const_iterator j = lanes.begin(); j != lanes.end(); ++j) {
1397 if (edges.find(c.
fromEdge) == edges.end()) {
1402#ifdef DEBUG_CONNECTIONS
1404 std::cout <<
"insertConLeft from=" << src->
id <<
"_" << c.
fromLane <<
" to=" << c.
toEdge <<
"_" << c.
toLane <<
"\n";
1416 return id.substr(1);
1427 if (!nc.
insert(
id, pos)) {
1443 NBNode* toJoin =
nullptr;
1445 if (e.
to !=
nullptr && e.
to != n) {
1450 if (e.
from !=
nullptr && e.
from != n) {
1455 if (toJoin !=
nullptr) {
1459 for (
NodeSet& joined : joinedNodeIDs) {
1460 if (joined.count(toJoin) != 0) {
1463 if (joined.count(n) != 0) {
1467 if (set1 ==
nullptr && set2 ==
nullptr) {
1468 joinedNodeIDs.push_back(
NodeSet());
1469 joinedNodeIDs.back().insert(n);
1470 joinedNodeIDs.back().insert(toJoin);
1471 }
else if (set1 ==
nullptr && set2 !=
nullptr) {
1472 set2->insert(toJoin);
1473 }
else if (set1 !=
nullptr && set2 ==
nullptr) {
1476 set1->insert(set2->begin(), set2->end());
1477 joinedNodeIDs.erase(std::find(joinedNodeIDs.begin(), joinedNodeIDs.end(), *set2));
1488 if (el.c != 0 || el.d != 0) {
1498 const double res = oc.
getFloat(
"opendrive.curve-resolution");
1499 for (
const auto& i : edges) {
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) {
1545 if (e.
geom.size() == 1 && e.
geom.front() != last) {
1547 e.
geom.push_back(last);
1551 std::cout <<
" initialGeom=" << e.
geom <<
"\n";
1554 if (oc.
exists(
"geometry.min-dist") && !oc.
isDefault(
"geometry.min-dist")) {
1557 if (e.
geom.size() > 4) {
1564 std::cout <<
" reducedGeom=" << e.
geom <<
"\n";
1575 for (std::vector<OpenDriveElevation>::iterator j = e.
elevations.begin(); j != e.
elevations.end(); ++j) {
1577 const double sNext = (j + 1) == e.
elevations.end() ? std::numeric_limits<double>::max() : (*(j + 1)).s;
1578 while (k < (
int)e.
geom.size() && pos < sNext) {
1583 if (k < (
int)e.
geom.size()) {
1586 pos += e.
geom[k - 1].distanceTo2D(e.
geom[k]);
1603 std::vector<double> laneOffsets;
1619 for (
auto j = offsets.begin(); j != offsets.end(); ++j) {
1621 const double sNext = (j + 1) == offsets.end() ? std::numeric_limits<double>::max() : (*(j + 1)).s;
1622 while (kk < (
int)geom.size() && ppos < sNext) {
1623 const double offset = el.
computeAt(ppos);
1624 laneOffsets.push_back(fabs(offset) > POSITION_EPS ? -offset : 0);
1626 if (kk < (
int)geom.size()) {
1629 ppos += geom[kk - 1].distanceTo2D(geom[kk]);
1649 double interpolatedOffset = 0;
1651 interpolatedOffset = result.front();
1652 }
else if (at == (
int)geom.size() - 1) {
1653 interpolatedOffset = result.back();
1655 interpolatedOffset = (result[at - 1] + result[at]) / 2;
1657 result.insert(result.begin() + at, interpolatedOffset);
1663 const int sign = left ? -1 : 1;
1664 for (
auto j = offsets.begin(); j != offsets.end(); ++j) {
1666 const double sNext = (j + 1) == offsets.end() ? std::numeric_limits<double>::max() : (*(j + 1)).s;
1667 while (kk < (
int)geom.size() && ppos < sNext) {
1668 const double offset = el.
computeAt(ppos);
1669 result[kk] += fabs(offset) > POSITION_EPS ? sign * offset : 0;
1671 if (kk < (
int)geom.size()) {
1674 ppos += geom[kk - 1].distanceTo2D(geom[kk]);
1683 for (
const auto& i : edges) {
1685#ifdef DEBUG_VARIABLE_SPEED
1688 std::cout <<
"revisitLaneSections e=" << e.
id <<
"\n";
1692 std::vector<OpenDriveLaneSection> newSections;
1694 std::vector<OpenDriveLaneSection> splitSections;
1695 const bool splitByAttrChange = section.buildAttributeChanges(tc, splitSections);
1696 if (!splitByAttrChange) {
1697 newSections.push_back(section);
1699 std::copy(splitSections.begin(), splitSections.end(), back_inserter(newSections));
1708 if (section.s <= lastS) {
1724 if ((j + 1)->s - j->s < POSITION_EPS) {
1725 WRITE_WARNINGF(
TL(
"Almost duplicate s-value '%' for lane sections occurred at edge '%'; first entry was removed."),
toString(j->s), e.
id);
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") {
2130 perms &= ~SVC_PEDESTRIAN;
2131 }
else if (deny ==
"passengerCar") {
2132 perms &= ~SVC_PASSENGER;
2133 }
else if (deny ==
"bus") {
2135 }
else if (deny ==
"delivery") {
2136 perms &= ~SVC_DELIVERY;
2137 }
else if (deny ==
"emergency") {
2138 perms &= ~SVC_EMERGENCY;
2139 }
else if (deny ==
"taxi") {
2141 }
else if (deny ==
"bicycle") {
2142 perms &= ~SVC_BICYCLE;
2143 }
else if (deny ==
"motorcycle") {
2144 perms &= ~SVC_MOTORCYCLE;
2145 }
else if (deny ==
"truck" || deny ==
"trucks") {
2146 perms &= ~SVC_TRUCK;
2147 perms &= ~SVC_TRAILER;
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) {
2363 std::vector<double> vals;
2369 std::vector<double> vals;
2376 std::vector<double> vals;
2382 std::vector<double> vals;
2391 std::vector<double> vals;
2401 if (pRange ==
"normalized") {
2402 vals.push_back(1.0);
2403 }
else if (pRange ==
"arcLength") {
2404 vals.push_back(-1.0);
2407 vals.push_back(1.0);
2456 int orientationCode = orientation ==
"-" ? -1 : orientation ==
"+" ? 1 : 0;
2467 int orientationCode = orientation ==
"-" ? -1 : orientation ==
"+" ? 1 : 0;
2523 WRITE_ERRORF(
TL(
"In laneLink-element: incoming road '%' is not known."), c.fromEdge);
2539 l.width =
MAX2(l.width, a);
2541#ifdef DEBUG_VARIABLE_WIDTHS
2548 <<
" type=" << l.type
2549 <<
" width=" << l.width
2555 <<
" entries=" << l.widthData.size()
2569 std::vector<std::pair<double, LaneAttributeChange> >::iterator i = std::find_if(attributeChanges.begin(), attributeChanges.end(),
same_position_finder(pos));
2570 if (i != attributeChanges.end()) {
2571 if (rule ==
"allow") {
2572 (*i).second.allowed.push_back(vClass);
2573 }
else if (rule ==
"deny") {
2574 (*i).second.denied.push_back(vClass);
2578 if (rule ==
"allow") {
2579 lac.
allowed.push_back(vClass);
2580 }
else if (rule ==
"deny") {
2581 lac.
denied.push_back(vClass);
2583 attributeChanges.push_back(std::make_pair(pos, lac));
2595 if (!unit.empty()) {
2597 if (unit ==
"km/h") {
2600 if (unit ==
"mph") {
2601 speed *= 1.609344 / 3.6;
2606 std::vector<std::pair<double, LaneAttributeChange> >::iterator i = std::find_if(attributeChanges.begin(), attributeChanges.end(),
same_position_finder(pos));
2607 if (i != attributeChanges.end()) {
2608 (*i).second.speed = speed;
2611 attributeChanges.push_back(std::make_pair(pos, lac));
2640 const std::string baseID = o.
id;
2655 for (
double x = 0; x <= length + NUMERICAL_EPS; x += dist) {
2657 const double a = x / length;
2658 o.
width = wStart * (1 - a) + wEnd * a;
2659 o.
t = tStart * (1 - a) + tEnd * a;
2687 size_t i = cdata.find(
"+proj");
2688 if (i != std::string::npos) {
2689 const std::string proj = cdata.substr(i);
2699 WRITE_ERRORF(
TL(
"Could not set projection (%). This can be ignored with --ignore-errors."), std::string(e.what()));
2703 WRITE_WARNINGF(
TL(
"geoReference format '%' currently not supported"), cdata);
2754 if (intType < 1000001 || (intType > 1000013 && intType != 1000020) || intType == 1000008) {
2761 bool foundDrivingType =
false;
2763 if (ls.s <= s && ls.s + ls.length > s) {
2766 if ((minLane < 0 && l.id >= minLane && l.id <= maxLane) && l.type ==
"driving") {
2767 foundDrivingType =
true;
2772 if ((minLane > 0 && l.id >= minLane && l.id <= maxLane) && l.type ==
"driving") {
2773 foundDrivingType =
true;
2779 if (!foundDrivingType) {
2794 const std::string& elementID,
2795 const std::string& contactPoint) {
2798 if (elementType ==
"road") {
2800 }
else if (elementType ==
"junction") {
2804 if (contactPoint ==
"start") {
2806 }
else if (contactPoint ==
"end") {
2846#ifdef DEBUG_VARIABLE_WIDTHS
2849 std::cout <<
"sanitizeWidths e=" << e->
id <<
" sections=" << e->
laneSections.size() <<
"\n";
2855 if (sec.rightLaneNumber > 0) {
2858 if (sec.leftLaneNumber > 0) {
2867 if (l.widthData.size() > 0) {
2868 auto& wd = l.widthData;
2869 const double threshold = POSITION_EPS;
2870 double maxNoShort = -std::numeric_limits<double>::max();
2872 for (
int i = 0; i < (int)wd.size(); i++) {
2873 const double wdLength = i < (int)wd.size() - 1 ? wd[i + 1].s - wd[i].s : length - seen;
2875 if (wdLength > threshold) {
2876 maxNoShort =
MAX2(maxNoShort, wd[i].a);
2879 if (maxNoShort > 0) {
2880 l.width = maxNoShort;
2889 std::vector<OpenDriveLaneSection> newSections;
2890#ifdef DEBUG_VARIABLE_WIDTHS
2893 std::cout <<
"splitMinWidths e=" << e->
id <<
" sections=" << e->
laneSections.size() <<
"\n";
2898 std::vector<double> splitPositions;
2899 const double sectionEnd = (j + 1) == e->
laneSections.end() ? e->
length : (*(j + 1)).s;
2900 const int section = (int)(j - e->
laneSections.begin());
2901#ifdef DEBUG_VARIABLE_WIDTHS
2903 std::cout <<
" findWidthSplit section=" << section <<
" sectionStart=" << sec.
s <<
" sectionOrigStart=" << sec.
sOrig <<
" sectionEnd=" << sectionEnd <<
"\n";
2912 newSections.push_back(sec);
2913 std::sort(splitPositions.begin(), splitPositions.end());
2915 double prevSplit = sec.
s;
2916 for (std::vector<double>::iterator it = splitPositions.begin(); it != splitPositions.end();) {
2917 if ((*it) - prevSplit < minDist || sectionEnd - (*it) < minDist) {
2919#ifdef DEBUG_VARIABLE_WIDTHS
2921 std::cout <<
" skip close split=" << (*it) <<
" prevSplit=" << prevSplit <<
"\n";
2924 it = splitPositions.erase(it);
2925 }
else if ((*it) < sec.
s) {
2927#ifdef DEBUG_VARIABLE_WIDTHS
2929 std::cout <<
" skip early split=" << (*it) <<
" s=" << sec.
s <<
"\n";
2932 it = splitPositions.erase(it);
2939 if (splitPositions.size() > 0) {
2940#ifdef DEBUG_VARIABLE_WIDTHS
2942 std::cout <<
" road=" << e->
id <<
" splitMinWidths section=" << section
2943 <<
" start=" << sec.
s
2944 <<
" origStart=" << sec.
sOrig
2945 <<
" end=" << sectionEnd <<
" minDist=" << minDist
2946 <<
" splitPositions=" <<
toString(splitPositions) <<
"\n";
2949#ifdef DEBUG_VARIABLE_WIDTHS
2951 std::cout <<
"first section...\n";
2955 for (std::vector<double>::iterator it = splitPositions.begin(); it != splitPositions.end(); ++it) {
2958#ifdef DEBUG_VARIABLE_WIDTHS
2960 std::cout <<
"splitAt " << secNew.
s <<
"\n";
2963 newSections.push_back(secNew);
2970 double end = (it + 1) == splitPositions.end() ? sectionEnd : *(it + 1);
2982 int section,
double sectionStart,
double sectionEnd,
2983 std::vector<double>& splitPositions) {
2988 double sPrev = l.widthData.front().s;
2989 double wPrev = l.widthData.front().computeAt(sPrev);
2990#ifdef DEBUG_VARIABLE_WIDTHS
2992 <<
"findWidthSplit section=" << section
2993 <<
" sectionStart=" << sectionStart
2994 <<
" sectionEnd=" << sectionEnd
2996 <<
" type=" << l.type
2997 <<
" widthEntries=" << l.widthData.size() <<
"\n"
3002 for (std::vector<OpenDriveWidth>::const_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);
3005#ifdef DEBUG_VARIABLE_WIDTHS
3008 <<
" s=" << (*it_w).s
3009 <<
" a=" << (*it_w).a <<
" b=" << (*it_w).b <<
" c=" << (*it_w).c <<
" d=" << (*it_w).d
3013 const double changeDist = fabs(
myMinWidth - wPrev);
3016 double splitPos = sPrev + (sEnd - sPrev) / fabs(w - wPrev) * changeDist;
3017 double wSplit = (*it_w).computeAt(splitPos);
3018#ifdef DEBUG_VARIABLE_WIDTHS
3020 std::cout <<
" candidate splitPos=" << splitPos <<
" w=" << wSplit <<
"\n";
3027 splitPos -= POSITION_EPS;
3028 if (splitPos < sPrev) {
3029#ifdef DEBUG_VARIABLE_WIDTHS
3031 std::cout <<
" aborting search splitPos=" << splitPos <<
" wSplit=" << wSplit <<
" sPrev=" << sPrev <<
" wPrev=" << wPrev <<
"\n";
3039 splitPos += POSITION_EPS;
3040 if (splitPos > sEnd) {
3041#ifdef DEBUG_VARIABLE_WIDTHS
3043 std::cout <<
" aborting search splitPos=" << splitPos <<
" wSplit=" << wSplit <<
" sEnd=" << sEnd <<
" w=" << w <<
"\n";
3050 wSplit = (*it_w).computeAt(splitPos);
3051#ifdef DEBUG_VARIABLE_WIDTHS
3053 std::cout <<
" refined splitPos=" << splitPos <<
" w=" << wSplit <<
"\n";
3057 splitPositions.push_back(sectionStart + splitPos);
3075 for (std::vector<OpenDriveLane>::iterator k = lanes.begin(); k != lanes.end(); ++k) {
3076 (*k).predecessor = (*k).id;
3094 for (std::vector<OpenDriveLane>::iterator k = lanes.begin(); k != lanes.end(); ++k) {
3097#ifdef DEBUG_VARIABLE_WIDTHS
3099 <<
"recomputeWidths lane=" << l.
id
3100 <<
" type=" << l.
type
3101 <<
" start=" << start
3103 <<
" sectionStart=" << sectionStart
3104 <<
" sectionEnd=" << sectionEnd
3105 <<
" widthEntries=" << l.
widthData.size() <<
"\n"
3110 double sPrevAbs = sPrev + sectionStart;
3111 for (std::vector<OpenDriveWidth>::iterator it_w = l.
widthData.begin(); it_w != l.
widthData.end(); ++it_w) {
3112 double sEnd = (it_w + 1) != l.
widthData.end() ? (*(it_w + 1)).s : sectionEnd - sectionStart;
3113 double sEndAbs = sEnd + sectionStart;
3114#ifdef DEBUG_VARIABLE_WIDTHS
3116 <<
" sPrev=" << sPrev <<
" sPrevAbs=" << sPrevAbs
3117 <<
" sEnd=" << sEnd <<
" sEndAbs=" << sEndAbs
3118 <<
" widthData s=" << (*it_w).s
3119 <<
" a=" << (*it_w).a
3120 <<
" b=" << (*it_w).b
3121 <<
" c=" << (*it_w).c
3122 <<
" d=" << (*it_w).d
3125 if (sPrevAbs <= start && sEndAbs >= start) {
3126#ifdef DEBUG_VARIABLE_WIDTHS
3128 std::cout <<
" atStart=" << start <<
" pos=" << start - sectionStart <<
" w=" << (*it_w).computeAt(start - sectionStart) <<
"\n";
3131 l.
width =
MAX2(l.
width, (*it_w).computeAt(start - sectionStart));
3133 if (sPrevAbs <= end && sEndAbs >= end) {
3134#ifdef DEBUG_VARIABLE_WIDTHS
3136 std::cout <<
" atEnd=" << end <<
" pos=" << end - sectionStart <<
" w=" << (*it_w).computeAt(end - sectionStart) <<
"\n";
3141 if (start <= sPrevAbs && end >= sPrevAbs) {
3142#ifdef DEBUG_VARIABLE_WIDTHS
3144 std::cout <<
" atSPrev=" << sPrev <<
" w=" << (*it_w).computeAt(sPrev) <<
"\n";
3149 if (start <= sEndAbs && end >= sEndAbs) {
3150#ifdef DEBUG_VARIABLE_WIDTHS
3152 std::cout <<
" atSEnd=" << sEnd <<
" w=" << (*it_w).computeAt(sEnd) <<
"\n";
3157#ifdef DEBUG_VARIABLE_WIDTHS
3159 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)
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 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 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
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 > & 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)
static bool hasNonLinearElevation(const OpenDriveEdge &e)
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.
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)
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
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.
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
const PositionVector simplified2(const bool closed, const double eps=NUMERICAL_EPS) const
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)
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