61 #define DEBUG_COND(road) ((road)->id == "175")
62 #define DEBUG_COND2(edgeID) (StringUtils::startsWith((edgeID), "disabled"))
63 #define DEBUG_COND3(roadID) (roadID == "175")
183 std::map<std::string, OpenDriveEdge*> edges;
186 std::vector<std::string> files = oc.
getStringVector(
"opendrive-files");
187 for (std::vector<std::string>::const_iterator file = files.begin(); file != files.end(); ++file) {
189 WRITE_ERROR(
"Could not open opendrive file '" + *file +
"'.");
192 handler.setFileName(*file);
198 std::map<std::string, OpenDriveEdge*> innerEdges, outerEdges;
199 for (std::map<std::string, OpenDriveEdge*>::iterator i = edges.begin(); i != edges.end(); ++i) {
200 if ((*i).second->isInner) {
201 innerEdges[(*i).first] = (*i).second;
203 outerEdges[(*i).first] = (*i).second;
218 std::map<std::string, Boundary> posMap;
219 std::map<std::string, std::string> edge2junction;
221 for (std::map<std::string, OpenDriveEdge*>::iterator i = innerEdges.begin(); i != innerEdges.end(); ++i) {
225 if (posMap.find(e->
junction) == posMap.end()) {
231 for (std::map<std::string, Boundary>::iterator i = posMap.begin(); i != posMap.end(); ++i) {
234 throw ProcessError(
"Could not add node '" + (*i).first +
"'.");
238 for (std::map<std::string, OpenDriveEdge*>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
240 for (std::vector<OpenDriveLink>::iterator j = e->
links.begin(); j != e->
links.end(); ++j) {
248 throw ProcessError(
"Could not build node '" + nid +
"'.");
255 if (edge2junction.find(l.
elementID) != edge2junction.end()) {
267 for (std::map<std::string, OpenDriveEdge*>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
269 for (std::vector<OpenDriveLink>::iterator j = e->
links.begin(); j != e->
links.end(); ++j) {
276 std::string id1 = e->
id;
281 std::string nid = id1 +
"." + id2;
286 throw ProcessError(
"Could not build node '" + nid +
"'.");
304 for (std::map<std::string, OpenDriveEdge*>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
306 if (e->
to !=
nullptr && e->
from !=
nullptr) {
309 for (std::map<std::string, OpenDriveEdge*>::iterator j = innerEdges.begin(); j != innerEdges.end(); ++j) {
311 for (std::vector<OpenDriveLink>::iterator k = ie->
links.begin(); k != ie->
links.end(); ++k) {
317 std::string nid = edge2junction[ie->
id];
329 for (std::map<std::string, OpenDriveEdge*>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
331 if ((e->
from ==
nullptr || e->
to ==
nullptr) && e->
geom.size() == 0) {
334 if (e->
from ==
nullptr) {
335 const std::string nid = e->
id +
".begin";
338 if (e->
to ==
nullptr) {
339 const std::string nid = e->
id +
".end";
348 const double defaultSpeed = tc.
getSpeed(
"");
351 for (std::map<std::string, OpenDriveEdge*>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
353 if (e->
geom.size() < 2) {
357 bool lanesBuilt =
false;
381 const double length2D = geomWithOffset.
length2D();
382 double cF = length2D == 0 ? 1 : e->
length / length2D;
383 NBEdge* prevRight =
nullptr;
384 NBEdge* prevLeft =
nullptr;
392 WRITE_WARNING(
"Edge '" + e->
id +
"' has to be split as it connects same junctions.")
396 const double minDist = oc.
getFloat(
"opendrive.curve-resolution");
407 double nextS = (j + 1)->s;
415 std::string
id = e->
id;
416 if (sFrom != e->
from || sTo != e->
to) {
421 #ifdef DEBUG_VARIABLE_WIDTHS
423 std::cout <<
" id=" <<
id <<
" sB=" << sB <<
" sE=" << sE <<
" geom=" << geom <<
"\n";
428 NBEdge* currRight =
nullptr;
429 if ((*j).rightLaneNumber > 0) {
430 currRight =
new NBEdge(
"-" +
id, sFrom, sTo, (*j).rightType, defaultSpeed, (*j).rightLaneNumber, priorityR,
434 for (std::vector<OpenDriveLane>::const_iterator k = lanes.begin(); k != lanes.end(); ++k) {
435 std::map<int, int>::const_iterator lp = (*j).laneMap.find((*k).id);
436 if (lp != (*j).laneMap.end()) {
437 int sumoLaneIndex = lp->second;
448 if (prevRight !=
nullptr) {
450 for (std::map<int, int>::const_iterator k = connections.begin(); k != connections.end(); ++k) {
451 #ifdef DEBUG_CONNECTIONS
453 std::cout <<
"addCon1 from=" << prevRight->
getID() <<
"_" << (*k).first <<
" to=" << currRight->
getID() <<
"_" << (*k).second <<
"\n";
459 prevRight = currRight;
464 NBEdge* currLeft =
nullptr;
465 if ((*j).leftLaneNumber > 0) {
466 currLeft =
new NBEdge(
id, sTo, sFrom, (*j).leftType, defaultSpeed, (*j).leftLaneNumber, priorityL,
470 for (std::vector<OpenDriveLane>::const_iterator k = lanes.begin(); k != lanes.end(); ++k) {
471 std::map<int, int>::const_iterator lp = (*j).laneMap.find((*k).id);
472 if (lp != (*j).laneMap.end()) {
473 int sumoLaneIndex = lp->second;
484 if (prevLeft !=
nullptr) {
485 std::map<int, int> connections = (*j).getInnerConnections(
OPENDRIVE_TAG_LEFT, *(j - 1));
486 for (std::map<int, int>::const_iterator k = connections.begin(); k != connections.end(); ++k) {
487 #ifdef DEBUG_CONNECTIONS
489 std::cout <<
"addCon2 from=" << currLeft->
getID() <<
"_" << (*k).first <<
" to=" << prevLeft->
getID() <<
"_" << (*k).second <<
"\n";
505 if (oc.
isSet(
"polygon-output")) {
524 centerLine.push_back(
Position(-o.length / 2, 0));
525 centerLine.push_back(
Position(o.length / 2, 0));
527 centerLine.
rotate2D(roadHdg + o.hdg);
539 for (
int i = 0; i < (int) shape.size(); i++) {
558 for (std::map<std::string, OpenDriveEdge*>::iterator i = edges.begin(); i != edges.end(); ++i) {
562 std::vector<Connection> connections2;
563 for (std::map<std::string, OpenDriveEdge*>::iterator j = edges.begin(); j != edges.end(); ++j) {
564 const std::set<Connection>& conns = (*j).second->connections;
566 for (std::set<Connection>::const_iterator i = conns.begin(); i != conns.end(); ++i) {
567 if (innerEdges.find((*i).fromEdge) != innerEdges.end()) {
571 if (innerEdges.find((*i).toEdge) != innerEdges.end()) {
572 std::set<Connection> seen;
575 connections2.push_back(*i);
580 for (std::vector<Connection>::const_iterator i = connections2.begin(); i != connections2.end(); ++i) {
581 #ifdef DEBUG_CONNECTIONS
582 std::cout <<
"connections2 " << (*i).getDescription() <<
"\n";
584 std::string fromEdge = (*i).fromEdge;
585 if (edges.find(fromEdge) == edges.end()) {
586 WRITE_WARNING(
"While setting connections: from-edge '" + fromEdge +
"' is not known.");
590 int fromLane = (*i).fromLane;
594 std::string toEdge = (*i).toEdge;
595 if (edges.find(toEdge) == edges.end()) {
596 WRITE_WARNING(
"While setting connections: to-edge '" + toEdge +
"' is not known.");
601 int toLane = (*i).toLane;
621 if (from ==
nullptr) {
622 WRITE_WARNING(
"Could not find fromEdge representation of '" + fromEdge +
"' in connection '" + (*i).origID +
"'.");
625 WRITE_WARNING(
"Could not find fromEdge representation of '" + toEdge +
"' in connection '" + (*i).origID +
"'.");
627 if (from ==
nullptr || to ==
nullptr) {
631 #ifdef DEBUG_CONNECTIONS
633 std::cout <<
"addCon3 from=" << from->
getID() <<
"_" << fromLane <<
" to=" << to->
getID() <<
"_" << toLane <<
"\n";
642 if ((*i).origID !=
"" && saveOrigIDs) {
645 for (std::vector<NBEdge::Connection>::iterator k = cons.begin(); k != cons.end(); ++k) {
646 if ((*k).fromLane == fromLane && (*k).toEdge == to && (*k).toLane == toLane) {
658 std::map<std::string, std::string> tlsControlled;
659 for (std::map<std::string, OpenDriveEdge*>::iterator i = edges.begin(); i != edges.end(); ++i) {
661 for (std::vector<OpenDriveSignal>::const_iterator j = e->
signals.begin(); j != e->
signals.end(); ++j) {
662 if ((*j).type !=
"1000001") {
665 std::vector<OpenDriveLaneSection>::iterator k = e->
laneSections.begin();
668 if ((*j).s > (*k).s && (*j).s <= (*(k + 1)).s) {
678 std::string
id = (*k).sumoID;
682 std::string fromID, toID;
683 for (std::vector<OpenDriveLink>::const_iterator l = e->
links.begin(); l != e->
links.end(); ++l) {
691 if ((*j).orientation < 0) {
692 fromID =
"-" + fromID;
696 if ((*j).orientation > 0) {
697 fromID =
"-" + fromID;
709 id = fromID +
"->" + toID;
711 WRITE_WARNING(
"Found a traffic light signal on an unknown edge (original edge id='" + e->
id +
"').");
715 if ((*j).orientation > 0) {
719 tlsControlled[id] = (*j).name;
723 for (std::map<std::string, std::string>::iterator i = tlsControlled.begin(); i != tlsControlled.end(); ++i) {
724 std::string
id = (*i).first;
725 if (
id.find(
"->") != std::string::npos) {
726 id =
id.substr(0,
id.find(
"->"));
730 WRITE_WARNING(
"Could not find edge '" +
id +
"' while building its traffic light.");
752 for (std::map<std::string, OpenDriveEdge*>::iterator i = edges.begin(); i != edges.end(); ++i) {
774 if (sumoLane.
width >= 0 && widthResolution > 0) {
775 sumoLane.
width = floor(sumoLane.
width / widthResolution + 0.5) * widthResolution;
777 sumoLane.
width -= widthResolution;
778 if (sumoLane.
width <= 0) {
781 }
else if (sumoLane.
width == 0) {
783 sumoLane.
width = widthResolution;
789 if (forbiddenNarrow) {
799 #ifdef DEBUG_CONNECTIONS
801 std::cout <<
" buildConnectionsToOuter " << c.
getDescription() <<
"\n";
802 std::cout <<
" dest=" << (dest ==
nullptr ?
"NULL" : dest->
id) <<
" seenlist=";
803 for (std::set<Connection>::const_iterator i = seen.begin(); i != seen.end(); ++i) {
804 std::cout <<
" " << (*i).fromEdge <<
"," << (*i).toEdge <<
" ";
809 if (dest ==
nullptr) {
814 const std::set<Connection>& conts = dest->
connections;
815 for (std::set<Connection>::const_iterator i = conts.begin(); i != conts.end(); ++i) {
816 auto innerEdgesIt = innerEdges.find((*i).toEdge);
817 #ifdef DEBUG_CONNECTIONS
819 std::cout <<
" toInner=" << (innerEdgesIt != innerEdges.end()) <<
" destCon " << (*i).getDescription() <<
"\n";
822 if (innerEdgesIt != innerEdges.end()) {
823 std::vector<Connection> t;
824 if (seen.count(*i) == 0) {
826 for (std::vector<Connection>::const_iterator j = t.begin(); j != t.end(); ++j) {
834 cn.shape = innerEdgesIt->second->geom + c.
shape;
854 int referenceLane = 0;
855 int offsetFactor = 1;
859 for (
const auto& destLane : dest->
laneSections.front().lanesByDir[lanesDir]) {
860 if (destLane.successor == c.
fromLane) {
861 referenceLane = destLane.id;
867 for (
const auto& destLane : dest->
laneSections.front().lanesByDir[lanesDir]) {
868 if (destLane.predecessor == c.
fromLane) {
869 referenceLane = destLane.id;
878 std::vector<double> offsets(dest->
geom.size(), 0);
882 #ifdef DEBUG_INTERNALSHAPES
883 std::string destPred;
887 for (
int laneSectionIndex = 0; laneSectionIndex < (int)dest->
laneSections.size(); laneSectionIndex++) {
888 auto& laneSection = dest->
laneSections[laneSectionIndex];
889 const double nextS = laneSectionIndex + 1 < (int)dest->
laneSections.size() ? dest->
laneSections[laneSectionIndex + 1].s : std::numeric_limits<double>::max();
894 for (
const OpenDriveLane& destLane : laneSection.lanesByDir[lanesDir]) {
899 #ifdef DEBUG_INTERNALSHAPES
900 destPred +=
" lane=" +
toString(destLane.id)
901 +
" pred=" +
toString(destLane.predecessor)
902 +
" succ=" +
toString(destLane.successor)
903 +
" wStart=" +
toString(destLane.widthData.front().computeAt(0))
904 +
" wEnd=" +
toString(destLane.widthData.front().computeAt(
cn.shape.length2D()))
905 +
" width=" +
toString(destLane.width) +
"\n";
907 if (abs(destLane.id) <= abs(referenceLane)) {
908 const double multiplier = offsetFactor * (destLane.id == referenceLane ? 0.5 : 1);
909 #ifdef DEBUG_INTERNALSHAPES
910 destPred +=
" multiplier=" +
toString(multiplier) +
"\n";
912 int widthDataIndex = 0;
913 while (s < nextS && i < (
int)
cn.shape.size()) {
915 const double dist =
cn.shape[i - 1].distanceTo2D(
cn.shape[i]);
920 while (widthDataIndex + 1 < (
int)destLane.widthData.size()
921 && sectionS >= destLane.widthData[widthDataIndex + 1].s) {
924 offsets[i] += destLane.widthData[widthDataIndex].computeAt(sectionS) * multiplier;
932 }
else if (finalS == s) {
934 while (s < nextS && i < (
int)
cn.shape.size()) {
936 const double dist =
cn.shape[i - 1].distanceTo2D(
cn.shape[i]);
952 cn.shape.move2side(offsets);
957 #ifdef DEBUG_INTERNALSHAPES
958 std::cout <<
"internalShape "
960 <<
" dest=" << dest->
id
961 <<
" refLane=" << referenceLane
962 <<
" destPred\n" << destPred
963 <<
" offsets=" << offsets
964 <<
"\n shape=" << dest->
geom
965 <<
"\n shape2=" <<
cn.shape
969 cn.shape =
cn.shape.reverse();
972 #ifdef DEBUG_CONNECTIONS
974 std::cout <<
" added connection\n";
1001 if (lane.id == in) {
1002 in = lane.successor;
1014 for (std::vector<OpenDriveLink>::iterator i = e.
links.begin(); i != e.
links.end(); ++i) {
1021 std::string connectedEdge = l.
elementID;
1022 std::string edgeID = e.
id;
1025 const std::map<int, int>& laneMap = laneSection.
laneMap;
1026 #ifdef DEBUG_CONNECTIONS
1028 std::cout <<
"edge=" << e.
id <<
" eType=" << l.
elementType <<
" lType=" << l.
linkType <<
" connectedEdge=" << connectedEdge <<
" laneSection=" << laneSection.
s <<
" map:\n";
1034 for (std::vector<OpenDriveLane>::const_iterator j = lanes.begin(); j != lanes.end(); ++j) {
1043 c.
toEdge = connectedEdge;
1051 if (edges.find(c.
fromEdge) == edges.end()) {
1052 WRITE_ERROR(
"While setting connections: incoming road '" + c.
fromEdge +
"' is not known.");
1056 #ifdef DEBUG_CONNECTIONS
1058 std::cout <<
"insertConRight from=" << src->
id <<
"_" << c.
fromLane <<
" to=" << c.
toEdge <<
"_" << c.
toLane <<
"\n";
1066 for (std::vector<OpenDriveLane>::const_iterator j = lanes.begin(); j != lanes.end(); ++j) {
1083 if (edges.find(c.
fromEdge) == edges.end()) {
1084 WRITE_ERROR(
"While setting connections: incoming road '" + c.
fromEdge +
"' is not known.");
1088 #ifdef DEBUG_CONNECTIONS
1090 std::cout <<
"insertConLeft from=" << src->
id <<
"_" << c.
fromLane <<
" to=" << c.
toEdge <<
"_" << c.
toLane <<
"\n";
1102 return id.substr(1);
1113 if (!nc.
insert(
id, pos)) {
1115 throw ProcessError(
"Could not add node '" +
id +
"'.");
1127 throw ProcessError(
"Could not find node '" + nodeID +
"'.");
1130 if (e.
to !=
nullptr && e.
to != n) {
1131 throw ProcessError(
"Edge '" + e.
id +
"' has two end nodes ('" + e.
to->
getID() +
"' and '" + nodeID +
"').");
1135 if (e.
from !=
nullptr && e.
from != n) {
1136 throw ProcessError(
"Edge '" + e.
id +
"' has two start nodes ('" + e.
from->
getID() +
"' and '" + nodeID +
"').");
1148 if (el.
c != 0 || el.
d != 0) {
1158 const double res = oc.
getFloat(
"opendrive.curve-resolution");
1159 for (std::map<std::string, OpenDriveEdge*>::iterator i = edges.begin(); i != edges.end(); ++i) {
1164 for (std::vector<OpenDriveGeometry>::iterator j = e.
geometries.begin(); j != e.
geometries.end(); ++j) {
1192 if (!e.
geom.back().almostSame(geom.front())) {
1193 const int index = (int)(j - e.
geometries.begin());
1199 for (PositionVector::iterator k = geom.begin(); k != geom.end(); ++k) {
1205 if (e.
geom.size() == 1 && e.
geom.front() != last) {
1207 e.
geom.push_back(last);
1209 if (oc.
exists(
"geometry.min-dist") && !oc.
isDefault(
"geometry.min-dist")) {
1213 WRITE_ERROR(
"Unable to project coordinates for edge '" + e.
id +
"'.");
1219 for (std::vector<OpenDriveElevation>::iterator j = e.
elevations.begin(); j != e.
elevations.end(); ++j) {
1221 const double sNext = (j + 1) == e.
elevations.end() ? std::numeric_limits<double>::max() : (*(j + 1)).s;
1222 while (k < (
int)e.
geom.size() && pos < sNext) {
1227 if (k < (
int)e.
geom.size()) {
1230 pos += e.
geom[k - 1].distanceTo2D(e.
geom[k]);
1237 for (std::vector<OpenDriveLaneOffset>::iterator j = e.
offsets.begin(); j != e.
offsets.end(); ++j) {
1252 for (std::vector<OpenDriveLaneOffset>::iterator j = e.
offsets.begin(); j != e.
offsets.end(); ++j) {
1254 const double sNext = (j + 1) == e.
offsets.end() ? std::numeric_limits<double>::max() : (*(j + 1)).s;
1255 while (k < (
int)e.
geom.size() && pos < sNext) {
1256 const double offset = el.
computeAt(pos);
1259 if (k < (
int)e.
geom.size()) {
1262 pos += e.
geom[k - 1].distanceTo2D(e.
geom[k]);
1274 for (std::map<std::string, OpenDriveEdge*>::iterator i = edges.begin(); i != edges.end(); ++i) {
1276 #ifdef DEBUG_VARIABLE_SPEED
1279 std::cout <<
"revisitLaneSections e=" << e.
id <<
"\n";
1282 std::vector<OpenDriveLaneSection>& laneSections = e.
laneSections;
1284 std::vector<OpenDriveLaneSection> newSections;
1285 for (std::vector<OpenDriveLaneSection>::iterator j = laneSections.begin(); j != laneSections.end(); ++j) {
1286 std::vector<OpenDriveLaneSection> splitSections;
1287 bool splitBySpeed = (*j).buildSpeedChanges(tc, splitSections);
1288 if (!splitBySpeed) {
1289 newSections.push_back(*j);
1291 std::copy(splitSections.begin(), splitSections.end(), back_inserter(newSections));
1300 for (std::vector<OpenDriveLaneSection>::const_iterator j = laneSections.begin(); j != laneSections.end() && sorted; ++j) {
1301 if ((*j).s <= lastS) {
1307 WRITE_WARNING(
"The sections of edge '" + e.
id +
"' are not sorted properly.");
1313 for (std::vector<OpenDriveLaneSection>::iterator j = laneSections.begin(); j != laneSections.end();) {
1314 bool simlarToLast = fabs((*j).s - lastS) <
POSITION_EPS;
1318 if (simlarToLast && !e.
isInner) {
1319 WRITE_WARNING(
"Almost duplicate s-value '" +
toString(lastS) +
"' for lane sections occurred at edge '" + e.
id +
"'; second entry was removed.");
1320 j = laneSections.erase(j);
1325 #ifdef DEBUG_VARIABLE_SPEED
1338 if (resolution > 0 && g.
length > 0) {
1339 const int numPoints = (int)ceil(g.
length / resolution) + 1;
1340 double dx = (end.
x() - start.
x()) / (numPoints - 1);
1341 double dy = (end.
y() - start.
y()) / (numPoints - 1);
1342 for (
int i = 0; i < numPoints; i++) {
1343 ret.push_back(
Position(g.
x + i * dx, g.
y + i * dy));
1346 ret.push_back(start);
1357 double curveStart = g.
params[0];
1358 double curveEnd = g.
params[1];
1360 double cDot = (curveEnd - curveStart) / g.
length;
1361 if (cDot == 0 || g.
length == 0) {
1366 double sStart = curveStart / cDot;
1367 double sEnd = curveEnd / cDot;
1373 odrSpiral(sStart, cDot, &x, &y, &tStart);
1374 for (s = sStart; s <= sEnd; s += resolution) {
1385 assert(ret.size() >= 2);
1386 assert(ret[0] != ret[1]);
1389 ret.
add(ret.front() * -1);
1395 << std::setprecision(4)
1396 <<
"edge=" << e.
id <<
" s=" << g.
s
1397 <<
" cStart=" << curveStart
1398 <<
" cEnd=" << curveEnd
1400 <<
" sStart=" << sStart
1404 <<
"\n beforeShift=" << ret1
1405 <<
"\n beforeRot=" << ret2
1409 ret.
add(g.
x, g.
y, 0);
1410 }
catch (
const std::runtime_error&
error) {
1411 WRITE_WARNING(
"Could not compute spiral geometry for edge '" + e.
id +
"' (" +
error.what() +
").");
1423 double centerX = g.
x;
1424 double centerY = g.
y;
1426 double curvature = g.
params[0];
1427 double radius = 1. / curvature;
1432 double startX = g.
x;
1433 double startY = g.
y;
1434 double geo_posS = g.
s;
1435 double geo_posE = g.
s;
1438 geo_posE += resolution;
1439 if (geo_posE - g.
s > g.
length) {
1442 if (geo_posE - g.
s > g.
length) {
1445 calcPointOnCurve(&endX, &endY, centerX, centerY, radius, geo_posE - geo_posS);
1447 dist += (geo_posE - geo_posS);
1449 ret.push_back(
Position(startX, startY));
1453 geo_posS = geo_posE;
1455 if (geo_posE - (g.
s + g.
length) < 0.001 && geo_posE - (g.
s + g.
length) > -0.001) {
1466 const double s = sin(g.
hdg);
1467 const double c = cos(g.
hdg);
1469 for (
double off = 0; off < g.
length + 2.; off += resolution) {
1472 double xnew = x * c - y * s;
1473 double ynew = x * s + y * c;
1474 ret.push_back(
Position(g.
x + xnew, g.
y + ynew));
1483 const double s = sin(g.
hdg);
1484 const double c = cos(g.
hdg);
1486 const double pStep = pMax / ceil(g.
length / resolution);
1488 for (
double p = 0; p <= pMax + pStep; p += pStep) {
1491 double xnew = x * c - y * s;
1492 double ynew = x * s + y * c;
1493 ret.push_back(
Position(g.
x + xnew, g.
y + ynew));
1501 double normx = 1.0f;
1502 double normy = 0.0f;
1503 double x2 = normx * cos(hdg) - normy * sin(hdg);
1504 double y2 = normx * sin(hdg) + normy * cos(hdg);
1505 normx = x2 * length;
1506 normy = y2 * length;
1507 return Position(start.
x() + normx, start.
y() + normy);
1517 if (ad_radius > 0) {
1524 normX = normX * cos(ad_hdg) + normY * sin(ad_hdg);
1525 normY = tmpX * sin(ad_hdg) + normY * cos(ad_hdg);
1528 normX = turn * normY;
1529 normY = -turn * tmpX;
1531 normX = fabs(ad_radius) * normX;
1532 normY = fabs(ad_radius) * normY;
1541 double ad_r,
double ad_length) {
1542 double rotAngle = ad_length / fabs(ad_r);
1543 double vx = *ad_x - ad_centerX;
1544 double vy = *ad_y - ad_centerY;
1554 vx = vx * cos(rotAngle) + turn * vy * sin(rotAngle);
1555 vy = -1 * turn * tmpx * sin(rotAngle) + vy * cos(rotAngle);
1556 *ad_x = vx + ad_centerX;
1557 *ad_y = vy + ad_centerY;
1574 bool singleType =
true;
1575 std::vector<std::string> types;
1576 const std::vector<OpenDriveLane>& dirLanesR = lanesByDir.find(
OPENDRIVE_TAG_RIGHT)->second;
1577 for (std::vector<OpenDriveLane>::const_reverse_iterator i = dirLanesR.rbegin(); i != dirLanesR.rend(); ++i) {
1579 laneMap[(*i).id] = sumoLane++;
1580 types.push_back((*i).type);
1581 if (types.front() != types.back()) {
1586 rightLaneNumber = sumoLane;
1587 rightType = sumoLane > 0 ? (singleType ? types.front() :
joinToString(types,
"|")) :
"";
1591 const std::vector<OpenDriveLane>& dirLanesL = lanesByDir.find(
OPENDRIVE_TAG_LEFT)->second;
1592 for (std::vector<OpenDriveLane>::const_iterator i = dirLanesL.begin(); i != dirLanesL.end(); ++i) {
1594 laneMap[(*i).id] = sumoLane++;
1595 types.push_back((*i).type);
1596 if (types.front() != types.back()) {
1601 leftLaneNumber = sumoLane;
1602 leftType = sumoLane > 0 ? (singleType ? types.front() :
joinToString(types,
"|")) :
"";
1608 std::map<int, int> ret;
1609 const std::vector<OpenDriveLane>& dirLanes = lanesByDir.find(dir)->second;
1610 for (std::vector<OpenDriveLane>::const_reverse_iterator i = dirLanes.rbegin(); i != dirLanes.rend(); ++i) {
1611 std::map<int, int>::const_iterator toP = laneMap.find((*i).id);
1612 if (toP == laneMap.end()) {
1616 int to = (*toP).second;
1619 from = (*i).predecessor;
1622 std::map<int, int>::const_iterator fromP = prev.
laneMap.find(from);
1623 if (fromP != prev.
laneMap.end()) {
1624 from = (*fromP).second;
1630 if (ret.find(from) != ret.end()) {
1634 std::swap(from, to);
1653 if (i != l.
speeds.end()) {
1654 l.
speed = (*i).second;
1661 if (i != l.
speeds.end()) {
1662 l.
speed = (*i).second;
1671 std::set<double> speedChangePositions;
1674 for (std::vector<std::pair<double, double> >::const_iterator l = (*k).speeds.begin(); l != (*k).speeds.end(); ++l) {
1675 speedChangePositions.insert((*l).first);
1676 if ((*l).first == 0) {
1677 (*k).speed = (*l).second;
1682 for (std::vector<std::pair<double, double> >::const_iterator l = (*k).speeds.begin(); l != (*k).speeds.end(); ++l) {
1683 speedChangePositions.insert((*l).first);
1684 if ((*l).first == 0) {
1685 (*k).speed = (*l).second;
1690 if (speedChangePositions.size() == 0) {
1693 if (*speedChangePositions.begin() > 0) {
1694 speedChangePositions.insert(0);
1696 #ifdef DEBUG_VARIABLE_SPEED
1698 <<
" buildSpeedChanges sectionStart=" << s
1699 <<
" speedChangePositions=" <<
joinToString(speedChangePositions,
", ")
1702 for (std::set<double>::iterator i = speedChangePositions.begin(); i != speedChangePositions.end(); ++i) {
1703 if (i == speedChangePositions.begin()) {
1704 newSections.push_back(*
this);
1706 newSections.push_back(buildLaneSection(*i));
1710 for (
int i = 0; i != (int)newSections.size(); ++i) {
1712 std::map<OpenDriveXMLTag, std::vector<OpenDriveLane> >& lanesByDir = ls.
lanesByDir;
1713 for (std::map<
OpenDriveXMLTag, std::vector<OpenDriveLane> >::iterator k = lanesByDir.begin(); k != lanesByDir.end(); ++k) {
1714 std::vector<OpenDriveLane>& lanes = (*k).second;
1715 for (
int j = 0; j != (int)lanes.size(); ++j) {
1721 l.
speed = newSections[i - 1].lanesByDir[(*k).first][j].speed;
1740 for (std::vector<OpenDriveSignal>::const_iterator i = signals.begin(); i != signals.end(); ++i) {
1742 if ((*i).type ==
"301" || (*i).type ==
"306") {
1745 if ((*i).type ==
"205" ) {
1782 if (majorVersion != 1 || minorVersion != 2) {
1848 std::vector<double> vals;
1854 std::vector<double> vals;
1861 std::vector<double> vals;
1867 std::vector<double> vals;
1876 std::vector<double> vals;
1886 if (pRange ==
"normalized") {
1887 vals.push_back(1.0);
1888 }
else if (pRange ==
"arcLength") {
1889 vals.push_back(-1.0);
1892 vals.push_back(1.0);
1969 WRITE_ERROR(
"In laneLink-element: incoming road '" + c.fromEdge +
"' is not known.");
1985 l.width =
MAX2(l.width, a);
1987 #ifdef DEBUG_VARIABLE_WIDTHS
1994 <<
" type=" << l.type
1995 <<
" width=" << l.width
2001 <<
" entries=" << l.widthData.size()
2015 if (!unit.empty()) {
2017 if (unit ==
"km/h") {
2020 if (unit ==
"mph") {
2021 speed *= 1.609344 / 3.6;
2053 const std::string baseID = o.
id;
2068 for (
double x = 0; x <= length +
NUMERICAL_EPS; x += dist) {
2070 const double a = x / length;
2071 o.
width = wStart * (1 - a) + wEnd * a;
2072 o.
t = tStart * (1 - a) + tEnd * a;
2089 size_t i = cdata.find(
"+proj");
2090 if (i != std::string::npos) {
2091 const std::string proj = cdata.substr(i);
2101 result =
new GeoConvHelper(proj, networkOffset, origBoundary, convBoundary);
2104 WRITE_ERROR(
"Could not set projection. (" + std::string(e.what()) +
")");
2108 WRITE_WARNING(
"geoReference format '" + cdata +
"' currently not supported");
2152 const std::string& elementID,
2153 const std::string& contactPoint) {
2156 if (elementType ==
"road") {
2158 }
else if (elementType ==
"junction") {
2162 if (contactPoint ==
"start") {
2164 }
else if (contactPoint ==
"end") {
2204 #ifdef DEBUG_VARIABLE_WIDTHS
2207 std::cout <<
"sanitizeWidths e=" << e->
id <<
" sections=" << e->
laneSections.size() <<
"\n";
2225 if (l.widthData.size() > 0) {
2226 auto& wd = l.widthData;
2228 double maxNoShort = -std::numeric_limits<double>::max();
2230 for (
int i = 0; i < (int)wd.size(); i++) {
2231 const double wdLength = i < (int)wd.size() - 1 ? wd[i + 1].s - wd[i].s : length - seen;
2233 if (wdLength > threshold) {
2234 maxNoShort =
MAX2(maxNoShort, wd[i].a);
2237 if (maxNoShort > 0) {
2238 l.width = maxNoShort;
2247 std::vector<OpenDriveLaneSection> newSections;
2248 #ifdef DEBUG_VARIABLE_WIDTHS
2251 std::cout <<
"splitMinWidths e=" << e->
id <<
" sections=" << e->
laneSections.size() <<
"\n";
2256 std::vector<double> splitPositions;
2257 const double sectionEnd = (j + 1) == e->
laneSections.end() ? e->
length : (*(j + 1)).s;
2258 const int section = (int)(j - e->
laneSections.begin());
2259 #ifdef DEBUG_VARIABLE_WIDTHS
2261 std::cout <<
" findWidthSplit section=" << section <<
" sectionStart=" << sec.
s <<
" sectionOrigStart=" << sec.
sOrig <<
" sectionEnd=" << sectionEnd <<
"\n";
2270 newSections.push_back(sec);
2271 std::sort(splitPositions.begin(), splitPositions.end());
2273 double prevSplit = sec.
s;
2274 for (std::vector<double>::iterator it = splitPositions.begin(); it != splitPositions.end();) {
2275 if ((*it) - prevSplit < minDist || sectionEnd - (*it) < minDist) {
2277 #ifdef DEBUG_VARIABLE_WIDTHS
2279 std::cout <<
" skip close split=" << (*it) <<
" prevSplit=" << prevSplit <<
"\n";
2282 it = splitPositions.erase(it);
2283 }
else if ((*it) < sec.
s) {
2285 #ifdef DEBUG_VARIABLE_WIDTHS
2287 std::cout <<
" skip early split=" << (*it) <<
" s=" << sec.
s <<
"\n";
2290 it = splitPositions.erase(it);
2297 if (splitPositions.size() > 0) {
2298 #ifdef DEBUG_VARIABLE_WIDTHS
2300 std::cout <<
" road=" << e->
id <<
" splitMinWidths section=" << section
2301 <<
" start=" << sec.
s
2302 <<
" origStart=" << sec.
sOrig
2303 <<
" end=" << sectionEnd <<
" minDist=" << minDist
2304 <<
" splitPositions=" <<
toString(splitPositions) <<
"\n";
2307 #ifdef DEBUG_VARIABLE_WIDTHS
2309 std::cout <<
"first section...\n";
2313 for (std::vector<double>::iterator it = splitPositions.begin(); it != splitPositions.end(); ++it) {
2316 #ifdef DEBUG_VARIABLE_WIDTHS
2318 std::cout <<
"splitAt " << secNew.
s <<
"\n";
2321 newSections.push_back(secNew);
2328 double end = (it + 1) == splitPositions.end() ? sectionEnd : *(it + 1);
2340 int section,
double sectionStart,
double sectionEnd,
2341 std::vector<double>& splitPositions) {
2343 for (std::vector<OpenDriveLane>::iterator k = lanes.begin(); k != lanes.end(); ++k) {
2348 double wPrev = l.
widthData.front().computeAt(sPrev);
2350 <<
"findWidthSplit section=" << section
2351 <<
" sectionStart=" << sectionStart
2352 <<
" sectionEnd=" << sectionEnd
2354 <<
" type=" << l.
type
2355 <<
" widthEntries=" << l.
widthData.size() <<
"\n"
2359 for (std::vector<OpenDriveWidth>::iterator it_w = l.
widthData.begin(); it_w != l.
widthData.end(); ++it_w) {
2360 double sEnd = (it_w + 1) != l.
widthData.end() ? (*(it_w + 1)).s : sectionEnd - sectionStart;
2361 double w = (*it_w).computeAt(sEnd);
2364 <<
" s=" << (*it_w).s
2365 <<
" a=" << (*it_w).a <<
" b=" << (*it_w).b <<
" c=" << (*it_w).c <<
" d=" << (*it_w).d
2368 const double changeDist = fabs(
myMinWidth - wPrev);
2371 double splitPos = sPrev + (sEnd - sPrev) / fabs(w - wPrev) * changeDist;
2372 double wSplit = (*it_w).computeAt(splitPos);
2374 std::cout <<
" candidate splitPos=" << splitPos <<
" w=" << wSplit <<
"\n";
2381 if (splitPos < sPrev) {
2383 std::cout <<
" aborting search splitPos=" << splitPos <<
" wSplit=" << wSplit <<
" sPrev=" << sPrev <<
" wPrev=" << wPrev <<
"\n";
2391 if (splitPos > sEnd) {
2393 std::cout <<
" aborting search splitPos=" << splitPos <<
" wSplit=" << wSplit <<
" sEnd=" << sEnd <<
" w=" << w <<
"\n";
2399 wSplit = (*it_w).computeAt(splitPos);
2401 std::cout <<
" refined splitPos=" << splitPos <<
" w=" << wSplit <<
"\n";
2404 splitPositions.push_back(sectionStart + splitPos);
2422 for (std::vector<OpenDriveLane>::iterator k = lanes.begin(); k != lanes.end(); ++k) {
2423 (*k).predecessor = (*k).id;
2441 for (std::vector<OpenDriveLane>::iterator k = lanes.begin(); k != lanes.end(); ++k) {
2444 #ifdef DEBUG_VARIABLE_WIDTHS
2446 <<
"recomputeWidths lane=" << l.
id
2447 <<
" type=" << l.
type
2448 <<
" start=" << start
2450 <<
" sectionStart=" << sectionStart
2451 <<
" sectionEnd=" << sectionEnd
2452 <<
" widthEntries=" << l.
widthData.size() <<
"\n"
2457 double sPrevAbs = sPrev + sectionStart;
2458 for (std::vector<OpenDriveWidth>::iterator it_w = l.
widthData.begin(); it_w != l.
widthData.end(); ++it_w) {
2459 double sEnd = (it_w + 1) != l.
widthData.end() ? (*(it_w + 1)).s : sectionEnd - sectionStart;
2460 double sEndAbs = sEnd + sectionStart;
2461 #ifdef DEBUG_VARIABLE_WIDTHS
2463 <<
" sPrev=" << sPrev <<
" sPrevAbs=" << sPrevAbs
2464 <<
" sEnd=" << sEnd <<
" sEndAbs=" << sEndAbs
2465 <<
" widthData s=" << (*it_w).s
2466 <<
" a=" << (*it_w).a
2467 <<
" b=" << (*it_w).b
2468 <<
" c=" << (*it_w).c
2469 <<
" d=" << (*it_w).d
2472 if (sPrevAbs <= start && sEndAbs >= start) {
2473 #ifdef DEBUG_VARIABLE_WIDTHS
2475 std::cout <<
" atStart=" << start <<
" pos=" << start - sectionStart <<
" w=" << (*it_w).computeAt(start - sectionStart) <<
"\n";
2478 l.
width =
MAX2(l.
width, (*it_w).computeAt(start - sectionStart));
2480 if (sPrevAbs <= end && sEndAbs >= end) {
2481 #ifdef DEBUG_VARIABLE_WIDTHS
2483 std::cout <<
" atEnd=" << end <<
" pos=" << end - sectionStart <<
" w=" << (*it_w).computeAt(end - sectionStart) <<
"\n";
2488 if (start <= sPrevAbs && end >= sPrevAbs) {
2489 #ifdef DEBUG_VARIABLE_WIDTHS
2491 std::cout <<
" atSPrev=" << sPrev <<
" w=" << (*it_w).computeAt(sPrev) <<
"\n";
2496 if (start <= sEndAbs && end >= sEndAbs) {
2497 #ifdef DEBUG_VARIABLE_WIDTHS
2499 std::cout <<
" atSEnd=" << sEnd <<
" w=" << (*it_w).computeAt(sEnd) <<
"\n";
2504 #ifdef DEBUG_VARIABLE_WIDTHS
2506 std::cout <<
" sPrev=" << sPrev <<
" sEnd=" << sEnd <<
" l.width=" << l.
width <<
"\n";