56 #ifdef CHECK_MEMORY_LEAKS
58 #endif // CHECK_MEMORY_LEAKS
156 std::map<std::string, OpenDriveEdge*> edges;
159 std::vector<std::string> files = oc.
getStringVector(
"opendrive-files");
160 for (std::vector<std::string>::const_iterator file = files.begin(); file != files.end(); ++file) {
162 WRITE_ERROR(
"Could not open opendrive file '" + *file +
"'.");
171 std::map<std::string, OpenDriveEdge*> innerEdges, outerEdges;
172 for (std::map<std::string, OpenDriveEdge*>::iterator i = edges.begin(); i != edges.end(); ++i) {
173 if ((*i).second->isInner) {
174 innerEdges[(*i).first] = (*i).second;
176 outerEdges[(*i).first] = (*i).second;
189 std::map<std::string, Boundary> posMap;
190 std::map<std::string, std::string> edge2junction;
192 for (std::map<std::string, OpenDriveEdge*>::iterator i = innerEdges.begin(); i != innerEdges.end(); ++i) {
196 if (posMap.find(e->
junction) == posMap.end()) {
202 for (std::map<std::string, Boundary>::iterator i = posMap.begin(); i != posMap.end(); ++i) {
204 throw ProcessError(
"Could not add node '" + (*i).first +
"'.");
208 for (std::map<std::string, OpenDriveEdge*>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
210 for (std::vector<OpenDriveLink>::iterator j = e->
links.begin(); j != e->
links.end(); ++j) {
217 if (edge2junction.find(l.
elementID) != edge2junction.end()) {
229 for (std::map<std::string, OpenDriveEdge*>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
231 for (std::vector<OpenDriveLink>::iterator j = e->
links.begin(); j != e->
links.end(); ++j) {
238 std::string id1 = e->
id;
243 std::string nid = id1 +
"." + id2;
248 throw ProcessError(
"Could not build node '" + nid +
"'.");
266 for (std::map<std::string, OpenDriveEdge*>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
268 if (e->
to != 0 && e->
from != 0) {
271 for (std::map<std::string, OpenDriveEdge*>::iterator j = innerEdges.begin(); j != innerEdges.end(); ++j) {
273 for (std::vector<OpenDriveLink>::iterator k = ie->
links.begin(); k != ie->
links.end(); ++k) {
279 std::string nid = edge2junction[ie->
id];
292 for (std::map<std::string, OpenDriveEdge*>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
295 const std::string nid = e->
id +
".begin";
299 const std::string nid = e->
id +
".end";
311 for (std::map<std::string, OpenDriveEdge*>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
316 if (noLanesRight == 0 && noLanesLeft == 0) {
344 WRITE_WARNING(
"Edge '" + e->
id +
"' has to be split as it connects same junctions.")
362 (*j).buildLaneMapping();
363 std::string
id = e->
id;
364 if (sFrom != e->
from || sTo != e->
to) {
371 if (rightLanesSection > 0) {
372 currRight =
new NBEdge(
"-" +
id, sFrom, sTo,
"", defaultSpeed, rightLanesSection, priorityR,
378 for (std::vector<OpenDriveLane>::const_iterator k = lanes.begin(); k != lanes.end(); ++k) {
379 std::map<int, int>::const_iterator lp = (*j).laneMap.find((*k).id);
380 if (lp != (*j).laneMap.end()) {
381 int sumoLaneIndex = lp->second;
391 sumoLane.
width = width;
397 if (prevRight != 0) {
399 for (std::map<int, int>::const_iterator k = connections.begin(); k != connections.end(); ++k) {
403 prevRight = currRight;
409 if (leftLanesSection > 0) {
410 currLeft =
new NBEdge(
id, sTo, sFrom,
"", defaultSpeed, leftLanesSection, priorityL,
416 for (std::vector<OpenDriveLane>::const_iterator k = lanes.begin(); k != lanes.end(); ++k) {
417 std::map<int, int>::const_iterator lp = (*j).laneMap.find((*k).id);
418 if (lp != (*j).laneMap.end()) {
419 int sumoLaneIndex = lp->second;
429 sumoLane.
width = width;
436 std::map<int, int> connections = (*j).getInnerConnections(
OPENDRIVE_TAG_LEFT, *(j - 1));
437 for (std::map<int, int>::const_iterator k = connections.begin(); k != connections.end(); ++k) {
456 for (std::map<std::string, OpenDriveEdge*>::iterator i = edges.begin(); i != edges.end(); ++i) {
460 std::vector<Connection> connections2;
461 for (std::map<std::string, OpenDriveEdge*>::iterator j = edges.begin(); j != edges.end(); ++j) {
462 const std::set<Connection>& conns = (*j).second->connections;
464 for (std::set<Connection>::const_iterator i = conns.begin(); i != conns.end(); ++i) {
465 if (innerEdges.find((*i).fromEdge) != innerEdges.end()) {
469 if (innerEdges.find((*i).toEdge) != innerEdges.end()) {
472 connections2.push_back(*i);
477 for (std::vector<Connection>::const_iterator i = connections2.begin(); i != connections2.end(); ++i) {
478 std::string fromEdge = (*i).fromEdge;
479 if (edges.find(fromEdge) == edges.end()) {
480 WRITE_WARNING(
"While setting connections: from-edge '" + fromEdge +
"' is not known.");
484 int fromLane = (*i).fromLane;
485 bool fromLast = ((*i).fromCP ==
OPENDRIVE_CP_END) ^ ((*i).fromLane > 0 && !(*i).all);
488 std::string toEdge = (*i).toEdge;
489 if (edges.find(toEdge) == edges.end()) {
490 WRITE_WARNING(
"While setting connections: to-edge '" + toEdge +
"' is not known.");
495 int toLane = (*i).toLane;
500 fromLane = toLast ? odTo->
laneSections.back().laneMap.begin()->first : odTo->
laneSections[0].laneMap.begin()->first;
516 WRITE_WARNING(
"Could not find fromEdge representation of '' in connection ''.");
519 WRITE_WARNING(
"Could not find fromEdge representation of '' in connection ''.");
521 if (from == 0 || to == 0) {
527 if ((*i).origID !=
"") {
530 for (std::vector<NBEdge::Connection>::iterator k = cons.begin(); k != cons.end(); ++k) {
531 if ((*k).fromLane == fromLane && (*k).toEdge == to && (*k).toLane == toLane) {
532 (*k).origID = (*i).origID +
" " +
toString((*i).origLane);
539 if (oc.
exists(
"geometry.min-dist") && oc.
isSet(
"geometry.min-dist")) {
540 oc.
unSet(
"geometry.min-dist");
542 for (std::map<std::string, OpenDriveEdge*>::iterator i = edges.begin(); i != edges.end(); ++i) {
557 const std::set<Connection>& conts = dest->
connections;
558 for (std::set<Connection>::const_iterator i = conts.begin(); i != conts.end(); ++i) {
559 if (innerEdges.find((*i).toEdge) != innerEdges.end()) {
560 std::vector<Connection> t;
562 for (std::vector<Connection>::const_iterator j = t.begin(); j != t.end(); ++j) {
573 if ((*i).fromLane == c.
toLane) {
590 for (std::vector<OpenDriveLink>::iterator i = e.
links.begin(); i != e.
links.end(); ++i) {
598 std::string edgeID = e.
id;
603 for (std::vector<OpenDriveLane>::const_iterator j = lanes.begin(); j != lanes.end(); ++j) {
620 if (edges.find(c.
fromEdge) == edges.end()) {
621 WRITE_ERROR(
"While setting connections: incoming road '" + c.
fromEdge +
"' is not known.");
630 for (std::vector<OpenDriveLane>::const_iterator j = lanes.begin(); j != lanes.end(); ++j) {
647 if (edges.find(c.
fromEdge) == edges.end()) {
648 WRITE_ERROR(
"While setting connections: incoming road '" + c.
fromEdge +
"' is not known.");
671 if (!nc.
insert(
id, pos)) {
685 throw ProcessError(
"Could not find node '" + nodeID +
"'.");
688 if (e.
to != 0 && e.
to != n) {
709 for (std::map<std::string, OpenDriveEdge*>::iterator i = edges.begin(); i != edges.end(); ++i) {
711 for (std::vector<OpenDriveGeometry>::iterator j = e.
geometries.begin(); j != e.
geometries.end(); ++j) {
713 std::vector<Position> geom;
725 if (geom.size() == 1) {
732 for (std::vector<Position>::iterator k = geom.begin(); k != geom.end(); ++k) {
736 if (oc.
exists(
"geometry.min-dist") && oc.
isSet(
"geometry.min-dist")) {
739 for (
unsigned int j = 0; j < e.
geom.size(); ++j) {
748 std::vector<Position>
751 std::vector<Position> ret;
758 std::vector<Position>
761 std::vector<Position> ret;
781 if (geo_posE - g.
s > g.
length) {
784 if (geo_posE - g.
s > g.
length) {
787 calcPointOnCurve(&endX, &endY, centerX, centerY, radius, geo_posE - geo_posS);
789 dist += (geo_posE - geo_posS);
790 if (curvature > 0.0) {
791 hdgE = g.
hdg + dist / fabs(radius);
793 hdgE = g.
hdg - dist / fabs(radius);
796 ret.push_back(
Position(startX, startY));
803 if (geo_posE - (g.
s + g.
length) < 0.001 && geo_posE - (g.
s + g.
length) > -0.001) {
811 std::vector<Position>
814 std::vector<Position> ret;
823 ret.push_back(
Position(g.
x + xnew, g.
y + ynew));
833 double x2 = normx * cos(hdg) - normy * sin(hdg);
834 double y2 = normx * sin(hdg) + normy * cos(hdg);
837 return Position(start.
x() + normx, start.
y() + normy);
854 normX = normX * cos(ad_hdg) + normY * sin(ad_hdg);
855 normY = tmpX * sin(ad_hdg) + normY * cos(ad_hdg);
858 normX = normX * cos(90 *
PI / 180) + turn * normY * sin(90 *
PI / 180);
859 normY = -1 * turn * tmpX * sin(90 *
PI / 180) + normY * cos(90 *
PI / 180);
861 normX = fabs(ad_radius) * normX;
862 normY = fabs(ad_radius) * normY;
872 double rotAngle = ad_length / fabs(ad_r);
873 double vx = *ad_x - ad_centerX;
874 double vy = *ad_y - ad_centerY;
884 vx = vx * cos(rotAngle) + turn * vy * sin(rotAngle);
885 vy = -1 * turn * tmpx * sin(rotAngle) + vy * cos(rotAngle);
886 *ad_x = vx + ad_centerX;
887 *ad_y = vy + ad_centerY;
903 unsigned int laneNum = 0;
904 const std::vector<OpenDriveLane>& dirLanes = lanesByDir.find(dir)->second;
905 for (std::vector<OpenDriveLane>::const_iterator i = dirLanes.begin(); i != dirLanes.end(); ++i) {
916 unsigned int sumoLane = 0;
917 const std::vector<OpenDriveLane>& dirLanesR = lanesByDir.find(
OPENDRIVE_TAG_RIGHT)->second;
918 for (std::vector<OpenDriveLane>::const_reverse_iterator i = dirLanesR.rbegin(); i != dirLanesR.rend(); ++i) {
920 laneMap[(*i).id] = sumoLane++;
924 const std::vector<OpenDriveLane>& dirLanesL = lanesByDir.find(
OPENDRIVE_TAG_LEFT)->second;
925 for (std::vector<OpenDriveLane>::const_iterator i = dirLanesL.begin(); i != dirLanesL.end(); ++i) {
927 laneMap[(*i).id] = sumoLane++;
935 std::map<int, int> ret;
936 const std::vector<OpenDriveLane>& dirLanes = lanesByDir.find(dir)->second;
937 for (std::vector<OpenDriveLane>::const_reverse_iterator i = dirLanes.rbegin(); i != dirLanes.rend(); ++i) {
938 std::map<int, int>::const_iterator toP = laneMap.find((*i).id);
939 if (toP == laneMap.end()) {
943 int to = (*toP).second;
946 from = (*i).predecessor;
949 std::map<int, int>::const_iterator fromP = prev.
laneMap.find(from);
950 if (fromP != prev.
laneMap.end()) {
951 from = (*fromP).second;
957 if (ret.find(from) != ret.end()) {
978 unsigned int maxLaneNum = 0;
979 for (std::vector<OpenDriveLaneSection>::const_iterator i = laneSections.begin(); i != laneSections.end(); ++i) {
980 maxLaneNum =
MAX2(maxLaneNum, (*i).getLaneNumber(dir));
989 for (std::vector<OpenDriveSignal>::const_iterator i = signals.begin(); i != signals.end(); ++i) {
991 if ((*i).type ==
"301" || (*i).type ==
"306") {
994 if ((*i).type ==
"205") {
1031 if (majorVersion != 1 || minorVersion != 2) {
1085 std::vector<SUMOReal> vals;
1090 std::vector<SUMOReal> vals;
1097 std::vector<SUMOReal> vals;
1103 std::vector<SUMOReal> vals;
1168 WRITE_ERROR(
"In laneLink-element: incoming road '" + c.fromEdge +
"' is not known.");
1180 l.width =
MAX2(l.width, width);
1232 const std::string& elementID,
1233 const std::string& contactPoint) {
1236 if (elementType ==
"road") {
1238 }
else if (elementType ==
"junction") {
1242 if (contactPoint ==
"start") {
1244 }
else if (contactPoint ==
"end") {