60 #ifdef CHECK_MEMORY_LEAKS
62 #endif // CHECK_MEMORY_LEAKS
164 std::map<std::string, OpenDriveEdge*> edges;
167 std::vector<std::string> files = oc.
getStringVector(
"opendrive-files");
168 for (std::vector<std::string>::const_iterator file = files.begin(); file != files.end(); ++file) {
170 WRITE_ERROR(
"Could not open opendrive file '" + *file +
"'.");
179 std::map<std::string, OpenDriveEdge*> innerEdges, outerEdges;
180 for (std::map<std::string, OpenDriveEdge*>::iterator i = edges.begin(); i != edges.end(); ++i) {
181 if ((*i).second->isInner) {
182 innerEdges[(*i).first] = (*i).second;
184 outerEdges[(*i).first] = (*i).second;
199 std::map<std::string, Boundary> posMap;
200 std::map<std::string, std::string> edge2junction;
202 for (std::map<std::string, OpenDriveEdge*>::iterator i = innerEdges.begin(); i != innerEdges.end(); ++i) {
206 if (posMap.find(e->
junction) == posMap.end()) {
212 for (std::map<std::string, Boundary>::iterator i = posMap.begin(); i != posMap.end(); ++i) {
214 throw ProcessError(
"Could not add node '" + (*i).first +
"'.");
218 for (std::map<std::string, OpenDriveEdge*>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
220 for (std::vector<OpenDriveLink>::iterator j = e->
links.begin(); j != e->
links.end(); ++j) {
227 if (edge2junction.find(l.
elementID) != edge2junction.end()) {
239 for (std::map<std::string, OpenDriveEdge*>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
241 for (std::vector<OpenDriveLink>::iterator j = e->
links.begin(); j != e->
links.end(); ++j) {
248 std::string id1 = e->
id;
253 std::string nid = id1 +
"." + id2;
258 throw ProcessError(
"Could not build node '" + nid +
"'.");
276 for (std::map<std::string, OpenDriveEdge*>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
278 if (e->
to != 0 && e->
from != 0) {
281 for (std::map<std::string, OpenDriveEdge*>::iterator j = innerEdges.begin(); j != innerEdges.end(); ++j) {
283 for (std::vector<OpenDriveLink>::iterator k = ie->
links.begin(); k != ie->
links.end(); ++k) {
289 std::string nid = edge2junction[ie->
id];
301 for (std::map<std::string, OpenDriveEdge*>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
304 const std::string nid = e->
id +
".begin";
308 const std::string nid = e->
id +
".end";
319 for (std::map<std::string, OpenDriveEdge*>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
321 bool lanesBuilt =
false;
345 WRITE_WARNING(
"Edge '" + e->
id +
"' has to be split as it connects same junctions.")
363 std::string
id = e->
id;
364 if (sFrom != e->
from || sTo != e->
to) {
372 if ((*j).rightLaneNumber > 0) {
373 currRight =
new NBEdge(
"-" +
id, sFrom, sTo,
"", defaultSpeed, (*j).rightLaneNumber, priorityR,
380 for (std::vector<OpenDriveLane>::const_iterator k = lanes.begin(); k != lanes.end(); ++k) {
381 std::map<int, int>::const_iterator lp = (*j).laneMap.find((*k).id);
382 if (lp != (*j).laneMap.end()) {
383 int sumoLaneIndex = lp->second;
394 if (prevRight != 0) {
396 for (std::map<int, int>::const_iterator k = connections.begin(); k != connections.end(); ++k) {
400 prevRight = currRight;
405 if ((*j).leftLaneNumber > 0) {
406 currLeft =
new NBEdge(
id, sTo, sFrom,
"", defaultSpeed, (*j).leftLaneNumber, priorityL,
413 for (std::vector<OpenDriveLane>::const_iterator k = lanes.begin(); k != lanes.end(); ++k) {
414 std::map<int, int>::const_iterator lp = (*j).laneMap.find((*k).id);
415 if (lp != (*j).laneMap.end()) {
416 int sumoLaneIndex = lp->second;
428 std::map<int, int> connections = (*j).getInnerConnections(
OPENDRIVE_TAG_LEFT, *(j - 1));
429 for (std::map<int, int>::const_iterator k = connections.begin(); k != connections.end(); ++k) {
450 for (std::map<std::string, OpenDriveEdge*>::iterator i = edges.begin(); i != edges.end(); ++i) {
454 std::vector<Connection> connections2;
455 for (std::map<std::string, OpenDriveEdge*>::iterator j = edges.begin(); j != edges.end(); ++j) {
456 const std::set<Connection>& conns = (*j).second->connections;
458 for (std::set<Connection>::const_iterator i = conns.begin(); i != conns.end(); ++i) {
459 if (innerEdges.find((*i).fromEdge) != innerEdges.end()) {
463 if (innerEdges.find((*i).toEdge) != innerEdges.end()) {
466 connections2.push_back(*i);
471 for (std::vector<Connection>::const_iterator i = connections2.begin(); i != connections2.end(); ++i) {
472 std::string fromEdge = (*i).fromEdge;
473 if (edges.find(fromEdge) == edges.end()) {
474 WRITE_WARNING(
"While setting connections: from-edge '" + fromEdge +
"' is not known.");
478 int fromLane = (*i).fromLane;
479 bool fromLast = ((*i).fromCP ==
OPENDRIVE_CP_END) ^ ((*i).fromLane > 0 && !(*i).all);
482 std::string toEdge = (*i).toEdge;
483 if (edges.find(toEdge) == edges.end()) {
484 WRITE_WARNING(
"While setting connections: to-edge '" + toEdge +
"' is not known.");
489 int toLane = (*i).toLane;
494 fromLane = toLast ? odTo->
laneSections.back().laneMap.begin()->first : odTo->
laneSections[0].laneMap.begin()->first;
510 WRITE_WARNING(
"Could not find fromEdge representation of '" + fromEdge +
"' in connection '" + (*i).origID +
"'.");
513 WRITE_WARNING(
"Could not find fromEdge representation of '" + toEdge +
"' in connection '" + (*i).origID +
"'.");
515 if (from == 0 || to == 0) {
521 if ((*i).origID !=
"") {
524 for (std::vector<NBEdge::Connection>::iterator k = cons.begin(); k != cons.end(); ++k) {
525 if ((*k).fromLane == fromLane && (*k).toEdge == to && (*k).toLane == toLane) {
526 (*k).origID = (*i).origID +
" " +
toString((*i).origLane);
537 std::map<std::string, std::string> tlsControlled;
538 for (std::map<std::string, OpenDriveEdge*>::iterator i = edges.begin(); i != edges.end(); ++i) {
540 for (std::vector<OpenDriveSignal>::const_iterator j = e->
signals.begin(); j != e->
signals.end(); ++j) {
541 if ((*j).type !=
"1000001") {
544 std::vector<OpenDriveLaneSection>::iterator k = e->
laneSections.begin();
547 if ((*j).s > (*k).s && (*j).s <= (*(k + 1)).s) {
557 std::string
id = (*k).sumoID;
561 std::string fromID, toID;
562 for (std::vector<OpenDriveLink>::const_iterator l = e->
links.begin(); l != e->
links.end(); ++l) {
567 fromID = (*l).elementID;
575 toID = (*l).elementID;
580 id = fromID +
"->" + toID;
582 WRITE_WARNING(
"Found a traffic light signal on an unknown edge (original edge id='" + e->
id +
"').");
587 if ((*j).orientation > 0) {
590 tlsControlled[id] = (*j).name;
594 for (std::map<std::string, std::string>::iterator i = tlsControlled.begin(); i != tlsControlled.end(); ++i) {
595 std::string
id = (*i).first;
596 if (
id.find(
"->") != std::string::npos) {
597 id =
id.substr(0,
id.find(
"->"));
601 WRITE_WARNING(
"Could not find edge '" +
id +
"' while building its traffic light.");
615 static_cast<NBOwnTLDef*
>(tlDef)->setSinglePhase();
624 if (oc.
exists(
"geometry.min-dist") && oc.
isSet(
"geometry.min-dist")) {
625 oc.
unSet(
"geometry.min-dist");
627 for (std::map<std::string, OpenDriveEdge*>::iterator i = edges.begin(); i != edges.end(); ++i) {
642 const std::set<Connection>& conts = dest->
connections;
643 for (std::set<Connection>::const_iterator i = conts.begin(); i != conts.end(); ++i) {
644 if (innerEdges.find((*i).toEdge) != innerEdges.end()) {
645 std::vector<Connection> t;
647 for (std::vector<Connection>::const_iterator j = t.begin(); j != t.end(); ++j) {
657 if ((*i).fromLane == c.
toLane) {
674 for (std::vector<OpenDriveLink>::iterator i = e.
links.begin(); i != e.
links.end(); ++i) {
682 std::string edgeID = e.
id;
685 const std::map<int, int>& laneMap = laneSection.
laneMap;
688 for (std::vector<OpenDriveLane>::const_iterator j = lanes.begin(); j != lanes.end(); ++j) {
705 if (edges.find(c.
fromEdge) == edges.end()) {
706 WRITE_ERROR(
"While setting connections: incoming road '" + c.
fromEdge +
"' is not known.");
715 for (std::vector<OpenDriveLane>::const_iterator j = lanes.begin(); j != lanes.end(); ++j) {
732 if (edges.find(c.
fromEdge) == edges.end()) {
733 WRITE_ERROR(
"While setting connections: incoming road '" + c.
fromEdge +
"' is not known.");
756 if (!nc.
insert(
id, pos)) {
770 throw ProcessError(
"Could not find node '" + nodeID +
"'.");
773 if (e.
to != 0 && e.
to != n) {
794 for (std::map<std::string, OpenDriveEdge*>::iterator i = edges.begin(); i != edges.end(); ++i) {
796 for (std::vector<OpenDriveGeometry>::iterator j = e.
geometries.begin(); j != e.
geometries.end(); ++j) {
798 std::vector<Position> geom;
817 for (std::vector<Position>::iterator k = geom.begin(); k != geom.end(); ++k) {
821 if (oc.
exists(
"geometry.min-dist") && oc.
isSet(
"geometry.min-dist")) {
824 for (
unsigned int j = 0; j < e.
geom.size(); ++j) {
835 for (std::map<std::string, OpenDriveEdge*>::iterator i = edges.begin(); i != edges.end(); ++i) {
837 std::vector<OpenDriveLaneSection>& laneSections = e.
laneSections;
839 std::vector<OpenDriveLaneSection> newSections;
840 for (std::vector<OpenDriveLaneSection>::iterator j = laneSections.begin(); j != laneSections.end(); ++j) {
841 std::vector<OpenDriveLaneSection> splitSections;
842 bool splitBySpeed = (*j).buildSpeedChanges(tc, splitSections);
844 newSections.push_back(*j);
846 std::copy(splitSections.begin(), splitSections.end(), back_inserter(newSections));
855 for (std::vector<OpenDriveLaneSection>::const_iterator j = laneSections.begin(); j != laneSections.end() && sorted; ++j) {
856 if ((*j).s <= lastS) {
862 WRITE_WARNING(
"The sections of edge '" + e.
id +
"' are not sorted properly.");
868 for (std::vector<OpenDriveLaneSection>::iterator j = laneSections.begin(); j != laneSections.end();) {
869 bool simlarToLast = fabs((*j).s - lastS) <
POSITION_EPS;
872 WRITE_WARNING(
"Almost duplicate s-value '" +
toString(lastS) +
"' for lane sections occured at edge '" + e.
id +
"'; second entry was removed.");
873 j = laneSections.erase(j);
882 std::vector<Position>
885 std::vector<Position> ret;
892 std::vector<Position>
895 std::vector<Position> ret;
900 std::vector<Point2D<double> > into;
902 for (std::vector<
Point2D<double> >::iterator i = into.begin(); i != into.end(); ++i) {
903 ret.push_back(
Position((*i).getX(), (*i).getY()));
909 std::vector<Position>
912 std::vector<Position> ret;
930 if (geo_posE - g.
s > g.
length) {
933 if (geo_posE - g.
s > g.
length) {
936 calcPointOnCurve(&endX, &endY, centerX, centerY, radius, geo_posE - geo_posS);
938 dist += (geo_posE - geo_posS);
940 ret.push_back(
Position(startX, startY));
946 if (geo_posE - (g.
s + g.
length) < 0.001 && geo_posE - (g.
s + g.
length) > -0.001) {
954 std::vector<Position>
957 std::vector<Position> ret;
965 ret.push_back(
Position(g.
x + xnew, g.
y + ynew));
975 double x2 = normx * cos(hdg) - normy * sin(hdg);
976 double y2 = normx * sin(hdg) + normy * cos(hdg);
979 return Position(start.
x() + normx, start.
y() + normy);
996 normX = normX * cos(ad_hdg) + normY * sin(ad_hdg);
997 normY = tmpX * sin(ad_hdg) + normY * cos(ad_hdg);
1000 normX = turn * normY;
1001 normY = -turn * tmpX;
1003 normX = fabs(ad_radius) * normX;
1004 normY = fabs(ad_radius) * normY;
1014 double rotAngle = ad_length / fabs(ad_r);
1015 double vx = *ad_x - ad_centerX;
1016 double vy = *ad_y - ad_centerY;
1026 vx = vx * cos(rotAngle) + turn * vy * sin(rotAngle);
1027 vy = -1 * turn * tmpx * sin(rotAngle) + vy * cos(rotAngle);
1028 *ad_x = vx + ad_centerX;
1029 *ad_y = vy + ad_centerY;
1045 unsigned int sumoLane = 0;
1046 const std::vector<OpenDriveLane>& dirLanesR = lanesByDir.find(
OPENDRIVE_TAG_RIGHT)->second;
1047 for (std::vector<OpenDriveLane>::const_reverse_iterator i = dirLanesR.rbegin(); i != dirLanesR.rend(); ++i) {
1049 laneMap[(*i).id] = sumoLane++;
1052 rightLaneNumber = sumoLane;
1054 const std::vector<OpenDriveLane>& dirLanesL = lanesByDir.find(
OPENDRIVE_TAG_LEFT)->second;
1055 for (std::vector<OpenDriveLane>::const_iterator i = dirLanesL.begin(); i != dirLanesL.end(); ++i) {
1057 laneMap[(*i).id] = sumoLane++;
1060 leftLaneNumber = sumoLane;
1066 std::map<int, int> ret;
1067 const std::vector<OpenDriveLane>& dirLanes = lanesByDir.find(dir)->second;
1068 for (std::vector<OpenDriveLane>::const_reverse_iterator i = dirLanes.rbegin(); i != dirLanes.rend(); ++i) {
1069 std::map<int, int>::const_iterator toP = laneMap.find((*i).id);
1070 if (toP == laneMap.end()) {
1074 int to = (*toP).second;
1077 from = (*i).predecessor;
1080 std::map<int, int>::const_iterator fromP = prev.
laneMap.find(from);
1081 if (fromP != prev.
laneMap.end()) {
1082 from = (*fromP).second;
1088 if (ret.find(from) != ret.end()) {
1092 std::swap(from, to);
1111 if (i != l.
speeds.end()) {
1112 l.
speed = (*i).second;
1119 if (i != l.
speeds.end()) {
1120 l.
speed = (*i).second;
1129 std::set<SUMOReal> speedChangePositions;
1132 for (std::vector<std::pair<SUMOReal, SUMOReal> >::const_iterator l = (*k).speeds.begin(); l != (*k).speeds.end(); ++l) {
1133 speedChangePositions.insert((*l).first);
1134 if ((*l).first == 0) {
1135 (*k).speed = (*l).second;
1140 for (std::vector<std::pair<SUMOReal, SUMOReal> >::const_iterator l = (*k).speeds.begin(); l != (*k).speeds.end(); ++l) {
1141 speedChangePositions.insert((*l).first);
1142 if ((*l).first == 0) {
1143 (*k).speed = (*l).second;
1148 if (speedChangePositions.size() == 0) {
1151 if (*speedChangePositions.begin() > 0) {
1152 speedChangePositions.insert(0);
1155 for (std::set<SUMOReal>::iterator i = speedChangePositions.begin(); i != speedChangePositions.end(); ++i) {
1156 if (i == speedChangePositions.begin()) {
1157 newSections.push_back(*
this);
1159 newSections.push_back(buildLaneSection(*i));
1163 for (
int i = 0; i != (
int)newSections.size(); ++i) {
1165 std::map<OpenDriveXMLTag, std::vector<OpenDriveLane> >& lanesByDir = ls.
lanesByDir;
1166 for (std::map<
OpenDriveXMLTag, std::vector<OpenDriveLane> >::iterator k = lanesByDir.begin(); k != lanesByDir.end(); ++k) {
1167 std::vector<OpenDriveLane>& lanes = (*k).second;
1168 for (
int j = 0; j != (
int)lanes.size(); ++j) {
1174 l.
speed = newSections[i - 1].lanesByDir[(*k).first][j].speed;
1192 for (std::vector<OpenDriveSignal>::const_iterator i = signals.begin(); i != signals.end(); ++i) {
1194 if ((*i).type ==
"301" || (*i).type ==
"306") {
1197 if ((*i).type ==
"205") {
1234 if (majorVersion != 1 || minorVersion != 2) {
1288 std::vector<SUMOReal> vals;
1293 std::vector<SUMOReal> vals;
1300 std::vector<SUMOReal> vals;
1306 std::vector<SUMOReal> vals;
1372 WRITE_ERROR(
"In laneLink-element: incoming road '" + c.fromEdge +
"' is not known.");
1384 l.width =
MAX2(l.width, width);
1441 const std::string& elementID,
1442 const std::string& contactPoint) {
1445 if (elementType ==
"road") {
1447 }
else if (elementType ==
"junction") {
1451 if (contactPoint ==
"start") {
1453 }
else if (contactPoint ==
"end") {