60 #define DEBUG_COND(road) ((road)->id == "1000003") 61 #define DEBUG_COND2(edgeID) (StringUtils::startsWith((edgeID), "2")) 171 std::map<std::string, OpenDriveEdge*> edges;
174 std::vector<std::string> files = oc.
getStringVector(
"opendrive-files");
175 for (std::vector<std::string>::const_iterator file = files.begin(); file != files.end(); ++file) {
177 WRITE_ERROR(
"Could not open opendrive file '" + *file +
"'.");
180 handler.setFileName(*file);
186 std::map<std::string, OpenDriveEdge*> innerEdges, outerEdges;
187 for (std::map<std::string, OpenDriveEdge*>::iterator i = edges.begin(); i != edges.end(); ++i) {
188 if ((*i).second->isInner) {
189 innerEdges[(*i).first] = (*i).second;
191 outerEdges[(*i).first] = (*i).second;
206 std::map<std::string, Boundary> posMap;
207 std::map<std::string, std::string> edge2junction;
209 for (std::map<std::string, OpenDriveEdge*>::iterator i = innerEdges.begin(); i != innerEdges.end(); ++i) {
213 if (posMap.find(e->
junction) == posMap.end()) {
219 for (std::map<std::string, Boundary>::iterator i = posMap.begin(); i != posMap.end(); ++i) {
222 throw ProcessError(
"Could not add node '" + (*i).first +
"'.");
226 for (std::map<std::string, OpenDriveEdge*>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
228 for (std::vector<OpenDriveLink>::iterator j = e->
links.begin(); j != e->
links.end(); ++j) {
236 throw ProcessError(
"Could not build node '" + nid +
"'.");
243 if (edge2junction.find(l.
elementID) != edge2junction.end()) {
255 for (std::map<std::string, OpenDriveEdge*>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
257 for (std::vector<OpenDriveLink>::iterator j = e->
links.begin(); j != e->
links.end(); ++j) {
264 std::string id1 = e->
id;
269 std::string nid = id1 +
"." + id2;
274 throw ProcessError(
"Could not build node '" + nid +
"'.");
292 for (std::map<std::string, OpenDriveEdge*>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
294 if (e->
to !=
nullptr && e->
from !=
nullptr) {
297 for (std::map<std::string, OpenDriveEdge*>::iterator j = innerEdges.begin(); j != innerEdges.end(); ++j) {
299 for (std::vector<OpenDriveLink>::iterator k = ie->
links.begin(); k != ie->
links.end(); ++k) {
305 std::string nid = edge2junction[ie->
id];
317 for (std::map<std::string, OpenDriveEdge*>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
319 if ((e->
from ==
nullptr || e->
to ==
nullptr) && e->
geom.size() == 0) {
322 if (e->
from ==
nullptr) {
323 const std::string nid = e->
id +
".begin";
326 if (e->
to ==
nullptr) {
327 const std::string nid = e->
id +
".end";
336 const double defaultSpeed = tc.
getSpeed(
"");
339 for (std::map<std::string, OpenDriveEdge*>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
341 if (e->
geom.size() == 0) {
345 bool lanesBuilt =
false;
361 double cF = length2D == 0 ? 1 : e->
length / length2D;
362 NBEdge* prevRight =
nullptr;
363 NBEdge* prevLeft =
nullptr;
371 WRITE_WARNING(
"Edge '" + e->
id +
"' has to be split as it connects same junctions.")
374 const double minDist = oc.
getFloat(
"opendrive.curve-resolution");
385 double nextS = (j + 1)->s;
393 std::string
id = e->
id;
394 if (sFrom != e->
from || sTo != e->
to) {
399 #ifdef DEBUG_VARIABLE_WIDTHS 401 std::cout <<
" id=" <<
id <<
" sB=" << sB <<
" sE=" << sE <<
" geom=" << geom <<
"\n";
406 NBEdge* currRight =
nullptr;
407 if ((*j).rightLaneNumber > 0) {
408 currRight =
new NBEdge(
"-" +
id, sFrom, sTo, (*j).rightType, defaultSpeed, (*j).rightLaneNumber, priorityR,
412 for (std::vector<OpenDriveLane>::const_iterator k = lanes.begin(); k != lanes.end(); ++k) {
413 std::map<int, int>::const_iterator lp = (*j).laneMap.find((*k).id);
414 if (lp != (*j).laneMap.end()) {
415 int sumoLaneIndex = lp->second;
439 if (prevRight !=
nullptr) {
441 for (std::map<int, int>::const_iterator k = connections.begin(); k != connections.end(); ++k) {
442 #ifdef DEBUG_CONNECTIONS 444 std::cout <<
"addCon1 from=" << prevRight->
getID() <<
"_" << (*k).first <<
" to=" << currRight->
getID() <<
"_" << (*k).second <<
"\n";
450 prevRight = currRight;
455 NBEdge* currLeft =
nullptr;
456 if ((*j).leftLaneNumber > 0) {
457 currLeft =
new NBEdge(
id, sTo, sFrom, (*j).leftType, defaultSpeed, (*j).leftLaneNumber, priorityL,
461 for (std::vector<OpenDriveLane>::const_iterator k = lanes.begin(); k != lanes.end(); ++k) {
462 std::map<int, int>::const_iterator lp = (*j).laneMap.find((*k).id);
463 if (lp != (*j).laneMap.end()) {
464 int sumoLaneIndex = lp->second;
488 if (prevLeft !=
nullptr) {
489 std::map<int, int> connections = (*j).getInnerConnections(
OPENDRIVE_TAG_LEFT, *(j - 1));
490 for (std::map<int, int>::const_iterator k = connections.begin(); k != connections.end(); ++k) {
491 #ifdef DEBUG_CONNECTIONS 493 std::cout <<
"addCon2 from=" << currLeft->
getID() <<
"_" << (*k).first <<
" to=" << prevLeft->
getID() <<
"_" << (*k).second <<
"\n";
517 for (std::map<std::string, OpenDriveEdge*>::iterator i = edges.begin(); i != edges.end(); ++i) {
521 std::vector<Connection> connections2;
522 for (std::map<std::string, OpenDriveEdge*>::iterator j = edges.begin(); j != edges.end(); ++j) {
523 const std::set<Connection>& conns = (*j).second->connections;
525 for (std::set<Connection>::const_iterator i = conns.begin(); i != conns.end(); ++i) {
526 if (innerEdges.find((*i).fromEdge) != innerEdges.end()) {
530 if (innerEdges.find((*i).toEdge) != innerEdges.end()) {
531 std::set<Connection> seen;
534 connections2.push_back(*i);
539 for (std::vector<Connection>::const_iterator i = connections2.begin(); i != connections2.end(); ++i) {
540 #ifdef DEBUG_CONNECTIONS 541 std::cout <<
"connections2 " << (*i).getDescription() <<
"\n";
543 std::string fromEdge = (*i).fromEdge;
544 if (edges.find(fromEdge) == edges.end()) {
545 WRITE_WARNING(
"While setting connections: from-edge '" + fromEdge +
"' is not known.");
549 int fromLane = (*i).fromLane;
553 std::string toEdge = (*i).toEdge;
554 if (edges.find(toEdge) == edges.end()) {
555 WRITE_WARNING(
"While setting connections: to-edge '" + toEdge +
"' is not known.");
560 int toLane = (*i).toLane;
580 if (from ==
nullptr) {
581 WRITE_WARNING(
"Could not find fromEdge representation of '" + fromEdge +
"' in connection '" + (*i).origID +
"'.");
584 WRITE_WARNING(
"Could not find fromEdge representation of '" + toEdge +
"' in connection '" + (*i).origID +
"'.");
586 if (from ==
nullptr || to ==
nullptr) {
590 #ifdef DEBUG_CONNECTIONS 592 std::cout <<
"addCon3 from=" << from->
getID() <<
"_" << fromLane <<
" to=" << to->
getID() <<
"_" << toLane <<
"\n";
601 if ((*i).origID !=
"" && saveOrigIDs) {
604 for (std::vector<NBEdge::Connection>::iterator k = cons.begin(); k != cons.end(); ++k) {
605 if ((*k).fromLane == fromLane && (*k).toEdge == to && (*k).toLane == toLane) {
617 std::map<std::string, std::string> tlsControlled;
618 for (std::map<std::string, OpenDriveEdge*>::iterator i = edges.begin(); i != edges.end(); ++i) {
620 for (std::vector<OpenDriveSignal>::const_iterator j = e->
signals.begin(); j != e->
signals.end(); ++j) {
621 if ((*j).type !=
"1000001") {
624 std::vector<OpenDriveLaneSection>::iterator k = e->
laneSections.begin();
627 if ((*j).s > (*k).s && (*j).s <= (*(k + 1)).s) {
637 std::string
id = (*k).sumoID;
641 std::string fromID, toID;
642 for (std::vector<OpenDriveLink>::const_iterator l = e->
links.begin(); l != e->
links.end(); ++l) {
650 if ((*j).orientation < 0) {
651 fromID =
"-" + fromID;
655 if ((*j).orientation > 0) {
656 fromID =
"-" + fromID;
668 id = fromID +
"->" + toID;
670 WRITE_WARNING(
"Found a traffic light signal on an unknown edge (original edge id='" + e->
id +
"').");
674 if ((*j).orientation > 0) {
678 tlsControlled[id] = (*j).name;
682 for (std::map<std::string, std::string>::iterator i = tlsControlled.begin(); i != tlsControlled.end(); ++i) {
683 std::string
id = (*i).first;
684 if (
id.find(
"->") != std::string::npos) {
685 id =
id.substr(0,
id.find(
"->"));
689 WRITE_WARNING(
"Could not find edge '" +
id +
"' while building its traffic light.");
711 for (std::map<std::string, OpenDriveEdge*>::iterator i = edges.begin(); i != edges.end(); ++i) {
737 if (dest ==
nullptr) {
742 const std::set<Connection>& conts = dest->
connections;
743 for (std::set<Connection>::const_iterator i = conts.begin(); i != conts.end(); ++i) {
744 auto innerEdgesIt = innerEdges.find((*i).toEdge);
745 if (innerEdgesIt != innerEdges.end()) {
746 std::vector<Connection> t;
747 if (seen.count(*i) == 0) {
749 for (std::vector<Connection>::const_iterator j = t.begin(); j != t.end(); ++j) {
757 cn.
shape = innerEdgesIt->second->geom + c.
shape;
765 if ((*i).fromLane == c.
toLane) {
777 int referenceLane = 0;
778 int offsetFactor = 1;
782 for (
const auto& destLane : dest->
laneSections.front().lanesByDir[lanesDir]) {
783 if (destLane.successor == c.
fromLane) {
784 referenceLane = destLane.id;
790 for (
const auto& destLane : dest->
laneSections.front().lanesByDir[lanesDir]) {
791 if (destLane.predecessor == c.
fromLane) {
792 referenceLane = destLane.id;
798 std::vector<double> offsets(dest->
geom.size(), 0);
799 #ifdef DEBUG_INTERNALSHAPES 800 std::string destPred;
802 for (
const auto& destLane : dest->
laneSections.front().lanesByDir[lanesDir]) {
803 #ifdef DEBUG_INTERNALSHAPES 804 destPred +=
" lane=" +
toString(destLane.id)
805 +
" pred=" +
toString(destLane.predecessor)
806 +
" succ=" +
toString(destLane.successor)
807 +
" wStart=" +
toString(destLane.widthData.front().computeAt(0))
809 +
" width=" +
toString(destLane.width) +
"\n";
811 if (abs(destLane.id) <= abs(referenceLane)) {
812 const double multiplier = offsetFactor * (destLane.id == referenceLane ? 0.5 : 1);
813 #ifdef DEBUG_INTERNALSHAPES 814 destPred +=
" multiplier=" +
toString(multiplier) +
"\n";
817 for (
int i = 0; i < (int)cn.
shape.size(); ++i) {
819 s += cn.
shape[i - 1].distanceTo2D(cn.
shape[i]);
821 offsets[i] += destLane.widthData.front().computeAt(s) * multiplier;
831 #ifdef DEBUG_INTERNALSHAPES 832 std::cout <<
"internalShape " 834 <<
" dest=" << dest->
id 835 <<
" refLane=" << referenceLane
836 <<
" destPred\n" << destPred
837 <<
" offsets=" << offsets
838 <<
"\n shape=" << dest->
geom 839 <<
"\n shape2=" << cn.
shape 855 for (std::vector<OpenDriveLink>::iterator i = e.
links.begin(); i != e.
links.end(); ++i) {
863 std::string edgeID = e.
id;
866 const std::map<int, int>& laneMap = laneSection.
laneMap;
867 #ifdef DEBUG_CONNECTIONS 869 std::cout <<
"edge=" << e.
id <<
" eType=" << l.
elementType <<
" lType=" << l.
linkType <<
" connectedEdge=" << connectedEdge <<
" laneSection=" << laneSection.
s <<
" map:\n";
875 for (std::vector<OpenDriveLane>::const_iterator j = lanes.begin(); j != lanes.end(); ++j) {
892 if (edges.find(c.
fromEdge) == edges.end()) {
893 WRITE_ERROR(
"While setting connections: incoming road '" + c.
fromEdge +
"' is not known.");
897 #ifdef DEBUG_CONNECTIONS 905 for (std::vector<OpenDriveLane>::const_iterator j = lanes.begin(); j != lanes.end(); ++j) {
922 if (edges.find(c.
fromEdge) == edges.end()) {
923 WRITE_ERROR(
"While setting connections: incoming road '" + c.
fromEdge +
"' is not known.");
927 #ifdef DEBUG_CONNECTIONS 929 std::cout <<
"insertConLeft from=" << src->
id <<
"_" << c.
fromLane <<
" to=" << c.
toEdge <<
"_" << c.
toLane <<
"\n";
952 if (!nc.
insert(
id, pos)) {
966 throw ProcessError(
"Could not find node '" + nodeID +
"'.");
969 if (e.
to !=
nullptr && e.
to != n) {
974 if (e.
from !=
nullptr && e.
from != n) {
985 const double res = oc.
getFloat(
"opendrive.curve-resolution");
986 for (std::map<std::string, OpenDriveEdge*>::iterator i = edges.begin(); i != edges.end(); ++i) {
989 for (std::vector<OpenDriveGeometry>::iterator j = e.
geometries.begin(); j != e.
geometries.end(); ++j) {
1017 if (!e.
geom.back().almostSame(geom.front())) {
1018 const int index = (int)(j - e.
geometries.begin());
1024 for (PositionVector::iterator k = geom.begin(); k != geom.end(); ++k) {
1029 if (oc.
exists(
"geometry.min-dist") && !oc.
isDefault(
"geometry.min-dist")) {
1033 WRITE_ERROR(
"Unable to project coordinates for edge '" + e.
id +
"'.");
1038 for (std::vector<OpenDriveElevation>::iterator j = e.
elevations.begin(); j != e.
elevations.end(); ++j) {
1040 const double sNext = (j + 1) == e.
elevations.end() ? std::numeric_limits<double>::max() : (*(j + 1)).s;
1041 while (k < (
int)e.
geom.size() && pos < sNext) {
1046 if (k < (
int)e.
geom.size()) {
1049 pos += e.
geom[k - 1].distanceTo2D(e.
geom[k]);
1056 for (std::vector<OpenDriveLaneOffset>::iterator j = e.
offsets.begin(); j != e.
offsets.end(); ++j) {
1072 for (std::vector<OpenDriveLaneOffset>::iterator j = e.
offsets.begin(); j != e.
offsets.end(); ++j) {
1074 const double sNext = (j + 1) == e.
offsets.end() ? std::numeric_limits<double>::max() : (*(j + 1)).s;
1075 while (k < (
int)e.
geom.size() && pos < sNext) {
1076 const double offset = el.
computeAt(pos);
1084 geom2.push_back(tmp[k]);
1086 geom2.push_back(e.
geom[k]);
1089 geom2.push_back(e.
geom[k]);
1092 if (k < (
int)e.
geom.size()) {
1095 pos += e.
geom[k - 1].distanceTo2D(e.
geom[k]);
1099 assert(e.
geom.size() == geom2.size());
1109 for (std::map<std::string, OpenDriveEdge*>::iterator i = edges.begin(); i != edges.end(); ++i) {
1111 #ifdef DEBUG_VARIABLE_SPEED 1114 std::cout <<
"revisitLaneSections e=" << e.
id <<
"\n";
1117 std::vector<OpenDriveLaneSection>& laneSections = e.
laneSections;
1119 std::vector<OpenDriveLaneSection> newSections;
1120 for (std::vector<OpenDriveLaneSection>::iterator j = laneSections.begin(); j != laneSections.end(); ++j) {
1121 std::vector<OpenDriveLaneSection> splitSections;
1122 bool splitBySpeed = (*j).buildSpeedChanges(tc, splitSections);
1123 if (!splitBySpeed) {
1124 newSections.push_back(*j);
1126 std::copy(splitSections.begin(), splitSections.end(), back_inserter(newSections));
1135 for (std::vector<OpenDriveLaneSection>::const_iterator j = laneSections.begin(); j != laneSections.end() && sorted; ++j) {
1136 if ((*j).s <= lastS) {
1142 WRITE_WARNING(
"The sections of edge '" + e.
id +
"' are not sorted properly.");
1148 for (std::vector<OpenDriveLaneSection>::iterator j = laneSections.begin(); j != laneSections.end();) {
1149 bool simlarToLast = fabs((*j).s - lastS) <
POSITION_EPS;
1152 WRITE_WARNING(
"Almost duplicate s-value '" +
toString(lastS) +
"' for lane sections occurred at edge '" + e.
id +
"'; second entry was removed.");
1153 j = laneSections.erase(j);
1158 #ifdef DEBUG_VARIABLE_SPEED 1179 double curveStart = g.
params[0];
1180 double curveEnd = g.
params[1];
1182 double cDot = (curveEnd - curveStart) / g.
length;
1183 if (cDot == 0 || g.
length == 0) {
1188 double sStart = curveStart / cDot;
1189 double sEnd = curveEnd / cDot;
1195 odrSpiral(sStart, cDot, &x, &y, &tStart);
1196 for (s = sStart; s <= sEnd; s += resolution) {
1207 assert(ret.size() >= 2);
1208 assert(ret[0] != ret[1]);
1211 ret.
add(ret.front() * -1);
1217 << std::setprecision(4)
1218 <<
"edge=" << e.
id <<
" s=" << g.
s 1219 <<
" cStart=" << curveStart
1220 <<
" cEnd=" << curveEnd
1222 <<
" sStart=" << sStart
1226 <<
"\n beforeShift=" << ret1
1227 <<
"\n beforeRot=" << ret2
1231 ret.
add(g.
x, g.
y, 0);
1232 }
catch (
const std::runtime_error&
error) {
1233 WRITE_WARNING(
"Could not compute spiral geometry for edge '" + e.
id +
"' (" + error.what() +
").");
1245 double centerX = g.
x;
1246 double centerY = g.
y;
1248 double curvature = g.
params[0];
1249 double radius = 1. / curvature;
1254 double startX = g.
x;
1255 double startY = g.
y;
1256 double geo_posS = g.
s;
1257 double geo_posE = g.
s;
1260 geo_posE += resolution;
1261 if (geo_posE - g.
s > g.
length) {
1264 if (geo_posE - g.
s > g.
length) {
1267 calcPointOnCurve(&endX, &endY, centerX, centerY, radius, geo_posE - geo_posS);
1269 dist += (geo_posE - geo_posS);
1271 ret.push_back(
Position(startX, startY));
1275 geo_posS = geo_posE;
1277 if (geo_posE - (g.
s + g.
length) < 0.001 && geo_posE - (g.
s + g.
length) > -0.001) {
1288 const double s = sin(g.
hdg);
1289 const double c = cos(g.
hdg);
1291 for (
double off = 0; off < g.
length + 2.; off += resolution) {
1294 double xnew = x * c - y * s;
1295 double ynew = x * s + y * c;
1296 ret.push_back(
Position(g.
x + xnew, g.
y + ynew));
1305 const double s = sin(g.
hdg);
1306 const double c = cos(g.
hdg);
1308 const double pStep = pMax / ceil(g.
length / resolution);
1310 for (
double p = 0; p <= pMax + pStep; p += pStep) {
1313 double xnew = x * c - y * s;
1314 double ynew = x * s + y * c;
1315 ret.push_back(
Position(g.
x + xnew, g.
y + ynew));
1323 double normx = 1.0f;
1324 double normy = 0.0f;
1325 double x2 = normx * cos(hdg) - normy * sin(hdg);
1326 double y2 = normx * sin(hdg) + normy * cos(hdg);
1327 normx = x2 * length;
1328 normy = y2 * length;
1329 return Position(start.
x() + normx, start.
y() + normy);
1339 if (ad_radius > 0) {
1346 normX = normX * cos(ad_hdg) + normY * sin(ad_hdg);
1347 normY = tmpX * sin(ad_hdg) + normY * cos(ad_hdg);
1350 normX = turn * normY;
1351 normY = -turn * tmpX;
1353 normX = fabs(ad_radius) * normX;
1354 normY = fabs(ad_radius) * normY;
1363 double ad_r,
double ad_length) {
1364 double rotAngle = ad_length / fabs(ad_r);
1365 double vx = *ad_x - ad_centerX;
1366 double vy = *ad_y - ad_centerY;
1376 vx = vx * cos(rotAngle) + turn * vy * sin(rotAngle);
1377 vy = -1 * turn * tmpx * sin(rotAngle) + vy * cos(rotAngle);
1378 *ad_x = vx + ad_centerX;
1379 *ad_y = vy + ad_centerY;
1396 bool singleType =
true;
1397 std::vector<std::string> types;
1399 for (std::vector<OpenDriveLane>::const_reverse_iterator i = dirLanesR.rbegin(); i != dirLanesR.rend(); ++i) {
1401 laneMap[(*i).id] = sumoLane++;
1402 types.push_back((*i).type);
1403 if (types.front() != types.back()) {
1414 for (std::vector<OpenDriveLane>::const_iterator i = dirLanesL.begin(); i != dirLanesL.end(); ++i) {
1416 laneMap[(*i).id] = sumoLane++;
1417 types.push_back((*i).type);
1418 if (types.front() != types.back()) {
1430 std::map<int, int> ret;
1431 const std::vector<OpenDriveLane>& dirLanes =
lanesByDir.find(dir)->second;
1432 for (std::vector<OpenDriveLane>::const_reverse_iterator i = dirLanes.rbegin(); i != dirLanes.rend(); ++i) {
1433 std::map<int, int>::const_iterator toP =
laneMap.find((*i).id);
1438 int to = (*toP).second;
1441 from = (*i).predecessor;
1444 std::map<int, int>::const_iterator fromP = prev.
laneMap.find(from);
1445 if (fromP != prev.
laneMap.end()) {
1446 from = (*fromP).second;
1452 if (ret.find(from) != ret.end()) {
1456 std::swap(from, to);
1475 if (i != l.
speeds.end()) {
1476 l.
speed = (*i).second;
1483 if (i != l.
speeds.end()) {
1484 l.
speed = (*i).second;
1493 std::set<double> speedChangePositions;
1496 for (std::vector<std::pair<double, double> >::const_iterator l = (*k).speeds.begin(); l != (*k).speeds.end(); ++l) {
1497 speedChangePositions.insert((*l).first);
1498 if ((*l).first == 0) {
1499 (*k).speed = (*l).second;
1504 for (std::vector<std::pair<double, double> >::const_iterator l = (*k).speeds.begin(); l != (*k).speeds.end(); ++l) {
1505 speedChangePositions.insert((*l).first);
1506 if ((*l).first == 0) {
1507 (*k).speed = (*l).second;
1512 if (speedChangePositions.size() == 0) {
1515 if (*speedChangePositions.begin() > 0) {
1516 speedChangePositions.insert(0);
1518 #ifdef DEBUG_VARIABLE_SPEED 1520 <<
" buildSpeedChanges sectionStart=" <<
s 1521 <<
" speedChangePositions=" <<
joinToString(speedChangePositions,
", ")
1524 for (std::set<double>::iterator i = speedChangePositions.begin(); i != speedChangePositions.end(); ++i) {
1525 if (i == speedChangePositions.begin()) {
1526 newSections.push_back(*
this);
1532 for (
int i = 0; i != (int)newSections.size(); ++i) {
1535 for (std::map<
OpenDriveXMLTag, std::vector<OpenDriveLane> >::iterator k = lanesByDir.begin(); k != lanesByDir.end(); ++k) {
1536 std::vector<OpenDriveLane>& lanes = (*k).second;
1537 for (
int j = 0; j != (int)lanes.size(); ++j) {
1543 l.
speed = newSections[i - 1].lanesByDir[(*k).first][j].speed;
1562 for (std::vector<OpenDriveSignal>::const_iterator i = signals.begin(); i != signals.end(); ++i) {
1564 if ((*i).type ==
"301" || (*i).type ==
"306") {
1567 if ((*i).type ==
"205" ) {
1604 if (majorVersion != 1 || minorVersion != 2) {
1670 std::vector<double> vals;
1676 std::vector<double> vals;
1683 std::vector<double> vals;
1689 std::vector<double> vals;
1698 std::vector<double> vals;
1708 if (pRange ==
"normalized") {
1709 vals.push_back(1.0);
1710 }
else if (pRange ==
"arcLength") {
1711 vals.push_back(-1.0);
1714 vals.push_back(1.0);
1786 WRITE_ERROR(
"In laneLink-element: incoming road '" + c.fromEdge +
"' is not known.");
1802 l.width =
MAX2(l.width, a);
1804 #ifdef DEBUG_VARIABLE_WIDTHS 1811 <<
" type=" << l.type
1812 <<
" width=" << l.width
1818 <<
" entries=" << l.widthData.size()
1832 if (!unit.empty()) {
1834 if (unit ==
"km/h") {
1837 if (unit ==
"mph") {
1838 speed *= 1.609344 / 3.6;
1856 size_t i = cdata.find(
"+proj");
1857 if (i != std::string::npos) {
1858 const std::string proj = cdata.substr(i);
1868 result =
new GeoConvHelper(proj, networkOffset, origBoundary, convBoundary);
1871 WRITE_ERROR(
"Could not set projection. (" + std::string(e.what()) +
")");
1875 WRITE_WARNING(
"geoReference format '" + cdata +
"' currently not supported");
1919 const std::string& elementID,
1920 const std::string& contactPoint) {
1923 if (elementType ==
"road") {
1925 }
else if (elementType ==
"junction") {
1929 if (contactPoint ==
"start") {
1931 }
else if (contactPoint ==
"end") {
1972 std::vector<OpenDriveLaneSection> newSections;
1973 #ifdef DEBUG_VARIABLE_WIDTHS 1976 std::cout <<
"splitMinWidths e=" << e->
id <<
" sections=" << e->
laneSections.size() <<
"\n";
1981 std::vector<double> splitPositions;
1982 const double sectionEnd = (j + 1) == e->
laneSections.end() ? e->
length : (*(j + 1)).s;
1983 const int section = (int)(j - e->
laneSections.begin());
1984 #ifdef DEBUG_VARIABLE_WIDTHS 1986 std::cout <<
" findWidthSplit section=" << section <<
" sectionStart=" << sec.
s <<
" sectionOrigStart=" << sec.
sOrig <<
" sectionEnd=" << sectionEnd <<
"\n";
1995 newSections.push_back(sec);
1996 std::sort(splitPositions.begin(), splitPositions.end());
1998 double prevSplit = sec.
s;
1999 for (std::vector<double>::iterator it = splitPositions.begin(); it != splitPositions.end();) {
2000 if ((*it) - prevSplit < minDist || sectionEnd - (*it) < minDist) {
2002 #ifdef DEBUG_VARIABLE_WIDTHS 2004 std::cout <<
" skip close split=" << (*it) <<
" prevSplit=" << prevSplit <<
"\n";
2007 it = splitPositions.erase(it);
2008 }
else if ((*it) < sec.
s) {
2010 #ifdef DEBUG_VARIABLE_WIDTHS 2012 std::cout <<
" skip early split=" << (*it) <<
" s=" << sec.
s <<
"\n";
2015 it = splitPositions.erase(it);
2022 if (splitPositions.size() > 0) {
2023 #ifdef DEBUG_VARIABLE_WIDTHS 2025 std::cout <<
" road=" << e->
id <<
" splitMinWidths section=" << section
2026 <<
" start=" << sec.
s 2027 <<
" origStart=" << sec.
sOrig 2028 <<
" end=" << sectionEnd <<
" minDist=" << minDist
2029 <<
" splitPositions=" <<
toString(splitPositions) <<
"\n";
2032 #ifdef DEBUG_VARIABLE_WIDTHS 2034 std::cout <<
"first section...\n";
2038 for (std::vector<double>::iterator it = splitPositions.begin(); it != splitPositions.end(); ++it) {
2041 #ifdef DEBUG_VARIABLE_WIDTHS 2043 std::cout <<
"splitAt " << secNew.
s <<
"\n";
2046 newSections.push_back(secNew);
2053 double end = (it + 1) == splitPositions.end() ? sectionEnd : *(it + 1);
2065 int section,
double sectionStart,
double sectionEnd,
2066 std::vector<double>& splitPositions) {
2068 for (std::vector<OpenDriveLane>::iterator k = lanes.begin(); k != lanes.end(); ++k) {
2073 double wPrev = l.
widthData.front().computeAt(sPrev);
2075 <<
"findWidthSplit section=" << section
2076 <<
" sectionStart=" << sectionStart
2077 <<
" sectionEnd=" << sectionEnd
2079 <<
" type=" << l.
type 2080 <<
" widthEntries=" << l.
widthData.size() <<
"\n" 2084 for (std::vector<OpenDriveWidth>::iterator it_w = l.
widthData.begin(); it_w != l.
widthData.end(); ++it_w) {
2085 double sEnd = (it_w + 1) != l.
widthData.end() ? (*(it_w + 1)).s : sectionEnd - sectionStart;
2086 double w = (*it_w).computeAt(sEnd);
2089 <<
" s=" << (*it_w).s
2090 <<
" a=" << (*it_w).a <<
" b=" << (*it_w).b <<
" c=" << (*it_w).c <<
" d=" << (*it_w).d
2093 const double changeDist = fabs(
myMinWidth - wPrev);
2096 double splitPos = sPrev + (sEnd - sPrev) / fabs(w - wPrev) * changeDist;
2097 double wSplit = (*it_w).computeAt(splitPos);
2099 std::cout <<
" candidate splitPos=" << splitPos <<
" w=" << wSplit <<
"\n";
2106 if (splitPos < sPrev) {
2108 std::cout <<
" aborting search splitPos=" << splitPos <<
" wSplit=" << wSplit <<
" sPrev=" << sPrev <<
" wPrev=" << wPrev <<
"\n";
2116 if (splitPos > sEnd) {
2118 std::cout <<
" aborting search splitPos=" << splitPos <<
" wSplit=" << wSplit <<
" sEnd=" << sEnd <<
" w=" << w <<
"\n";
2124 wSplit = (*it_w).computeAt(splitPos);
2126 std::cout <<
" refined splitPos=" << splitPos <<
" w=" << wSplit <<
"\n";
2129 splitPositions.push_back(sectionStart + splitPos);
2147 for (std::vector<OpenDriveLane>::iterator k = lanes.begin(); k != lanes.end(); ++k) {
2148 (*k).predecessor = (*k).id;
2166 for (std::vector<OpenDriveLane>::iterator k = lanes.begin(); k != lanes.end(); ++k) {
2169 #ifdef DEBUG_VARIABLE_WIDTHS 2171 <<
"recomputeWidths lane=" << l.
id 2172 <<
" type=" << l.
type 2173 <<
" start=" << start
2175 <<
" sectionStart=" << sectionStart
2176 <<
" sectionEnd=" << sectionEnd
2177 <<
" widthEntries=" << l.
widthData.size() <<
"\n" 2182 double sPrevAbs = sPrev + sectionStart;
2183 for (std::vector<OpenDriveWidth>::iterator it_w = l.
widthData.begin(); it_w != l.
widthData.end(); ++it_w) {
2184 double sEnd = (it_w + 1) != l.
widthData.end() ? (*(it_w + 1)).s : sectionEnd - sectionStart;
2185 double sEndAbs = sEnd + sectionStart;
2186 #ifdef DEBUG_VARIABLE_WIDTHS 2188 <<
" sPrev=" << sPrev <<
" sPrevAbs=" << sPrevAbs
2189 <<
" sEnd=" << sEnd <<
" sEndAbs=" << sEndAbs
2190 <<
" widthData s=" << (*it_w).s
2191 <<
" a=" << (*it_w).a
2192 <<
" b=" << (*it_w).b
2193 <<
" c=" << (*it_w).c
2194 <<
" d=" << (*it_w).d
2197 if (sPrevAbs <= start && sEndAbs >= start) {
2198 #ifdef DEBUG_VARIABLE_WIDTHS 2200 std::cout <<
" atStart=" << start <<
" pos=" << start - sectionStart <<
" w=" << (*it_w).computeAt(start - sectionStart) <<
"\n";
2203 l.
width =
MAX2(l.
width, (*it_w).computeAt(start - sectionStart));
2205 if (sPrevAbs <= end && sEndAbs >= end) {
2206 #ifdef DEBUG_VARIABLE_WIDTHS 2208 std::cout <<
" atEnd=" << end <<
" pos=" << end - sectionStart <<
" w=" << (*it_w).computeAt(end - sectionStart) <<
"\n";
2213 if (start <= sPrevAbs && end >= sPrevAbs) {
2214 #ifdef DEBUG_VARIABLE_WIDTHS 2216 std::cout <<
" atSPrev=" << sPrev <<
" w=" << (*it_w).computeAt(sPrev) <<
"\n";
2221 if (start <= sEndAbs && end >= sEndAbs) {
2222 #ifdef DEBUG_VARIABLE_WIDTHS 2224 std::cout <<
" atSEnd=" << sEnd <<
" w=" << (*it_w).computeAt(sEnd) <<
"\n";
2229 #ifdef DEBUG_VARIABLE_WIDTHS 2231 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)
#define DEBUG_COND2(edgeID)
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.
static const double UNSPECIFIED_VISIBILITY_DISTANCE
unspecified foe visibility for connections
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".
static bool myImportInternalShapes
static void setLoaded(const GeoConvHelper &loaded)
sets the coordinate transformation loaded from a location element
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)
void myCharacters(int element, const std::string &chars)
Callback method for characters to implement by derived classes.
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
static const double UNSPECIFIED_SPEED
unspecified lane speed
Representation of a lane.
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, const bool uncontrolled=UNSPECIFIED_CONNECTION_UNCONTROLLED)
Adds a connection between the specified this edge's lane and an approached one.
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)
static methods for processing the coordinates conversion for the current net
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
static const double UNSPECIFIED_CONTPOS
unspecified internal junction position
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
vehicle is a passenger car (a "normal" car)
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.
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.
std::string getDescription() const
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()
void add(double x, double y, double z=0)
Makes the boundary include the given coordinate.
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.