173 std::map<std::string, OpenDriveEdge*> edges;
176 std::vector<std::string> files = oc.
getStringVector(
"opendrive-files");
177 for (std::vector<std::string>::const_iterator file = files.begin(); file != files.end(); ++file) {
179 WRITE_ERROR(
"Could not open opendrive file '" + *file +
"'.");
182 handler.setFileName(*file);
188 std::map<std::string, OpenDriveEdge*> innerEdges, outerEdges;
189 for (std::map<std::string, OpenDriveEdge*>::iterator i = edges.begin(); i != edges.end(); ++i) {
190 if ((*i).second->isInner) {
191 innerEdges[(*i).first] = (*i).second;
193 outerEdges[(*i).first] = (*i).second;
208 std::map<std::string, Boundary> posMap;
209 std::map<std::string, std::string> edge2junction;
211 for (std::map<std::string, OpenDriveEdge*>::iterator i = innerEdges.begin(); i != innerEdges.end(); ++i) {
215 if (posMap.find(e->
junction) == posMap.end()) {
221 for (std::map<std::string, Boundary>::iterator i = posMap.begin(); i != posMap.end(); ++i) {
224 throw ProcessError(
"Could not add node '" + (*i).first +
"'.");
228 for (std::map<std::string, OpenDriveEdge*>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
230 for (std::vector<OpenDriveLink>::iterator j = e->
links.begin(); j != e->
links.end(); ++j) {
238 throw ProcessError(
"Could not build node '" + nid +
"'.");
245 if (edge2junction.find(l.
elementID) != edge2junction.end()) {
257 for (std::map<std::string, OpenDriveEdge*>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
259 for (std::vector<OpenDriveLink>::iterator j = e->
links.begin(); j != e->
links.end(); ++j) {
266 std::string id1 = e->
id;
271 std::string nid = id1 +
"." + id2;
276 throw ProcessError(
"Could not build node '" + nid +
"'.");
294 for (std::map<std::string, OpenDriveEdge*>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
296 if (e->
to != 0 && e->
from != 0) {
299 for (std::map<std::string, OpenDriveEdge*>::iterator j = innerEdges.begin(); j != innerEdges.end(); ++j) {
301 for (std::vector<OpenDriveLink>::iterator k = ie->
links.begin(); k != ie->
links.end(); ++k) {
307 std::string nid = edge2junction[ie->
id];
319 for (std::map<std::string, OpenDriveEdge*>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
321 if ((e->
from == 0 || e->
to == 0) && e->
geom.size() == 0) {
325 const std::string nid = e->
id +
".begin";
329 const std::string nid = e->
id +
".end";
338 double defaultSpeed = tc.
getSpeed(
"");
341 for (std::map<std::string, OpenDriveEdge*>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
343 if (e->
geom.size() == 0) {
347 bool lanesBuilt =
false;
363 double cF = length2D == 0 ? 1 : e->
length / length2D;
373 WRITE_WARNING(
"Edge '" + e->
id +
"' has to be split as it connects same junctions.")
376 const double minDist = oc.
getFloat(
"opendrive.curve-resolution");
387 double nextS = (j + 1)->s;
395 std::string
id = e->
id;
396 if (sFrom != e->
from || sTo != e->
to) {
401 #ifdef DEBUG_VARIABLE_WIDTHS 403 std::cout <<
" id=" <<
id <<
" sB=" << sB <<
" sE=" << sE <<
" geom=" << geom <<
"\n";
409 if ((*j).rightLaneNumber > 0) {
410 currRight =
new NBEdge(
"-" +
id, sFrom, sTo, (*j).rightType, defaultSpeed, (*j).rightLaneNumber, priorityR,
414 for (std::vector<OpenDriveLane>::const_iterator k = lanes.begin(); k != lanes.end(); ++k) {
415 std::map<int, int>::const_iterator lp = (*j).laneMap.find((*k).id);
416 if (lp != (*j).laneMap.end()) {
417 int sumoLaneIndex = lp->second;
441 if (prevRight != 0) {
443 for (std::map<int, int>::const_iterator k = connections.begin(); k != connections.end(); ++k) {
444 #ifdef DEBUG_CONNECTIONS 446 std::cout <<
"addCon1 from=" << prevRight->
getID() <<
"_" << (*k).first <<
" to=" << currRight->
getID() <<
"_" << (*k).second <<
"\n";
452 prevRight = currRight;
458 if ((*j).leftLaneNumber > 0) {
459 currLeft =
new NBEdge(
id, sTo, sFrom, (*j).leftType, defaultSpeed, (*j).leftLaneNumber, priorityL,
463 for (std::vector<OpenDriveLane>::const_iterator k = lanes.begin(); k != lanes.end(); ++k) {
464 std::map<int, int>::const_iterator lp = (*j).laneMap.find((*k).id);
465 if (lp != (*j).laneMap.end()) {
466 int sumoLaneIndex = lp->second;
491 std::map<int, int> connections = (*j).getInnerConnections(
OPENDRIVE_TAG_LEFT, *(j - 1));
492 for (std::map<int, int>::const_iterator k = connections.begin(); k != connections.end(); ++k) {
493 #ifdef DEBUG_CONNECTIONS 495 std::cout <<
"addCon2 from=" << currLeft->
getID() <<
"_" << (*k).first <<
" to=" << prevLeft->
getID() <<
"_" << (*k).second <<
"\n";
519 for (std::map<std::string, OpenDriveEdge*>::iterator i = edges.begin(); i != edges.end(); ++i) {
523 std::vector<Connection> connections2;
524 for (std::map<std::string, OpenDriveEdge*>::iterator j = edges.begin(); j != edges.end(); ++j) {
525 const std::set<Connection>& conns = (*j).second->connections;
527 for (std::set<Connection>::const_iterator i = conns.begin(); i != conns.end(); ++i) {
528 if (innerEdges.find((*i).fromEdge) != innerEdges.end()) {
532 if (innerEdges.find((*i).toEdge) != innerEdges.end()) {
533 std::set<Connection> seen;
536 connections2.push_back(*i);
541 for (std::vector<Connection>::const_iterator i = connections2.begin(); i != connections2.end(); ++i) {
542 std::string fromEdge = (*i).fromEdge;
543 if (edges.find(fromEdge) == edges.end()) {
544 WRITE_WARNING(
"While setting connections: from-edge '" + fromEdge +
"' is not known.");
548 int fromLane = (*i).fromLane;
549 bool fromLast = ((*i).fromCP ==
OPENDRIVE_CP_END) ^ ((*i).fromLane > 0 && !(*i).all);
552 std::string toEdge = (*i).toEdge;
553 if (edges.find(toEdge) == edges.end()) {
554 WRITE_WARNING(
"While setting connections: to-edge '" + toEdge +
"' is not known.");
559 int toLane = (*i).toLane;
580 WRITE_WARNING(
"Could not find fromEdge representation of '" + fromEdge +
"' in connection '" + (*i).origID +
"'.");
583 WRITE_WARNING(
"Could not find fromEdge representation of '" + toEdge +
"' in connection '" + (*i).origID +
"'.");
585 if (from == 0 || to == 0) {
589 #ifdef DEBUG_CONNECTIONS 591 std::cout <<
"addCon3 from=" << from->
getID() <<
"_" << fromLane <<
" to=" << to->
getID() <<
"_" << toLane <<
"\n";
596 if ((*i).origID !=
"" && saveOrigIDs) {
599 for (std::vector<NBEdge::Connection>::iterator k = cons.begin(); k != cons.end(); ++k) {
600 if ((*k).fromLane == fromLane && (*k).toEdge == to && (*k).toLane == toLane) {
612 std::map<std::string, std::string> tlsControlled;
613 for (std::map<std::string, OpenDriveEdge*>::iterator i = edges.begin(); i != edges.end(); ++i) {
615 for (std::vector<OpenDriveSignal>::const_iterator j = e->
signals.begin(); j != e->
signals.end(); ++j) {
616 if ((*j).type !=
"1000001") {
619 std::vector<OpenDriveLaneSection>::iterator k = e->
laneSections.begin();
622 if ((*j).s > (*k).s && (*j).s <= (*(k + 1)).s) {
632 std::string
id = (*k).sumoID;
636 std::string fromID, toID;
637 for (std::vector<OpenDriveLink>::const_iterator l = e->
links.begin(); l != e->
links.end(); ++l) {
645 if ((*j).orientation < 0) {
646 fromID =
"-" + fromID;
650 if ((*j).orientation > 0) {
651 fromID =
"-" + fromID;
663 id = fromID +
"->" + toID;
665 WRITE_WARNING(
"Found a traffic light signal on an unknown edge (original edge id='" + e->
id +
"').");
669 if ((*j).orientation > 0) {
673 tlsControlled[id] = (*j).name;
677 for (std::map<std::string, std::string>::iterator i = tlsControlled.begin(); i != tlsControlled.end(); ++i) {
678 std::string
id = (*i).first;
679 if (
id.find(
"->") != std::string::npos) {
680 id =
id.substr(0,
id.find(
"->"));
684 WRITE_WARNING(
"Could not find edge '" +
id +
"' while building its traffic light.");
706 for (std::map<std::string, OpenDriveEdge*>::iterator i = edges.begin(); i != edges.end(); ++i) {
737 const std::set<Connection>& conts = dest->
connections;
738 for (std::set<Connection>::const_iterator i = conts.begin(); i != conts.end(); ++i) {
739 if (innerEdges.find((*i).toEdge) != innerEdges.end()) {
740 std::vector<Connection> t;
741 if (seen.count(*i) == 0) {
743 for (std::vector<Connection>::const_iterator j = t.begin(); j != t.end(); ++j) {
756 if ((*i).fromLane == c.
toLane) {
773 for (std::vector<OpenDriveLink>::iterator i = e.
links.begin(); i != e.
links.end(); ++i) {
781 std::string edgeID = e.
id;
784 const std::map<int, int>& laneMap = laneSection.
laneMap;
785 #ifdef DEBUG_CONNECTIONS 787 std::cout <<
"edge=" << e.
id <<
" eType=" << l.
elementType <<
" lType=" << l.
linkType <<
" connectedEdge=" << connectedEdge <<
" laneSection=" << laneSection.
s <<
" map:\n";
793 for (std::vector<OpenDriveLane>::const_iterator j = lanes.begin(); j != lanes.end(); ++j) {
810 if (edges.find(c.
fromEdge) == edges.end()) {
811 WRITE_ERROR(
"While setting connections: incoming road '" + c.
fromEdge +
"' is not known.");
815 #ifdef DEBUG_CONNECTIONS 823 for (std::vector<OpenDriveLane>::const_iterator j = lanes.begin(); j != lanes.end(); ++j) {
840 if (edges.find(c.
fromEdge) == edges.end()) {
841 WRITE_ERROR(
"While setting connections: incoming road '" + c.
fromEdge +
"' is not known.");
845 #ifdef DEBUG_CONNECTIONS 847 std::cout <<
"insertConLeft from=" << src->
id <<
"_" << c.
fromLane <<
" to=" << c.
toEdge <<
"_" << c.
toLane <<
"\n";
870 if (!nc.
insert(
id, pos)) {
884 throw ProcessError(
"Could not find node '" + nodeID +
"'.");
887 if (e.
to != 0 && e.
to != n) {
903 const double res = oc.
getFloat(
"opendrive.curve-resolution");
904 for (std::map<std::string, OpenDriveEdge*>::iterator i = edges.begin(); i != edges.end(); ++i) {
907 for (std::vector<OpenDriveGeometry>::iterator j = e.
geometries.begin(); j != e.
geometries.end(); ++j) {
935 if (!e.
geom.back().almostSame(geom.front())) {
936 const int index = (int)(j - e.
geometries.begin());
942 for (PositionVector::iterator k = geom.begin(); k != geom.end(); ++k) {
947 if (oc.
exists(
"geometry.min-dist") && !oc.
isDefault(
"geometry.min-dist")) {
951 WRITE_ERROR(
"Unable to project coordinates for edge '" + e.
id +
"'.");
956 for (std::vector<OpenDriveElevation>::iterator j = e.
elevations.begin(); j != e.
elevations.end(); ++j) {
958 const double sNext = (j + 1) == e.
elevations.end() ? std::numeric_limits<double>::max() : (*(j + 1)).s;
959 while (k < (
int)e.
geom.size() && pos < sNext) {
964 if (k < (
int)e.
geom.size()) {
967 pos += e.
geom[k - 1].distanceTo2D(e.
geom[k]);
974 for (std::vector<OpenDriveLaneOffset>::iterator j = e.
offsets.begin(); j != e.
offsets.end(); ++j) {
990 for (std::vector<OpenDriveLaneOffset>::iterator j = e.
offsets.begin(); j != e.
offsets.end(); ++j) {
992 const double sNext = (j + 1) == e.
offsets.end() ? std::numeric_limits<double>::max() : (*(j + 1)).s;
993 while (k < (
int)e.
geom.size() && pos < sNext) {
1002 geom2.push_back(tmp[k]);
1004 geom2.push_back(e.
geom[k]);
1007 geom2.push_back(e.
geom[k]);
1010 if (k < (
int)e.
geom.size()) {
1013 pos += e.
geom[k - 1].distanceTo2D(e.
geom[k]);
1017 assert(e.
geom.size() == geom2.size());
1027 for (std::map<std::string, OpenDriveEdge*>::iterator i = edges.begin(); i != edges.end(); ++i) {
1029 #ifdef DEBUG_VARIABLE_SPEED 1032 std::cout <<
"revisitLaneSections e=" << e.
id <<
"\n";
1035 std::vector<OpenDriveLaneSection>& laneSections = e.
laneSections;
1037 std::vector<OpenDriveLaneSection> newSections;
1038 for (std::vector<OpenDriveLaneSection>::iterator j = laneSections.begin(); j != laneSections.end(); ++j) {
1039 std::vector<OpenDriveLaneSection> splitSections;
1040 bool splitBySpeed = (*j).buildSpeedChanges(tc, splitSections);
1041 if (!splitBySpeed) {
1042 newSections.push_back(*j);
1044 std::copy(splitSections.begin(), splitSections.end(), back_inserter(newSections));
1053 for (std::vector<OpenDriveLaneSection>::const_iterator j = laneSections.begin(); j != laneSections.end() && sorted; ++j) {
1054 if ((*j).s <= lastS) {
1060 WRITE_WARNING(
"The sections of edge '" + e.
id +
"' are not sorted properly.");
1066 for (std::vector<OpenDriveLaneSection>::iterator j = laneSections.begin(); j != laneSections.end();) {
1067 bool simlarToLast = fabs((*j).s - lastS) <
POSITION_EPS;
1070 WRITE_WARNING(
"Almost duplicate s-value '" +
toString(lastS) +
"' for lane sections occured at edge '" + e.
id +
"'; second entry was removed.");
1071 j = laneSections.erase(j);
1076 #ifdef DEBUG_VARIABLE_SPEED 1097 double curveStart = g.
params[0];
1098 double curveEnd = g.
params[1];
1100 double cDot = (curveEnd - curveStart) / g.
length;
1101 if (cDot == 0 || g.
length == 0) {
1105 double sStart = curveStart / cDot;
1106 double sEnd = curveEnd / cDot;
1112 odrSpiral(sStart, cDot, &x, &y, &tStart);
1113 for (s = sStart; s <= sEnd; s += resolution) {
1124 assert(ret.size() >= 2);
1125 assert(ret[0] != ret[1]);
1128 ret.
add(ret.front() * -1);
1134 << std::setprecision(4)
1135 <<
"edge=" << e.
id <<
" s=" << g.
s 1136 <<
" cStart=" << curveStart
1137 <<
" cEnd=" << curveEnd
1139 <<
" sStart=" << sStart
1143 <<
"\n beforeShift=" << ret1
1144 <<
"\n beforeRot=" << ret2
1148 ret.
add(g.
x, g.
y, 0);
1149 }
catch (
const std::runtime_error&
error) {
1150 WRITE_WARNING(
"Could not compute spiral geometry for edge '" + e.
id +
"' (" + error.what() +
").");
1162 double centerX = g.
x;
1163 double centerY = g.
y;
1165 double curvature = g.
params[0];
1166 double radius = 1. / curvature;
1171 double startX = g.
x;
1172 double startY = g.
y;
1173 double geo_posS = g.
s;
1174 double geo_posE = g.
s;
1177 geo_posE += resolution;
1178 if (geo_posE - g.
s > g.
length) {
1181 if (geo_posE - g.
s > g.
length) {
1184 calcPointOnCurve(&endX, &endY, centerX, centerY, radius, geo_posE - geo_posS);
1186 dist += (geo_posE - geo_posS);
1188 ret.push_back(
Position(startX, startY));
1192 geo_posS = geo_posE;
1194 if (geo_posE - (g.
s + g.
length) < 0.001 && geo_posE - (g.
s + g.
length) > -0.001) {
1205 const double s = sin(g.
hdg);
1206 const double c = cos(g.
hdg);
1208 for (
double off = 0; off < g.
length + 2.; off += resolution) {
1211 double xnew = x * c - y * s;
1212 double ynew = x * s + y * c;
1213 ret.push_back(
Position(g.
x + xnew, g.
y + ynew));
1222 const double s = sin(g.
hdg);
1223 const double c = cos(g.
hdg);
1225 const double pStep = pMax / ceil(g.
length / resolution);
1227 for (
double p = 0; p <= pMax + pStep; p += pStep) {
1230 double xnew = x * c - y * s;
1231 double ynew = x * s + y * c;
1232 ret.push_back(
Position(g.
x + xnew, g.
y + ynew));
1240 double normx = 1.0f;
1241 double normy = 0.0f;
1242 double x2 = normx * cos(hdg) - normy * sin(hdg);
1243 double y2 = normx * sin(hdg) + normy * cos(hdg);
1244 normx = x2 * length;
1245 normy = y2 * length;
1246 return Position(start.
x() + normx, start.
y() + normy);
1256 if (ad_radius > 0) {
1263 normX = normX * cos(ad_hdg) + normY * sin(ad_hdg);
1264 normY = tmpX * sin(ad_hdg) + normY * cos(ad_hdg);
1267 normX = turn * normY;
1268 normY = -turn * tmpX;
1270 normX = fabs(ad_radius) * normX;
1271 normY = fabs(ad_radius) * normY;
1280 double ad_r,
double ad_length) {
1281 double rotAngle = ad_length / fabs(ad_r);
1282 double vx = *ad_x - ad_centerX;
1283 double vy = *ad_y - ad_centerY;
1293 vx = vx * cos(rotAngle) + turn * vy * sin(rotAngle);
1294 vy = -1 * turn * tmpx * sin(rotAngle) + vy * cos(rotAngle);
1295 *ad_x = vx + ad_centerX;
1296 *ad_y = vy + ad_centerY;
1313 bool singleType =
true;
1314 std::vector<std::string> types;
1316 for (std::vector<OpenDriveLane>::const_reverse_iterator i = dirLanesR.rbegin(); i != dirLanesR.rend(); ++i) {
1318 laneMap[(*i).id] = sumoLane++;
1319 types.push_back((*i).type);
1320 if (types.front() != types.back()) {
1331 for (std::vector<OpenDriveLane>::const_iterator i = dirLanesL.begin(); i != dirLanesL.end(); ++i) {
1333 laneMap[(*i).id] = sumoLane++;
1334 types.push_back((*i).type);
1335 if (types.front() != types.back()) {
1347 std::map<int, int> ret;
1348 const std::vector<OpenDriveLane>& dirLanes =
lanesByDir.find(dir)->second;
1349 for (std::vector<OpenDriveLane>::const_reverse_iterator i = dirLanes.rbegin(); i != dirLanes.rend(); ++i) {
1350 std::map<int, int>::const_iterator toP =
laneMap.find((*i).id);
1355 int to = (*toP).second;
1358 from = (*i).predecessor;
1361 std::map<int, int>::const_iterator fromP = prev.
laneMap.find(from);
1362 if (fromP != prev.
laneMap.end()) {
1363 from = (*fromP).second;
1369 if (ret.find(from) != ret.end()) {
1373 std::swap(from, to);
1392 if (i != l.
speeds.end()) {
1393 l.
speed = (*i).second;
1400 if (i != l.
speeds.end()) {
1401 l.
speed = (*i).second;
1410 std::set<double> speedChangePositions;
1413 for (std::vector<std::pair<double, double> >::const_iterator l = (*k).speeds.begin(); l != (*k).speeds.end(); ++l) {
1414 speedChangePositions.insert((*l).first);
1415 if ((*l).first == 0) {
1416 (*k).speed = (*l).second;
1421 for (std::vector<std::pair<double, double> >::const_iterator l = (*k).speeds.begin(); l != (*k).speeds.end(); ++l) {
1422 speedChangePositions.insert((*l).first);
1423 if ((*l).first == 0) {
1424 (*k).speed = (*l).second;
1429 if (speedChangePositions.size() == 0) {
1432 if (*speedChangePositions.begin() > 0) {
1433 speedChangePositions.insert(0);
1435 #ifdef DEBUG_VARIABLE_SPEED 1437 <<
" buildSpeedChanges sectionStart=" <<
s 1438 <<
" speedChangePositions=" <<
joinToString(speedChangePositions,
", ")
1441 for (std::set<double>::iterator i = speedChangePositions.begin(); i != speedChangePositions.end(); ++i) {
1442 if (i == speedChangePositions.begin()) {
1443 newSections.push_back(*
this);
1449 for (
int i = 0; i != (int)newSections.size(); ++i) {
1452 for (std::map<
OpenDriveXMLTag, std::vector<OpenDriveLane> >::iterator k = lanesByDir.begin(); k != lanesByDir.end(); ++k) {
1453 std::vector<OpenDriveLane>& lanes = (*k).second;
1454 for (
int j = 0; j != (int)lanes.size(); ++j) {
1460 l.
speed = newSections[i - 1].lanesByDir[(*k).first][j].speed;
1479 for (std::vector<OpenDriveSignal>::const_iterator i = signals.begin(); i != signals.end(); ++i) {
1481 if ((*i).type ==
"301" || (*i).type ==
"306") {
1484 if ((*i).type ==
"205" ) {
1521 if (majorVersion != 1 || minorVersion != 2) {
1587 std::vector<double> vals;
1593 std::vector<double> vals;
1600 std::vector<double> vals;
1606 std::vector<double> vals;
1615 std::vector<double> vals;
1625 if (pRange ==
"normalized") {
1626 vals.push_back(1.0);
1627 }
else if (pRange ==
"arcLength") {
1628 vals.push_back(-1.0);
1631 vals.push_back(1.0);
1703 WRITE_ERROR(
"In laneLink-element: incoming road '" + c.fromEdge +
"' is not known.");
1719 l.width =
MAX2(l.width, a);
1721 #ifdef DEBUG_VARIABLE_WIDTHS 1728 <<
" type=" << l.type
1729 <<
" width=" << l.width
1735 <<
" entries=" << l.widthData.size()
1749 if (!unit.empty()) {
1751 if (unit ==
"km/h") {
1754 if (unit ==
"mph") {
1755 speed *= 1.609344 / 3.6;
1808 const std::string& elementID,
1809 const std::string& contactPoint) {
1812 if (elementType ==
"road") {
1814 }
else if (elementType ==
"junction") {
1818 if (contactPoint ==
"start") {
1820 }
else if (contactPoint ==
"end") {
1861 std::vector<OpenDriveLaneSection> newSections;
1862 #ifdef DEBUG_VARIABLE_WIDTHS 1865 std::cout <<
"splitMinWidths e=" << e->
id <<
" sections=" << e->
laneSections.size() <<
"\n";
1870 std::vector<double> splitPositions;
1871 const double sectionEnd = (j + 1) == e->
laneSections.end() ? e->
length : (*(j + 1)).s;
1872 const int section = (int)(j - e->
laneSections.begin());
1873 #ifdef DEBUG_VARIABLE_WIDTHS 1875 std::cout <<
" findWidthSplit section=" << section <<
" sectionStart=" << sec.
s <<
" sectionOrigStart=" << sec.
sOrig <<
" sectionEnd=" << sectionEnd <<
"\n";
1884 newSections.push_back(sec);
1885 std::sort(splitPositions.begin(), splitPositions.end());
1887 double prevSplit = sec.
s;
1888 for (std::vector<double>::iterator it = splitPositions.begin(); it != splitPositions.end();) {
1889 if ((*it) - prevSplit < minDist || sectionEnd - (*it) < minDist) {
1891 #ifdef DEBUG_VARIABLE_WIDTHS 1893 std::cout <<
" skip close split=" << (*it) <<
" prevSplit=" << prevSplit <<
"\n";
1896 it = splitPositions.erase(it);
1897 }
else if ((*it) < sec.
s) {
1899 #ifdef DEBUG_VARIABLE_WIDTHS 1901 std::cout <<
" skip early split=" << (*it) <<
" s=" << sec.
s <<
"\n";
1904 it = splitPositions.erase(it);
1911 if (splitPositions.size() > 0) {
1912 #ifdef DEBUG_VARIABLE_WIDTHS 1914 std::cout <<
" road=" << e->
id <<
" splitMinWidths section=" << section
1915 <<
" start=" << sec.
s 1916 <<
" origStart=" << sec.
sOrig 1917 <<
" end=" << sectionEnd <<
" minDist=" << minDist
1918 <<
" splitPositions=" <<
toString(splitPositions) <<
"\n";
1921 #ifdef DEBUG_VARIABLE_WIDTHS 1923 std::cout <<
"first section...\n";
1927 for (std::vector<double>::iterator it = splitPositions.begin(); it != splitPositions.end(); ++it) {
1930 #ifdef DEBUG_VARIABLE_WIDTHS 1932 std::cout <<
"splitAt " << secNew.
s <<
"\n";
1935 newSections.push_back(secNew);
1942 double end = (it + 1) == splitPositions.end() ? sectionEnd : *(it + 1);
1954 int section,
double sectionStart,
double sectionEnd,
1955 std::vector<double>& splitPositions) {
1957 for (std::vector<OpenDriveLane>::iterator k = lanes.begin(); k != lanes.end(); ++k) {
1962 double wPrev = l.
widthData.front().computeAt(sPrev);
1964 <<
"findWidthSplit section=" << section
1965 <<
" sectionStart=" << sectionStart
1966 <<
" sectionEnd=" << sectionEnd
1968 <<
" type=" << l.
type 1969 <<
" widthEntries=" << l.
widthData.size() <<
"\n" 1973 for (std::vector<OpenDriveWidth>::iterator it_w = l.
widthData.begin(); it_w != l.
widthData.end(); ++it_w) {
1974 double sEnd = (it_w + 1) != l.
widthData.end() ? (*(it_w + 1)).s : sectionEnd - sectionStart;
1975 double w = (*it_w).computeAt(sEnd);
1978 <<
" s=" << (*it_w).s
1979 <<
" a=" << (*it_w).a <<
" b=" << (*it_w).b <<
" c=" << (*it_w).c <<
" d=" << (*it_w).d
1982 const double changeDist = fabs(
myMinWidth - wPrev);
1985 double splitPos = sPrev + (sEnd - sPrev) / fabs(w - wPrev) * changeDist;
1986 double wSplit = (*it_w).computeAt(splitPos);
1988 std::cout <<
" candidate splitPos=" << splitPos <<
" w=" << wSplit <<
"\n";
1995 if (splitPos < sPrev) {
1997 std::cout <<
" aborting search splitPos=" << splitPos <<
" wSplit=" << wSplit <<
" sPrev=" << sPrev <<
" wPrev=" << wPrev <<
"\n";
2005 if (splitPos > sEnd) {
2007 std::cout <<
" aborting search splitPos=" << splitPos <<
" wSplit=" << wSplit <<
" sEnd=" << sEnd <<
" w=" << w <<
"\n";
2013 wSplit = (*it_w).computeAt(splitPos);
2015 std::cout <<
" refined splitPos=" << splitPos <<
" w=" << wSplit <<
"\n";
2018 splitPositions.push_back(sectionStart + splitPos);
2036 for (std::vector<OpenDriveLane>::iterator k = lanes.begin(); k != lanes.end(); ++k) {
2037 (*k).predecessor = (*k).id;
2055 for (std::vector<OpenDriveLane>::iterator k = lanes.begin(); k != lanes.end(); ++k) {
2058 #ifdef DEBUG_VARIABLE_WIDTHS 2060 <<
"recomputeWidths lane=" << l.
id 2061 <<
" type=" << l.
type 2062 <<
" start=" << start
2064 <<
" sectionStart=" << sectionStart
2065 <<
" sectionEnd=" << sectionEnd
2066 <<
" widthEntries=" << l.
widthData.size() <<
"\n" 2071 double sPrevAbs = sPrev + sectionStart;
2072 for (std::vector<OpenDriveWidth>::iterator it_w = l.
widthData.begin(); it_w != l.
widthData.end(); ++it_w) {
2073 double sEnd = (it_w + 1) != l.
widthData.end() ? (*(it_w + 1)).s : sectionEnd - sectionStart;
2074 double sEndAbs = sEnd + sectionStart;
2075 #ifdef DEBUG_VARIABLE_WIDTHS 2077 <<
" sPrev=" << sPrev <<
" sPrevAbs=" << sPrevAbs
2078 <<
" sEnd=" << sEnd <<
" sEndAbs=" << sEndAbs
2079 <<
" widthData s=" << (*it_w).s
2080 <<
" a=" << (*it_w).a
2081 <<
" b=" << (*it_w).b
2082 <<
" c=" << (*it_w).c
2083 <<
" d=" << (*it_w).d
2086 if (sPrevAbs <= start && sEndAbs >= start) {
2087 #ifdef DEBUG_VARIABLE_WIDTHS 2089 std::cout <<
" atStart=" << start <<
" pos=" << start - sectionStart <<
" w=" << (*it_w).computeAt(start - sectionStart) <<
"\n";
2092 l.
width =
MAX2(l.
width, (*it_w).computeAt(start - sectionStart));
2094 if (sPrevAbs <= end && sEndAbs >= end) {
2095 #ifdef DEBUG_VARIABLE_WIDTHS 2097 std::cout <<
" atEnd=" << end <<
" pos=" << end - sectionStart <<
" w=" << (*it_w).computeAt(end - sectionStart) <<
"\n";
2102 if (start <= sPrevAbs && end >= sPrevAbs) {
2103 #ifdef DEBUG_VARIABLE_WIDTHS 2105 std::cout <<
" atSPrev=" << sPrev <<
" w=" << (*it_w).computeAt(sPrev) <<
"\n";
2110 if (start <= sEndAbs && end >= sEndAbs) {
2111 #ifdef DEBUG_VARIABLE_WIDTHS 2113 std::cout <<
" atSEnd=" << sEnd <<
" w=" << (*it_w).computeAt(sEnd) <<
"\n";
2118 #ifdef DEBUG_VARIABLE_WIDTHS 2120 std::cout <<
" sPrev=" << sPrev <<
" sEnd=" << sEnd <<
" l.width=" << l.
width <<
"\n";
std::map< std::string, OpenDriveEdge * > & myEdges
ContactPoint contactPoint
bool gDebugFlag1
global utility flags for debugging
NBNode * retrieve(const std::string &id) const
Returns the node with the given name.
std::vector< int > myElementStack
std::string rightType
the composite type built from all used lane types
double length2D() const
Returns the length.
OpenDriveLaneSection(double sArg)
Constructor.
double sOrig
The original starting offset of this lane section (differs from s if the section had to be split) ...
double getSpeed(const std::string &type) const
Returns the maximal velocity for the given type [m/s].
static PositionVector geomFromLine(const OpenDriveEdge &e, const OpenDriveGeometry &g)
static StringBijection< int >::Entry openDriveAttrs[]
The names of openDrive-XML attributes (for passing to GenericSAXHandler)
NBTypeCont & getTypeCont()
Returns a reference to the type container.
OpenDriveLaneSection buildLaneSection(double startPos)
std::vector< OpenDriveWidth > widthData
void addLink(LinkType lt, const std::string &elementType, const std::string &elementID, const std::string &contactPoint)
Representation of an OpenDrive link.
static PositionVector geomFromArc(const OpenDriveEdge &e, const OpenDriveGeometry &g, double resolution)
static bool isReadable(std::string path)
Checks whether the given file is readable.
std::string junction
The id of the junction the edge belongs to.
std::vector< OpenDriveElevation > elevations
static void calculateCurveCenter(double *ad_x, double *ad_y, double ad_radius, double ad_hdg)
int indexOfClosest(const Position &p) const
index of the closest position to p
GeometryType
OpenDrive geometry type enumeration.
int rightLaneNumber
The number of lanes on the right and on the left side, respectively.
static void computeShapes(std::map< std::string, OpenDriveEdge *> &edges)
Computes a polygon representation of each edge's geometry.
double distanceTo2D(const Position &p2) const
returns the euclidean distance in the x-y-plane
int gPrecision
the precision for floating point outputs
Position positionAtOffset2D(double pos, double lateralOffset=0) const
Returns the position at the given length.
Representation of a lane section.
const std::string & getFileName() const
returns the current file name
double y() const
Returns the y-position.
int SVCPermissions
bitset where each bit declares whether a certain SVC may use this edge/lane
The representation of a single edge during network building.
NIImporter_OpenDrive(const NBTypeCont &tc, std::map< std::string, OpenDriveEdge *> &edges)
Constructor.
Representation of an openDrive "link".
double x() const
Returns the x-position.
The base class for traffic light logic definitions.
static const double UNSPECIFIED_OFFSET
unspecified lane offset
ContactPoint myCurrentContactPoint
void odrSpiral(double s, double cDot, double *x, double *y, double *t)
PositionVector reverse() const
reverse position vector
std::map< OpenDriveXMLTag, std::vector< OpenDriveLane > > lanesByDir
The lanes, sorted by their direction.
static bool transformCoordinates(PositionVector &from, bool includeInBoundary=true, GeoConvHelper *from_srs=0)
static PositionVector geomFromPoly(const OpenDriveEdge &e, const OpenDriveGeometry &g, double resolution)
static void calcPointOnCurve(double *ad_x, double *ad_y, double ad_centerX, double ad_centerY, double ad_r, double ad_length)
double length
The length of the edge.
bool getShallBeDiscarded(const std::string &type) const
Returns the information whether edges of this type shall be discarded.
std::set< Connection > connections
bool getBool(const std::string &name) const
Returns the boolean-value of the named option (only for Option_Bool)
Poly3 OpenDriveElevation
LaneOffset has the same fields as Elevation.
Representation of a signal.
const std::string & getID() const
Returns the id.
bool isDefault(const std::string &name) const
Returns the information whether the named option has still the default value.
Lane & getLaneStruct(int lane)
static bool runParser(GenericSAXHandler &handler, const std::string &file, const bool isNet=false)
Runs the given handler on the given file; returns if everything's ok.
friend bool operator<(const Connection &c1, const Connection &c2)
virtual bool hasAttribute(int id) const =0
Returns the information whether the named (by its enum-value) attribute is within the current list...
#define UNUSED_PARAMETER(x)
static const double UNSPECIFIED_WIDTH
unspecified lane width
OpenDriveEdge myCurrentEdge
A class that stores a 2D geometrical boundary.
static NBNode * getOrBuildNode(const std::string &id, const Position &pos, NBNodeCont &nc)
Builds a node or returns the already built.
void error(const XERCES_CPP_NAMESPACE::SAXParseException &exception)
Handler for XML-errors.
#define WRITE_WARNING(msg)
The connection was computed and validated.
static OptionsCont & getOptions()
Retrieves the options.
double getWidth(const std::string &type) const
Returns the lane width for the given type [m].
static std::string revertID(const std::string &id)
static void findWidthSplit(const NBTypeCont &tc, std::vector< OpenDriveLane > &lanes, int section, double sectionStart, double sectionEnd, std::vector< double > &splitPositions)
std::string myCurrentConnectingRoad
Representation of a lane.
OpenDriveXMLTag myCurrentLaneDirection
An (internal) definition of a single lane of an edge.
SVCPermissions permissions
List of vehicle types that are allowed on this lane.
bool isTLControlled() const
Returns whether this node is controlled by any tls.
std::vector< OpenDriveLink > links
static double naviDegree(const double angle)
A handler which converts occuring elements and attributes into enums.
int getPriority(OpenDriveXMLTag dir) const
Returns the edge's priority, regarding the direction.
void removeDoublePoints(double minDist=POSITION_EPS, bool assertLength=false)
Removes positions if too near.
bool knows(const std::string &type) const
Returns whether the named type is in the container.
static PositionVector geomFromSpiral(const OpenDriveEdge &e, const OpenDriveGeometry &g, double resolution)
Poly3 OpenDriveLaneOffset
static void setStraightConnections(std::vector< OpenDriveLane > &lanes)
bool insert(NBEdge *edge, bool ignorePrunning=false)
Adds an edge to the dictionary.
std::string toString(const T &t, std::streamsize accuracy=gPrecision)
std::string type
The lane's type.
bool isUsableFileList(const std::string &name) const
Checks whether the named option is usable as a file list (with at least a single file) ...
Encapsulated SAX-Attributes.
static StringBijection< TrafficLightType > TrafficLightTypes
traffic light types
T get(int attr, const char *objectid, bool &ok, bool report=true) const
Tries to read given attribute assuming it is an int.
static Position calculateStraightEndPoint(double hdg, double length, const Position &start)
static void recomputeWidths(OpenDriveLaneSection &sec, double start, double end, double sectionStart, double sectionEnd)
A point in 2D or 3D with translation and scaling methods.
NBEdgeCont & getEdgeCont()
std::string id
The id of the edge.
static void loadNetwork(const OptionsCont &oc, NBNetBuilder &nb)
Loads content of the optionally given SUMO file.
bool buildSpeedChanges(const NBTypeCont &tc, std::vector< OpenDriveLaneSection > &newSections)
static void splitMinWidths(OpenDriveEdge *e, const NBTypeCont &tc, double minDist)
const NBTypeCont & myTypeContainer
T get(const std::string &str) const
std::vector< OpenDriveLaneOffset > offsets
bool myConnectionWasEmpty
double speed
The lane's speed (set in post-processing)
bool exists(const std::string &name) const
Returns the information whether the named option is known.
std::string myCurrentJunctionID
std::vector< OpenDriveLaneSection > laneSections
std::map< int, int > laneMap
A mapping from OpenDrive to SUMO-index (the first is signed, the second unsigned) ...
std::vector< std::string > getStringVector(const std::string &name) const
Returns the list of string-vector-value of the named option (only for Option_String) ...
#define PROGRESS_BEGIN_MESSAGE(msg)
void setParameter(const std::string &key, const std::string &value)
Sets a parameter.
void myStartElement(int element, const SUMOSAXAttributes &attrs)
Called on the opening of a tag;.
int insertAtClosest(const Position &p)
inserts p between the two closest positions and returns the insertion index
OpenDriveXMLTag
Numbers representing openDrive-XML - element names.
static bool myImportWidths
The connection was given by the user.
double speed
The speed allowed on this lane.
double width
This lane's width.
double getFloat(const std::string &name) const
Returns the double-value of the named option (only for Option_Float)
void move2side(double amount)
move position vector to side using certain ammount
static PositionVector geomFromParamPoly(const OpenDriveEdge &e, const OpenDriveGeometry &g, double resolution)
static StringBijection< int >::Entry openDriveTags[]
The names of openDrive-XML elements (for passing to GenericSAXHandler)
static bool myImportAllTypes
void buildLaneMapping(const NBTypeCont &tc)
Build the mapping from OpenDrive to SUMO lanes.
std::vector< OpenDriveSignal > signals
LinkType
OpenDrive link type enumeration.
PositionVector getSubpart2D(double beginOffset, double endOffset) const
get subpart of a position vector in two dimensions (Z is ignored)
void rotate2D(double angle)
static void revisitLaneSections(const NBTypeCont &tc, std::map< std::string, OpenDriveEdge *> &edges)
Rechecks lane sections of the given edges.
T getOpt(int attr, const char *objectid, bool &ok, T defaultValue, bool report=true) const
Tries to read given attribute assuming it is an int.
void addTrafficLight(NBTrafficLightDefinition *tlDef)
Adds a traffic light to the list of traffic lights that control this node.
~NIImporter_OpenDrive()
Destructor.
bool addLane2LaneConnection(int fromLane, NBEdge *dest, int toLane, Lane2LaneInfoType type, bool mayUseSameDestination=false, bool mayDefinitelyPass=false, bool keepClear=true, double contPos=UNSPECIFIED_CONTPOS, double visibility=UNSPECIFIED_VISIBILITY_DISTANCE, double speed=UNSPECIFIED_SPEED, const PositionVector &customShape=PositionVector::EMPTY)
Adds a connection between the specified this edge's lane and an approached one.
std::map< int, int > getInnerConnections(OpenDriveXMLTag dir, const OpenDriveLaneSection &prev)
Returns the links from the previous to this lane section.
const std::vector< Connection > & getConnections() const
Returns the connections.
NBNodeCont & getNodeCont()
Returns a reference to the node container.
Instance responsible for building networks.
const std::set< NBTrafficLightDefinition * > & getControllingTLS() const
Returns the traffic lights that were assigned to this node (The set of tls that control this node) ...
Representation of an OpenDrive geometry part.
NBEdge * retrieve(const std::string &id, bool retrieveExtracted=false) const
Returns the edge that has the given id.
A storage for options typed value containers)
bool insert(const std::string &id, const Position &position, NBDistrict *district=0)
Inserts a node into the map.
NBTrafficLightLogicCont & getTLLogicCont()
Returns a reference to the traffic light logics container.
const std::string SUMO_PARAM_ORIGID
static void setEdgeLinks2(OpenDriveEdge &e, const std::map< std::string, OpenDriveEdge *> &edges)
std::vector< OpenDriveGeometry > geometries
Represents a single node (junction) during network building.
void addGeometryShape(GeometryType type, const std::vector< double > &vals)
A class for sorting lane sections by their s-value.
bool insert(NBTrafficLightDefinition *logic, bool forceInsert=false)
Adds a logic definition to the dictionary.
double computeAt(double pos) const
static void setNodeSecure(NBNodeCont &nc, OpenDriveEdge &e, const std::string &nodeID, NIImporter_OpenDrive::LinkType lt)
Boundary getBoxBoundary() const
Returns a boundary enclosing this list of lines.
void push_back_noDoublePos(const Position &p)
insert in back a non double position
A connection between two roads.
bool wasIgnored(std::string id) const
Returns whether the edge with the id was ignored during parsing.
Container for nodes during the netbuilding process.
std::vector< std::pair< double, double > > speeds
List of positions/speeds of speed changes.
#define PROGRESS_DONE_MESSAGE()
A traffic light logics which must be computed (only nodes/edges are given)
void add(double xoff, double yoff, double zoff)
public emergency vehicles
Position positionAtOffset(double pos, double lateralOffset=0) const
Returns the position at the given length.
Importer for networks stored in openDrive format.
double s
The starting offset of this lane section.
std::string myCurrentIncomingRoad
NBNode * getToNode() const
Returns the destination node of the edge.
std::vector< double > params
std::string joinToString(const std::vector< T > &v, const T_BETWEEN &between, std::streamsize accuracy=gPrecision)
SVCPermissions getPermissions(const std::string &type) const
Returns allowed vehicle classes for the given type.
A storage for available types of edges.
std::string streetName
The road name of the edge.
static void buildConnectionsToOuter(const Connection &c, const std::map< std::string, OpenDriveEdge *> &innerEdges, std::vector< Connection > &into, std::set< Connection > &seen)
void myEndElement(int element)
Called when a closing tag occurs.