62 #define DEBUG_COND(road) ((road)->id == "175")
63 #define DEBUG_COND2(edgeID) (StringUtils::startsWith((edgeID), "disabled"))
64 #define DEBUG_COND3(roadID) (roadID == "175")
184 std::map<std::string, OpenDriveEdge*> edges;
187 std::vector<std::string> files = oc.
getStringVector(
"opendrive-files");
188 for (std::vector<std::string>::const_iterator file = files.begin(); file != files.end(); ++file) {
190 WRITE_ERROR(
"Could not open opendrive file '" + *file +
"'.");
193 handler.setFileName(*file);
199 std::map<std::string, OpenDriveEdge*> innerEdges, outerEdges;
200 for (std::map<std::string, OpenDriveEdge*>::iterator i = edges.begin(); i != edges.end(); ++i) {
201 if ((*i).second->isInner) {
202 innerEdges[(*i).first] = (*i).second;
204 outerEdges[(*i).first] = (*i).second;
219 std::map<std::string, Boundary> posMap;
220 std::map<std::string, std::string> edge2junction;
222 for (std::map<std::string, OpenDriveEdge*>::iterator i = innerEdges.begin(); i != innerEdges.end(); ++i) {
226 if (posMap.find(e->
junction) == posMap.end()) {
232 for (std::map<std::string, Boundary>::iterator i = posMap.begin(); i != posMap.end(); ++i) {
235 throw ProcessError(
"Could not add node '" + (*i).first +
"'.");
239 for (std::map<std::string, OpenDriveEdge*>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
241 for (std::vector<OpenDriveLink>::iterator j = e->
links.begin(); j != e->
links.end(); ++j) {
249 throw ProcessError(
"Could not build node '" + nid +
"'.");
256 if (edge2junction.find(l.
elementID) != edge2junction.end()) {
268 for (std::map<std::string, OpenDriveEdge*>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
270 for (std::vector<OpenDriveLink>::iterator j = e->
links.begin(); j != e->
links.end(); ++j) {
277 std::string id1 = e->
id;
282 std::string nid = id1 +
"." + id2;
287 throw ProcessError(
"Could not build node '" + nid +
"'.");
305 for (std::map<std::string, OpenDriveEdge*>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
307 if (e->
to !=
nullptr && e->
from !=
nullptr) {
310 for (std::map<std::string, OpenDriveEdge*>::iterator j = innerEdges.begin(); j != innerEdges.end(); ++j) {
312 for (std::vector<OpenDriveLink>::iterator k = ie->
links.begin(); k != ie->
links.end(); ++k) {
318 std::string nid = edge2junction[ie->
id];
330 for (std::map<std::string, OpenDriveEdge*>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
332 if ((e->
from ==
nullptr || e->
to ==
nullptr) && e->
geom.size() == 0) {
335 if (e->
from ==
nullptr) {
336 const std::string nid = e->
id +
".begin";
339 if (e->
to ==
nullptr) {
340 const std::string nid = e->
id +
".end";
349 const double defaultSpeed = tc.
getSpeed(
"");
352 for (std::map<std::string, OpenDriveEdge*>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
354 if (e->
geom.size() < 2) {
358 bool lanesBuilt =
false;
382 const double length2D = geomWithOffset.
length2D();
383 double cF = length2D == 0 ? 1 : e->
length / length2D;
384 NBEdge* prevRight =
nullptr;
385 NBEdge* prevLeft =
nullptr;
393 WRITE_WARNING(
"Edge '" + e->
id +
"' has to be split as it connects same junctions.")
397 const double minDist = oc.
getFloat(
"opendrive.curve-resolution");
408 double nextS = (j + 1)->s;
416 std::string
id = e->
id;
417 if (sFrom != e->
from || sTo != e->
to) {
422 #ifdef DEBUG_VARIABLE_WIDTHS
424 std::cout <<
" id=" <<
id <<
" sB=" << sB <<
" sE=" << sE <<
" geom=" << geom <<
"\n";
429 NBEdge* currRight =
nullptr;
430 if ((*j).rightLaneNumber > 0) {
431 currRight =
new NBEdge(
"-" +
id, sFrom, sTo, (*j).rightType, defaultSpeed, (*j).rightLaneNumber, priorityR,
435 for (std::vector<OpenDriveLane>::const_iterator k = lanes.begin(); k != lanes.end(); ++k) {
436 std::map<int, int>::const_iterator lp = (*j).laneMap.find((*k).id);
437 if (lp != (*j).laneMap.end()) {
438 int sumoLaneIndex = lp->second;
449 if (prevRight !=
nullptr) {
451 for (std::map<int, int>::const_iterator k = connections.begin(); k != connections.end(); ++k) {
452 #ifdef DEBUG_CONNECTIONS
454 std::cout <<
"addCon1 from=" << prevRight->
getID() <<
"_" << (*k).first <<
" to=" << currRight->
getID() <<
"_" << (*k).second <<
"\n";
460 prevRight = currRight;
465 NBEdge* currLeft =
nullptr;
466 if ((*j).leftLaneNumber > 0) {
467 currLeft =
new NBEdge(
id, sTo, sFrom, (*j).leftType, defaultSpeed, (*j).leftLaneNumber, priorityL,
471 for (std::vector<OpenDriveLane>::const_iterator k = lanes.begin(); k != lanes.end(); ++k) {
472 std::map<int, int>::const_iterator lp = (*j).laneMap.find((*k).id);
473 if (lp != (*j).laneMap.end()) {
474 int sumoLaneIndex = lp->second;
485 if (prevLeft !=
nullptr) {
486 std::map<int, int> connections = (*j).getInnerConnections(
OPENDRIVE_TAG_LEFT, *(j - 1));
487 for (std::map<int, int>::const_iterator k = connections.begin(); k != connections.end(); ++k) {
488 #ifdef DEBUG_CONNECTIONS
490 std::cout <<
"addCon2 from=" << currLeft->
getID() <<
"_" << (*k).first <<
" to=" << prevLeft->
getID() <<
"_" << (*k).second <<
"\n";
506 if (oc.
isSet(
"polygon-output")) {
525 centerLine.push_back(
Position(-o.length / 2, 0));
526 centerLine.push_back(
Position(o.length / 2, 0));
528 centerLine.
rotate2D(roadHdg + o.hdg);
540 for (
int i = 0; i < (int) shape.size(); i++) {
559 for (std::map<std::string, OpenDriveEdge*>::iterator i = edges.begin(); i != edges.end(); ++i) {
563 std::vector<Connection> connections2;
564 for (std::map<std::string, OpenDriveEdge*>::iterator j = edges.begin(); j != edges.end(); ++j) {
565 const std::set<Connection>& conns = (*j).second->connections;
567 for (std::set<Connection>::const_iterator i = conns.begin(); i != conns.end(); ++i) {
568 if (innerEdges.find((*i).fromEdge) != innerEdges.end()) {
572 if (innerEdges.find((*i).toEdge) != innerEdges.end()) {
573 std::set<Connection> seen;
576 connections2.push_back(*i);
581 for (std::vector<Connection>::const_iterator i = connections2.begin(); i != connections2.end(); ++i) {
582 #ifdef DEBUG_CONNECTIONS
583 std::cout <<
"connections2 " << (*i).getDescription() <<
"\n";
585 std::string fromEdge = (*i).fromEdge;
586 if (edges.find(fromEdge) == edges.end()) {
587 WRITE_WARNING(
"While setting connections: from-edge '" + fromEdge +
"' is not known.");
591 int fromLane = (*i).fromLane;
595 std::string toEdge = (*i).toEdge;
596 if (edges.find(toEdge) == edges.end()) {
597 WRITE_WARNING(
"While setting connections: to-edge '" + toEdge +
"' is not known.");
602 int toLane = (*i).toLane;
622 if (from ==
nullptr) {
623 WRITE_WARNING(
"Could not find fromEdge representation of '" + fromEdge +
"' in connection '" + (*i).origID +
"'.");
626 WRITE_WARNING(
"Could not find fromEdge representation of '" + toEdge +
"' in connection '" + (*i).origID +
"'.");
628 if (from ==
nullptr || to ==
nullptr) {
632 #ifdef DEBUG_CONNECTIONS
634 std::cout <<
"addCon3 from=" << from->
getID() <<
"_" << fromLane <<
" to=" << to->
getID() <<
"_" << toLane <<
"\n";
643 if ((*i).origID !=
"" && saveOrigIDs) {
646 for (std::vector<NBEdge::Connection>::iterator k = cons.begin(); k != cons.end(); ++k) {
647 if ((*k).fromLane == fromLane && (*k).toEdge == to && (*k).toLane == toLane) {
659 std::map<std::string, std::string> tlsControlled;
660 for (std::map<std::string, OpenDriveEdge*>::iterator i = edges.begin(); i != edges.end(); ++i) {
662 for (std::vector<OpenDriveSignal>::const_iterator j = e->
signals.begin(); j != e->
signals.end(); ++j) {
663 if ((*j).type !=
"1000001") {
666 std::vector<OpenDriveLaneSection>::iterator k = e->
laneSections.begin();
669 if ((*j).s > (*k).s && (*j).s <= (*(k + 1)).s) {
679 std::string
id = (*k).sumoID;
683 std::string fromID, toID;
684 for (std::vector<OpenDriveLink>::const_iterator l = e->
links.begin(); l != e->
links.end(); ++l) {
692 if ((*j).orientation < 0) {
693 fromID =
"-" + fromID;
697 if ((*j).orientation > 0) {
698 fromID =
"-" + fromID;
710 id = fromID +
"->" + toID;
712 WRITE_WARNING(
"Found a traffic light signal on an unknown edge (original edge id='" + e->
id +
"').");
716 if ((*j).orientation > 0) {
720 tlsControlled[id] = (*j).name;
724 for (std::map<std::string, std::string>::iterator i = tlsControlled.begin(); i != tlsControlled.end(); ++i) {
725 std::string
id = (*i).first;
726 if (
id.find(
"->") != std::string::npos) {
727 id =
id.substr(0,
id.find(
"->"));
731 WRITE_WARNING(
"Could not find edge '" +
id +
"' while building its traffic light.");
753 for (std::map<std::string, OpenDriveEdge*>::iterator i = edges.begin(); i != edges.end(); ++i) {
775 if (sumoLane.
width >= 0 && widthResolution > 0) {
776 sumoLane.
width = floor(sumoLane.
width / widthResolution + 0.5) * widthResolution;
778 sumoLane.
width -= widthResolution;
779 if (sumoLane.
width <= 0) {
782 }
else if (sumoLane.
width == 0) {
784 sumoLane.
width = widthResolution;
790 if (forbiddenNarrow) {
800 #ifdef DEBUG_CONNECTIONS
802 std::cout <<
" buildConnectionsToOuter " << c.
getDescription() <<
"\n";
803 std::cout <<
" dest=" << (dest ==
nullptr ?
"NULL" : dest->
id) <<
" seenlist=";
804 for (std::set<Connection>::const_iterator i = seen.begin(); i != seen.end(); ++i) {
805 std::cout <<
" " << (*i).fromEdge <<
"," << (*i).toEdge <<
" ";
810 if (dest ==
nullptr) {
815 const std::set<Connection>& conts = dest->
connections;
816 for (std::set<Connection>::const_iterator i = conts.begin(); i != conts.end(); ++i) {
817 auto innerEdgesIt = innerEdges.find((*i).toEdge);
818 #ifdef DEBUG_CONNECTIONS
820 std::cout <<
" toInner=" << (innerEdgesIt != innerEdges.end()) <<
" destCon " << (*i).getDescription() <<
"\n";
823 if (innerEdgesIt != innerEdges.end()) {
824 std::vector<Connection> t;
825 if (seen.count(*i) == 0) {
827 for (std::vector<Connection>::const_iterator j = t.begin(); j != t.end(); ++j) {
835 cn.shape = innerEdgesIt->second->geom + c.
shape;
855 int referenceLane = 0;
856 int offsetFactor = 1;
860 for (
const auto& destLane : dest->
laneSections.front().lanesByDir[lanesDir]) {
861 if (destLane.successor == c.
fromLane) {
862 referenceLane = destLane.id;
868 for (
const auto& destLane : dest->
laneSections.front().lanesByDir[lanesDir]) {
869 if (destLane.predecessor == c.
fromLane) {
870 referenceLane = destLane.id;
879 std::vector<double> offsets(dest->
geom.size(), 0);
883 #ifdef DEBUG_INTERNALSHAPES
884 std::string destPred;
888 for (
int laneSectionIndex = 0; laneSectionIndex < (int)dest->
laneSections.size(); laneSectionIndex++) {
889 auto& laneSection = dest->
laneSections[laneSectionIndex];
890 const double nextS = laneSectionIndex + 1 < (int)dest->
laneSections.size() ? dest->
laneSections[laneSectionIndex + 1].s : std::numeric_limits<double>::max();
895 for (
const OpenDriveLane& destLane : laneSection.lanesByDir[lanesDir]) {
900 #ifdef DEBUG_INTERNALSHAPES
901 destPred +=
" lane=" +
toString(destLane.id)
902 +
" pred=" +
toString(destLane.predecessor)
903 +
" succ=" +
toString(destLane.successor)
904 +
" wStart=" +
toString(destLane.widthData.front().computeAt(0))
905 +
" wEnd=" +
toString(destLane.widthData.front().computeAt(
cn.shape.length2D()))
906 +
" width=" +
toString(destLane.width) +
"\n";
908 if (abs(destLane.id) <= abs(referenceLane)) {
909 const double multiplier = offsetFactor * (destLane.id == referenceLane ? 0.5 : 1);
910 #ifdef DEBUG_INTERNALSHAPES
911 destPred +=
" multiplier=" +
toString(multiplier) +
"\n";
913 int widthDataIndex = 0;
914 while (s < nextS && i < (
int)
cn.shape.size()) {
916 const double dist =
cn.shape[i - 1].distanceTo2D(
cn.shape[i]);
921 while (widthDataIndex + 1 < (
int)destLane.widthData.size()
922 && sectionS >= destLane.widthData[widthDataIndex + 1].s) {
925 offsets[i] += destLane.widthData[widthDataIndex].computeAt(sectionS) * multiplier;
933 }
else if (finalS == s) {
935 while (s < nextS && i < (
int)
cn.shape.size()) {
937 const double dist =
cn.shape[i - 1].distanceTo2D(
cn.shape[i]);
953 cn.shape.move2side(offsets);
958 #ifdef DEBUG_INTERNALSHAPES
959 std::cout <<
"internalShape "
961 <<
" dest=" << dest->
id
962 <<
" refLane=" << referenceLane
963 <<
" destPred\n" << destPred
964 <<
" offsets=" << offsets
965 <<
"\n shape=" << dest->
geom
966 <<
"\n shape2=" <<
cn.shape
970 cn.shape =
cn.shape.reverse();
973 #ifdef DEBUG_CONNECTIONS
975 std::cout <<
" added connection\n";
1002 if (lane.id == in) {
1003 in = lane.successor;
1015 for (std::vector<OpenDriveLink>::iterator i = e.
links.begin(); i != e.
links.end(); ++i) {
1022 std::string connectedEdge = l.
elementID;
1023 std::string edgeID = e.
id;
1026 const std::map<int, int>& laneMap = laneSection.
laneMap;
1027 #ifdef DEBUG_CONNECTIONS
1029 std::cout <<
"edge=" << e.
id <<
" eType=" << l.
elementType <<
" lType=" << l.
linkType <<
" connectedEdge=" << connectedEdge <<
" laneSection=" << laneSection.
s <<
" map:\n";
1035 for (std::vector<OpenDriveLane>::const_iterator j = lanes.begin(); j != lanes.end(); ++j) {
1044 c.
toEdge = connectedEdge;
1052 if (edges.find(c.
fromEdge) == edges.end()) {
1053 WRITE_ERROR(
"While setting connections: incoming road '" + c.
fromEdge +
"' is not known.");
1057 #ifdef DEBUG_CONNECTIONS
1059 std::cout <<
"insertConRight from=" << src->
id <<
"_" << c.
fromLane <<
" to=" << c.
toEdge <<
"_" << c.
toLane <<
"\n";
1067 for (std::vector<OpenDriveLane>::const_iterator j = lanes.begin(); j != lanes.end(); ++j) {
1084 if (edges.find(c.
fromEdge) == edges.end()) {
1085 WRITE_ERROR(
"While setting connections: incoming road '" + c.
fromEdge +
"' is not known.");
1089 #ifdef DEBUG_CONNECTIONS
1091 std::cout <<
"insertConLeft from=" << src->
id <<
"_" << c.
fromLane <<
" to=" << c.
toEdge <<
"_" << c.
toLane <<
"\n";
1103 return id.substr(1);
1114 if (!nc.
insert(
id, pos)) {
1116 throw ProcessError(
"Could not add node '" +
id +
"'.");
1128 throw ProcessError(
"Could not find node '" + nodeID +
"'.");
1131 if (e.
to !=
nullptr && e.
to != n) {
1132 throw ProcessError(
"Edge '" + e.
id +
"' has two end nodes ('" + e.
to->
getID() +
"' and '" + nodeID +
"').");
1136 if (e.
from !=
nullptr && e.
from != n) {
1137 throw ProcessError(
"Edge '" + e.
id +
"' has two start nodes ('" + e.
from->
getID() +
"' and '" + nodeID +
"').");
1149 if (el.
c != 0 || el.
d != 0) {
1159 const double res = oc.
getFloat(
"opendrive.curve-resolution");
1160 for (std::map<std::string, OpenDriveEdge*>::iterator i = edges.begin(); i != edges.end(); ++i) {
1165 for (std::vector<OpenDriveGeometry>::iterator j = e.
geometries.begin(); j != e.
geometries.end(); ++j) {
1193 if (!e.
geom.back().almostSame(geom.front())) {
1194 const int index = (int)(j - e.
geometries.begin());
1200 for (PositionVector::iterator k = geom.begin(); k != geom.end(); ++k) {
1206 if (e.
geom.size() == 1 && e.
geom.front() != last) {
1208 e.
geom.push_back(last);
1210 if (oc.
exists(
"geometry.min-dist") && !oc.
isDefault(
"geometry.min-dist")) {
1214 WRITE_ERROR(
"Unable to project coordinates for edge '" + e.
id +
"'.");
1220 for (std::vector<OpenDriveElevation>::iterator j = e.
elevations.begin(); j != e.
elevations.end(); ++j) {
1222 const double sNext = (j + 1) == e.
elevations.end() ? std::numeric_limits<double>::max() : (*(j + 1)).s;
1223 while (k < (
int)e.
geom.size() && pos < sNext) {
1228 if (k < (
int)e.
geom.size()) {
1231 pos += e.
geom[k - 1].distanceTo2D(e.
geom[k]);
1238 for (std::vector<OpenDriveLaneOffset>::iterator j = e.
offsets.begin(); j != e.
offsets.end(); ++j) {
1253 for (std::vector<OpenDriveLaneOffset>::iterator j = e.
offsets.begin(); j != e.
offsets.end(); ++j) {
1255 const double sNext = (j + 1) == e.
offsets.end() ? std::numeric_limits<double>::max() : (*(j + 1)).s;
1256 while (k < (
int)e.
geom.size() && pos < sNext) {
1257 const double offset = el.
computeAt(pos);
1260 if (k < (
int)e.
geom.size()) {
1263 pos += e.
geom[k - 1].distanceTo2D(e.
geom[k]);
1275 for (std::map<std::string, OpenDriveEdge*>::iterator i = edges.begin(); i != edges.end(); ++i) {
1277 #ifdef DEBUG_VARIABLE_SPEED
1280 std::cout <<
"revisitLaneSections e=" << e.
id <<
"\n";
1283 std::vector<OpenDriveLaneSection>& laneSections = e.
laneSections;
1285 std::vector<OpenDriveLaneSection> newSections;
1286 for (std::vector<OpenDriveLaneSection>::iterator j = laneSections.begin(); j != laneSections.end(); ++j) {
1287 std::vector<OpenDriveLaneSection> splitSections;
1288 bool splitBySpeed = (*j).buildSpeedChanges(tc, splitSections);
1289 if (!splitBySpeed) {
1290 newSections.push_back(*j);
1292 std::copy(splitSections.begin(), splitSections.end(), back_inserter(newSections));
1301 for (std::vector<OpenDriveLaneSection>::const_iterator j = laneSections.begin(); j != laneSections.end() && sorted; ++j) {
1302 if ((*j).s <= lastS) {
1308 WRITE_WARNING(
"The sections of edge '" + e.
id +
"' are not sorted properly.");
1314 for (std::vector<OpenDriveLaneSection>::iterator j = laneSections.begin(); j != laneSections.end();) {
1315 bool simlarToLast = fabs((*j).s - lastS) <
POSITION_EPS;
1319 if (simlarToLast && !e.
isInner) {
1320 WRITE_WARNING(
"Almost duplicate s-value '" +
toString(lastS) +
"' for lane sections occurred at edge '" + e.
id +
"'; second entry was removed.");
1321 j = laneSections.erase(j);
1326 #ifdef DEBUG_VARIABLE_SPEED
1339 if (resolution > 0 && g.
length > 0) {
1340 const int numPoints = (int)ceil(g.
length / resolution) + 1;
1341 double dx = (end.
x() - start.
x()) / (numPoints - 1);
1342 double dy = (end.
y() - start.
y()) / (numPoints - 1);
1343 for (
int i = 0; i < numPoints; i++) {
1344 ret.push_back(
Position(g.
x + i * dx, g.
y + i * dy));
1347 ret.push_back(start);
1358 double curveStart = g.
params[0];
1359 double curveEnd = g.
params[1];
1361 double cDot = (curveEnd - curveStart) / g.
length;
1362 if (cDot == 0 || g.
length == 0) {
1367 double sStart = curveStart / cDot;
1368 double sEnd = curveEnd / cDot;
1374 odrSpiral(sStart, cDot, &x, &y, &tStart);
1375 for (s = sStart; s <= sEnd; s += resolution) {
1386 assert(ret.size() >= 2);
1387 assert(ret[0] != ret[1]);
1390 ret.
add(ret.front() * -1);
1396 << std::setprecision(4)
1397 <<
"edge=" << e.
id <<
" s=" << g.
s
1398 <<
" cStart=" << curveStart
1399 <<
" cEnd=" << curveEnd
1401 <<
" sStart=" << sStart
1405 <<
"\n beforeShift=" << ret1
1406 <<
"\n beforeRot=" << ret2
1410 ret.
add(g.
x, g.
y, 0);
1411 }
catch (
const std::runtime_error&
error) {
1412 WRITE_WARNING(
"Could not compute spiral geometry for edge '" + e.
id +
"' (" +
error.what() +
").");
1424 double centerX = g.
x;
1425 double centerY = g.
y;
1427 double curvature = g.
params[0];
1428 double radius = 1. / curvature;
1433 double startX = g.
x;
1434 double startY = g.
y;
1435 double geo_posS = g.
s;
1436 double geo_posE = g.
s;
1439 geo_posE += resolution;
1440 if (geo_posE - g.
s > g.
length) {
1443 if (geo_posE - g.
s > g.
length) {
1446 calcPointOnCurve(&endX, &endY, centerX, centerY, radius, geo_posE - geo_posS);
1448 dist += (geo_posE - geo_posS);
1450 ret.push_back(
Position(startX, startY));
1454 geo_posS = geo_posE;
1456 if (geo_posE - (g.
s + g.
length) < 0.001 && geo_posE - (g.
s + g.
length) > -0.001) {
1467 const double s = sin(g.
hdg);
1468 const double c = cos(g.
hdg);
1470 for (
double off = 0; off < g.
length + 2.; off += resolution) {
1473 double xnew = x * c - y * s;
1474 double ynew = x * s + y * c;
1475 ret.push_back(
Position(g.
x + xnew, g.
y + ynew));
1484 const double s = sin(g.
hdg);
1485 const double c = cos(g.
hdg);
1487 const double pStep = pMax / ceil(g.
length / resolution);
1489 for (
double p = 0; p <= pMax + pStep; p += pStep) {
1492 double xnew = x * c - y * s;
1493 double ynew = x * s + y * c;
1494 ret.push_back(
Position(g.
x + xnew, g.
y + ynew));
1502 double normx = 1.0f;
1503 double normy = 0.0f;
1504 double x2 = normx * cos(hdg) - normy * sin(hdg);
1505 double y2 = normx * sin(hdg) + normy * cos(hdg);
1506 normx = x2 * length;
1507 normy = y2 * length;
1508 return Position(start.
x() + normx, start.
y() + normy);
1518 if (ad_radius > 0) {
1525 normX = normX * cos(ad_hdg) + normY * sin(ad_hdg);
1526 normY = tmpX * sin(ad_hdg) + normY * cos(ad_hdg);
1529 normX = turn * normY;
1530 normY = -turn * tmpX;
1532 normX = fabs(ad_radius) * normX;
1533 normY = fabs(ad_radius) * normY;
1542 double ad_r,
double ad_length) {
1543 double rotAngle = ad_length / fabs(ad_r);
1544 double vx = *ad_x - ad_centerX;
1545 double vy = *ad_y - ad_centerY;
1555 vx = vx * cos(rotAngle) + turn * vy * sin(rotAngle);
1556 vy = -1 * turn * tmpx * sin(rotAngle) + vy * cos(rotAngle);
1557 *ad_x = vx + ad_centerX;
1558 *ad_y = vy + ad_centerY;
1575 bool singleType =
true;
1576 std::vector<std::string> types;
1577 const std::vector<OpenDriveLane>& dirLanesR = lanesByDir.find(
OPENDRIVE_TAG_RIGHT)->second;
1578 for (std::vector<OpenDriveLane>::const_reverse_iterator i = dirLanesR.rbegin(); i != dirLanesR.rend(); ++i) {
1580 laneMap[(*i).id] = sumoLane++;
1581 types.push_back((*i).type);
1582 if (types.front() != types.back()) {
1587 rightLaneNumber = sumoLane;
1588 rightType = sumoLane > 0 ? (singleType ? types.front() :
joinToString(types,
"|")) :
"";
1592 const std::vector<OpenDriveLane>& dirLanesL = lanesByDir.find(
OPENDRIVE_TAG_LEFT)->second;
1593 for (std::vector<OpenDriveLane>::const_iterator i = dirLanesL.begin(); i != dirLanesL.end(); ++i) {
1595 laneMap[(*i).id] = sumoLane++;
1596 types.push_back((*i).type);
1597 if (types.front() != types.back()) {
1602 leftLaneNumber = sumoLane;
1603 leftType = sumoLane > 0 ? (singleType ? types.front() :
joinToString(types,
"|")) :
"";
1609 std::map<int, int> ret;
1610 const std::vector<OpenDriveLane>& dirLanes = lanesByDir.find(dir)->second;
1611 for (std::vector<OpenDriveLane>::const_reverse_iterator i = dirLanes.rbegin(); i != dirLanes.rend(); ++i) {
1612 std::map<int, int>::const_iterator toP = laneMap.find((*i).id);
1613 if (toP == laneMap.end()) {
1617 int to = (*toP).second;
1620 from = (*i).predecessor;
1623 std::map<int, int>::const_iterator fromP = prev.
laneMap.find(from);
1624 if (fromP != prev.
laneMap.end()) {
1625 from = (*fromP).second;
1631 if (ret.find(from) != ret.end()) {
1635 std::swap(from, to);
1654 if (i != l.
speeds.end()) {
1655 l.
speed = (*i).second;
1662 if (i != l.
speeds.end()) {
1663 l.
speed = (*i).second;
1672 std::set<double> speedChangePositions;
1675 for (std::vector<std::pair<double, double> >::const_iterator l = (*k).speeds.begin(); l != (*k).speeds.end(); ++l) {
1676 speedChangePositions.insert((*l).first);
1677 if ((*l).first == 0) {
1678 (*k).speed = (*l).second;
1683 for (std::vector<std::pair<double, double> >::const_iterator l = (*k).speeds.begin(); l != (*k).speeds.end(); ++l) {
1684 speedChangePositions.insert((*l).first);
1685 if ((*l).first == 0) {
1686 (*k).speed = (*l).second;
1691 if (speedChangePositions.size() == 0) {
1694 if (*speedChangePositions.begin() > 0) {
1695 speedChangePositions.insert(0);
1697 #ifdef DEBUG_VARIABLE_SPEED
1699 <<
" buildSpeedChanges sectionStart=" << s
1700 <<
" speedChangePositions=" <<
joinToString(speedChangePositions,
", ")
1703 for (std::set<double>::iterator i = speedChangePositions.begin(); i != speedChangePositions.end(); ++i) {
1704 if (i == speedChangePositions.begin()) {
1705 newSections.push_back(*
this);
1707 newSections.push_back(buildLaneSection(*i));
1711 for (
int i = 0; i != (int)newSections.size(); ++i) {
1713 std::map<OpenDriveXMLTag, std::vector<OpenDriveLane> >& lanesByDir = ls.
lanesByDir;
1714 for (std::map<
OpenDriveXMLTag, std::vector<OpenDriveLane> >::iterator k = lanesByDir.begin(); k != lanesByDir.end(); ++k) {
1715 std::vector<OpenDriveLane>& lanes = (*k).second;
1716 for (
int j = 0; j != (int)lanes.size(); ++j) {
1722 l.
speed = newSections[i - 1].lanesByDir[(*k).first][j].speed;
1741 for (std::vector<OpenDriveSignal>::const_iterator i = signals.begin(); i != signals.end(); ++i) {
1743 if ((*i).type ==
"301" || (*i).type ==
"306") {
1746 if ((*i).type ==
"205" ) {
1783 if (majorVersion != 1 || minorVersion != 2) {
1849 std::vector<double> vals;
1855 std::vector<double> vals;
1862 std::vector<double> vals;
1868 std::vector<double> vals;
1877 std::vector<double> vals;
1887 if (pRange ==
"normalized") {
1888 vals.push_back(1.0);
1889 }
else if (pRange ==
"arcLength") {
1890 vals.push_back(-1.0);
1893 vals.push_back(1.0);
1970 WRITE_ERROR(
"In laneLink-element: incoming road '" + c.fromEdge +
"' is not known.");
1986 l.width =
MAX2(l.width, a);
1988 #ifdef DEBUG_VARIABLE_WIDTHS
1995 <<
" type=" << l.type
1996 <<
" width=" << l.width
2002 <<
" entries=" << l.widthData.size()
2016 if (!unit.empty()) {
2018 if (unit ==
"km/h") {
2021 if (unit ==
"mph") {
2022 speed *= 1.609344 / 3.6;
2054 const std::string baseID = o.
id;
2069 for (
double x = 0; x <= length +
NUMERICAL_EPS; x += dist) {
2071 const double a = x / length;
2072 o.
width = wStart * (1 - a) + wEnd * a;
2073 o.
t = tStart * (1 - a) + tEnd * a;
2090 size_t i = cdata.find(
"+proj");
2091 if (i != std::string::npos) {
2092 const std::string proj = cdata.substr(i);
2102 result =
new GeoConvHelper(proj, networkOffset, origBoundary, convBoundary);
2105 WRITE_ERROR(
"Could not set projection. (" + std::string(e.what()) +
")");
2109 WRITE_WARNING(
"geoReference format '" + cdata +
"' currently not supported");
2153 const std::string& elementID,
2154 const std::string& contactPoint) {
2157 if (elementType ==
"road") {
2159 }
else if (elementType ==
"junction") {
2163 if (contactPoint ==
"start") {
2165 }
else if (contactPoint ==
"end") {
2205 #ifdef DEBUG_VARIABLE_WIDTHS
2208 std::cout <<
"sanitizeWidths e=" << e->
id <<
" sections=" << e->
laneSections.size() <<
"\n";
2226 if (l.widthData.size() > 0) {
2227 auto& wd = l.widthData;
2229 double maxNoShort = -std::numeric_limits<double>::max();
2231 for (
int i = 0; i < (int)wd.size(); i++) {
2232 const double wdLength = i < (int)wd.size() - 1 ? wd[i + 1].s - wd[i].s : length - seen;
2234 if (wdLength > threshold) {
2235 maxNoShort =
MAX2(maxNoShort, wd[i].a);
2238 if (maxNoShort > 0) {
2239 l.width = maxNoShort;
2248 std::vector<OpenDriveLaneSection> newSections;
2249 #ifdef DEBUG_VARIABLE_WIDTHS
2252 std::cout <<
"splitMinWidths e=" << e->
id <<
" sections=" << e->
laneSections.size() <<
"\n";
2257 std::vector<double> splitPositions;
2258 const double sectionEnd = (j + 1) == e->
laneSections.end() ? e->
length : (*(j + 1)).s;
2259 const int section = (int)(j - e->
laneSections.begin());
2260 #ifdef DEBUG_VARIABLE_WIDTHS
2262 std::cout <<
" findWidthSplit section=" << section <<
" sectionStart=" << sec.
s <<
" sectionOrigStart=" << sec.
sOrig <<
" sectionEnd=" << sectionEnd <<
"\n";
2271 newSections.push_back(sec);
2272 std::sort(splitPositions.begin(), splitPositions.end());
2274 double prevSplit = sec.
s;
2275 for (std::vector<double>::iterator it = splitPositions.begin(); it != splitPositions.end();) {
2276 if ((*it) - prevSplit < minDist || sectionEnd - (*it) < minDist) {
2278 #ifdef DEBUG_VARIABLE_WIDTHS
2280 std::cout <<
" skip close split=" << (*it) <<
" prevSplit=" << prevSplit <<
"\n";
2283 it = splitPositions.erase(it);
2284 }
else if ((*it) < sec.
s) {
2286 #ifdef DEBUG_VARIABLE_WIDTHS
2288 std::cout <<
" skip early split=" << (*it) <<
" s=" << sec.
s <<
"\n";
2291 it = splitPositions.erase(it);
2298 if (splitPositions.size() > 0) {
2299 #ifdef DEBUG_VARIABLE_WIDTHS
2301 std::cout <<
" road=" << e->
id <<
" splitMinWidths section=" << section
2302 <<
" start=" << sec.
s
2303 <<
" origStart=" << sec.
sOrig
2304 <<
" end=" << sectionEnd <<
" minDist=" << minDist
2305 <<
" splitPositions=" <<
toString(splitPositions) <<
"\n";
2308 #ifdef DEBUG_VARIABLE_WIDTHS
2310 std::cout <<
"first section...\n";
2314 for (std::vector<double>::iterator it = splitPositions.begin(); it != splitPositions.end(); ++it) {
2317 #ifdef DEBUG_VARIABLE_WIDTHS
2319 std::cout <<
"splitAt " << secNew.
s <<
"\n";
2322 newSections.push_back(secNew);
2329 double end = (it + 1) == splitPositions.end() ? sectionEnd : *(it + 1);
2341 int section,
double sectionStart,
double sectionEnd,
2342 std::vector<double>& splitPositions) {
2344 for (std::vector<OpenDriveLane>::iterator k = lanes.begin(); k != lanes.end(); ++k) {
2349 double wPrev = l.
widthData.front().computeAt(sPrev);
2351 <<
"findWidthSplit section=" << section
2352 <<
" sectionStart=" << sectionStart
2353 <<
" sectionEnd=" << sectionEnd
2355 <<
" type=" << l.
type
2356 <<
" widthEntries=" << l.
widthData.size() <<
"\n"
2360 for (std::vector<OpenDriveWidth>::iterator it_w = l.
widthData.begin(); it_w != l.
widthData.end(); ++it_w) {
2361 double sEnd = (it_w + 1) != l.
widthData.end() ? (*(it_w + 1)).s : sectionEnd - sectionStart;
2362 double w = (*it_w).computeAt(sEnd);
2365 <<
" s=" << (*it_w).s
2366 <<
" a=" << (*it_w).a <<
" b=" << (*it_w).b <<
" c=" << (*it_w).c <<
" d=" << (*it_w).d
2369 const double changeDist = fabs(
myMinWidth - wPrev);
2372 double splitPos = sPrev + (sEnd - sPrev) / fabs(w - wPrev) * changeDist;
2373 double wSplit = (*it_w).computeAt(splitPos);
2375 std::cout <<
" candidate splitPos=" << splitPos <<
" w=" << wSplit <<
"\n";
2382 if (splitPos < sPrev) {
2384 std::cout <<
" aborting search splitPos=" << splitPos <<
" wSplit=" << wSplit <<
" sPrev=" << sPrev <<
" wPrev=" << wPrev <<
"\n";
2392 if (splitPos > sEnd) {
2394 std::cout <<
" aborting search splitPos=" << splitPos <<
" wSplit=" << wSplit <<
" sEnd=" << sEnd <<
" w=" << w <<
"\n";
2400 wSplit = (*it_w).computeAt(splitPos);
2402 std::cout <<
" refined splitPos=" << splitPos <<
" w=" << wSplit <<
"\n";
2405 splitPositions.push_back(sectionStart + splitPos);
2423 for (std::vector<OpenDriveLane>::iterator k = lanes.begin(); k != lanes.end(); ++k) {
2424 (*k).predecessor = (*k).id;
2442 for (std::vector<OpenDriveLane>::iterator k = lanes.begin(); k != lanes.end(); ++k) {
2445 #ifdef DEBUG_VARIABLE_WIDTHS
2447 <<
"recomputeWidths lane=" << l.
id
2448 <<
" type=" << l.
type
2449 <<
" start=" << start
2451 <<
" sectionStart=" << sectionStart
2452 <<
" sectionEnd=" << sectionEnd
2453 <<
" widthEntries=" << l.
widthData.size() <<
"\n"
2458 double sPrevAbs = sPrev + sectionStart;
2459 for (std::vector<OpenDriveWidth>::iterator it_w = l.
widthData.begin(); it_w != l.
widthData.end(); ++it_w) {
2460 double sEnd = (it_w + 1) != l.
widthData.end() ? (*(it_w + 1)).s : sectionEnd - sectionStart;
2461 double sEndAbs = sEnd + sectionStart;
2462 #ifdef DEBUG_VARIABLE_WIDTHS
2464 <<
" sPrev=" << sPrev <<
" sPrevAbs=" << sPrevAbs
2465 <<
" sEnd=" << sEnd <<
" sEndAbs=" << sEndAbs
2466 <<
" widthData s=" << (*it_w).s
2467 <<
" a=" << (*it_w).a
2468 <<
" b=" << (*it_w).b
2469 <<
" c=" << (*it_w).c
2470 <<
" d=" << (*it_w).d
2473 if (sPrevAbs <= start && sEndAbs >= start) {
2474 #ifdef DEBUG_VARIABLE_WIDTHS
2476 std::cout <<
" atStart=" << start <<
" pos=" << start - sectionStart <<
" w=" << (*it_w).computeAt(start - sectionStart) <<
"\n";
2479 l.
width =
MAX2(l.
width, (*it_w).computeAt(start - sectionStart));
2481 if (sPrevAbs <= end && sEndAbs >= end) {
2482 #ifdef DEBUG_VARIABLE_WIDTHS
2484 std::cout <<
" atEnd=" << end <<
" pos=" << end - sectionStart <<
" w=" << (*it_w).computeAt(end - sectionStart) <<
"\n";
2489 if (start <= sPrevAbs && end >= sPrevAbs) {
2490 #ifdef DEBUG_VARIABLE_WIDTHS
2492 std::cout <<
" atSPrev=" << sPrev <<
" w=" << (*it_w).computeAt(sPrev) <<
"\n";
2497 if (start <= sEndAbs && end >= sEndAbs) {
2498 #ifdef DEBUG_VARIABLE_WIDTHS
2500 std::cout <<
" atSEnd=" << sEnd <<
" w=" << (*it_w).computeAt(sEnd) <<
"\n";
2505 #ifdef DEBUG_VARIABLE_WIDTHS
2507 std::cout <<
" sPrev=" << sPrev <<
" sEnd=" << sEnd <<
" l.width=" << l.
width <<
"\n";