56 #ifdef CHECK_MEMORY_LEAKS
58 #endif // CHECK_MEMORY_LEAKS
141 std::vector<OpenDriveEdge> innerEdges, outerEdges;
144 std::vector<std::string> files = oc.
getStringVector(
"opendrive-files");
145 for (std::vector<std::string>::const_iterator file = files.begin(); file != files.end(); ++file) {
147 WRITE_ERROR(
"Could not open opendrive file '" + *file +
"'.");
165 std::map<std::string, Boundary> posMap;
166 std::map<std::string, std::string> edge2junction;
168 for (std::vector<OpenDriveEdge>::iterator i = innerEdges.begin(); i != innerEdges.end(); ++i) {
172 if (posMap.find(e.
junction) == posMap.end()) {
178 for (std::map<std::string, Boundary>::iterator i = posMap.begin(); i != posMap.end(); ++i) {
180 throw ProcessError(
"Could not add node '" + (*i).first +
"'.");
184 for (std::vector<OpenDriveEdge>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
186 for (std::vector<OpenDriveLink>::iterator j = e.
links.begin(); j != e.
links.end(); ++j) {
193 if (edge2junction.find(l.
elementID) != edge2junction.end()) {
205 for (std::vector<OpenDriveEdge>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
207 for (std::vector<OpenDriveLink>::iterator j = e.
links.begin(); j != e.
links.end(); ++j) {
214 std::string id1 = e.
id;
219 std::string nid = id1 +
"." + id2;
224 throw ProcessError(
"Could not build node '" + nid +
"'.");
242 for (std::vector<OpenDriveEdge>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
244 if (e.
to != 0 && e.
from != 0) {
247 for (std::vector<OpenDriveEdge>::iterator j = innerEdges.begin(); j != innerEdges.end(); ++j) {
249 for (std::vector<OpenDriveLink>::iterator k = ie.
links.begin(); k != ie.
links.end(); ++k) {
255 std::string nid = edge2junction[ie.
id];
268 for (std::vector<OpenDriveEdge>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
271 std::string nid = e.
id +
".begin";
276 std::string nid = e.
id +
".end";
286 std::map<NBEdge*, std::map<int, int> > fromLaneMap;
287 std::map<NBEdge*, std::map<int, int> > toLaneMap;
290 for (std::vector<OpenDriveEdge>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
296 if (noLanesRight != 0 || noLanesLeft != 0) {
301 if (noLanesRight > 0) {
306 throw ProcessError(
"Could not add edge '" + std::string(
"-") + e.
id +
"'.");
308 for (
unsigned int j = 0; j < noLanesRight; ++j) {
315 if (noLanesLeft > 0) {
323 for (
unsigned int j = 0; j < noLanesLeft; ++j) {
335 std::vector<Connection> connections;
338 for (std::vector<OpenDriveEdge>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
340 for (std::vector<OpenDriveLink>::iterator j = e.
links.begin(); j != e.
links.end(); ++j) {
346 if (edge2junction.find(l.
elementID) != edge2junction.end()) {
354 if (p == outerEdges.end()) {
357 std::string id1 = e.
id;
358 std::string id2 = (*p).id;
362 std::string nid = id1 +
"." + id2;
373 for (std::vector<OpenDriveEdge>::iterator i = innerEdges.begin(); i != innerEdges.end(); ++i) {
375 std::string pred, succ;
377 for (std::vector<OpenDriveLink>::iterator j = e.
links.begin(); j != e.
links.end(); ++j) {
381 std::cout <<
"unsupported" << std::endl;
384 if (edge2junction.find(l.
elementID) != edge2junction.end()) {
386 std::cout <<
"unsupported" << std::endl;
391 std::cout <<
"double succ" << std::endl;
397 std::cout <<
"double pred" << std::endl;
405 std::cout <<
"Both dirs given!" << std::endl;
408 bool isReversed =
false;
415 if (succ ==
"" || pred ==
"") {
416 std::cout <<
"Missing edge." << std::endl;
419 std::vector<OpenDriveEdge>::iterator predEdge = std::find_if(outerEdges.begin(), outerEdges.end(),
edge_by_id_finder(pred));
420 if (predEdge == outerEdges.end()) {
423 std::vector<OpenDriveEdge>::iterator succEdge = std::find_if(outerEdges.begin(), outerEdges.end(),
edge_by_id_finder(succ));
424 if (succEdge == outerEdges.end()) {
427 NBEdge* fromEdge, *toEdge;
450 connections.push_back(c);
453 for (std::vector<Connection>::const_iterator i = connections.begin(); i != connections.end(); ++i) {
454 if ((*i).from == 0 || (*i).to == 0) {
455 std::cout <<
"Nope." << std::endl;
458 (*i).from->addEdge2EdgeConnection((*i).to);
459 std::map<int, int> fromMap = fromLaneMap[(*i).from];
460 std::map<int, int> toMap = fromLaneMap[(*i).to];
462 for (std::vector<std::pair<int, int> >::const_iterator j = (*i).lanes.begin(); j != (*i).lanes.end(); ++j, --index) {
463 int fromLane = fromMap[(*j).first];
464 int toLane = toMap[(*j).second];
465 if (static_cast<unsigned int>(fromLane) >= (*i).from->getNumLanes() || fromLane < 0) {
466 std::cout <<
"False " << std::endl;
468 if (static_cast<unsigned int>(toLane) >= (*i).to->getNumLanes() || toLane < 0) {
469 std::cout <<
"False " << std::endl;
474 std::vector<NBEdge::Connection>& cons = (*i).from->getConnections();
475 for (std::vector<NBEdge::Connection>::iterator k = cons.begin(); k != cons.end(); ++k) {
476 if ((*k).fromLane == fromLane && (*k).toEdge == (*i).to && (*k).toLane == toLane) {
477 (*k).origID = (*i).id +
" " +
toString(index);
490 LinkType lt,
const std::string& via,
491 std::vector<NIImporter_OpenDrive::Connection>& connections) {
515 if (from == 0 && to == 0) {
519 connections.push_back(c);
526 std::vector<NIImporter_OpenDrive::Connection>& connections) {
529 if (fromEdge == 0 || !node->
hasIncoming(fromEdge)) {
536 if (fromEdge != 0 && toEdge != 0) {
541 connections.push_back(c);
545 if (fromEdge == 0 || !node->
hasOutgoing(fromEdge)) {
552 if (fromEdge != 0 && toEdge != 0) {
557 connections.push_back(c);
567 const std::vector<OpenDriveLane>& fromLanes = fromLS.
lanesByDir.find(fromLaneDir)->second;
569 const std::vector<OpenDriveLane>& toLanes = toLS.
lanesByDir.find(toLaneDir)->second;
573 for (std::vector<OpenDriveLane>::const_iterator i = fromLanes.begin(); i != fromLanes.end(); ++i) {
578 c.
lanes.push_back(std::make_pair((*i).id, (*i).successor));
581 c.
lanes.push_back(std::make_pair((*i).id, (*i).predecessor));
584 for (std::vector<OpenDriveLane>::const_iterator i = toLanes.begin(); i != toLanes.end(); ++i) {
589 c.
lanes.push_back(std::make_pair((*i).predecessor, (*i).id));
592 c.
lanes.push_back(std::make_pair((*i).successor, (*i).id));
604 setLaneConnections(from2via, from, fromAtBegin, fromLaneDir, via, viaIsReversed, viaLaneDir);
607 for (std::vector<std::pair<int, int> >::const_iterator i = from2via.
lanes.begin(); i != from2via.
lanes.end(); ++i) {
608 int fromLane = (*i).first;
609 int viaLane = (*i).second;
610 for (std::vector<std::pair<int, int> >::const_iterator j = via2to.
lanes.begin(); j != via2to.
lanes.end(); ++j) {
611 if ((*j).first == viaLane) {
612 c.
lanes.push_back(std::make_pair(fromLane, (*j).second));
625 if (!nc.
insert(
id, pos)) {
639 throw ProcessError(
"Could not find node '" + nodeID +
"'.");
642 if (e.
to != 0 && e.
to != n) {
661 const std::string& edgeID,
662 const std::string& nodeID) {
679 const std::string& edgeID,
680 const std::string& nodeID) {
696 for (std::vector<OpenDriveEdge>::iterator i = edges.begin(); i != edges.end(); ++i) {
698 for (std::vector<OpenDriveGeometry>::iterator j = e.
geometries.begin(); j != e.
geometries.end(); ++j) {
700 std::vector<Position> geom;
716 for (std::vector<Position>::iterator k = geom.begin(); k != geom.end(); ++k) {
720 for (
unsigned int j = 0; j < e.
geom.
size(); ++j) {
728 std::vector<Position>
731 std::vector<Position> ret;
738 std::vector<Position>
741 std::vector<Position> ret;
761 if (geo_posE - g.
s > g.
length) {
764 if (geo_posE - g.
s > g.
length) {
767 calcPointOnCurve(&endX, &endY, centerX, centerY, radius, geo_posE - geo_posS);
769 dist += (geo_posE - geo_posS);
770 if (curvature > 0.0) {
771 hdgE = g.
hdg + dist / fabs(radius);
773 hdgE = g.
hdg - dist / fabs(radius);
776 ret.push_back(
Position(startX, startY));
783 if (geo_posE - (g.
s + g.
length) < 0.001 && geo_posE - (g.
s + g.
length) > -0.001) {
791 std::vector<Position>
795 std::vector<Position> ret;
804 double x2 = normx * cos(hdg) - normy * sin(hdg);
805 double y2 = normx * sin(hdg) + normy * cos(hdg);
808 return Position(start.
x() + normx, start.
y() + normy);
825 normX = normX * cos(ad_hdg) + normY * sin(ad_hdg);
826 normY = tmpX * sin(ad_hdg) + normY * cos(ad_hdg);
829 normX = normX * cos(90 *
PI / 180) + turn * normY * sin(90 *
PI / 180);
830 normY = -1 * turn * tmpX * sin(90 *
PI / 180) + normY * cos(90 *
PI / 180);
832 normX = fabs(ad_radius) * normX;
833 normY = fabs(ad_radius) * normY;
843 double rotAngle = ad_length / fabs(ad_r);
844 double vx = *ad_x - ad_centerX;
845 double vy = *ad_y - ad_centerY;
855 vx = vx * cos(rotAngle) + turn * vy * sin(rotAngle);
856 vy = -1 * turn * tmpx * sin(rotAngle) + vy * cos(rotAngle);
857 *ad_x = vx + ad_centerX;
858 *ad_y = vy + ad_centerY;
874 unsigned int laneNum = 0;
875 const std::vector<OpenDriveLane>& dirLanes = lanesByDir.find(dir)->second;
876 for (std::vector<OpenDriveLane>::const_iterator i = dirLanes.begin(); i != dirLanes.end(); ++i) {
887 std::map<int, int> ret;
888 unsigned int sumoLane = 0;
889 const std::vector<OpenDriveLane>& dirLanes = lanesByDir.find(dir)->second;
891 for (std::vector<OpenDriveLane>::const_reverse_iterator i = dirLanes.rbegin(); i != dirLanes.rend(); ++i) {
893 ret[(*i).id] = sumoLane++;
897 for (std::vector<OpenDriveLane>::const_iterator i = dirLanes.begin(); i != dirLanes.end(); ++i) {
899 ret[(*i).id] = sumoLane++;
912 unsigned int maxLaneNum = 0;
913 for (std::vector<OpenDriveLaneSection>::const_iterator i = laneSections.begin(); i != laneSections.end(); ++i) {
914 maxLaneNum =
MAX2(maxLaneNum, (*i).getLaneNumber(dir));
924 for (std::vector<OpenDriveSignal>::const_iterator i = signals.begin(); i != signals.end(); ++i) {
926 if ((*i).type ==
"301" || (*i).type ==
"306") {
929 if ((*i).type ==
"205") {
949 std::vector<OpenDriveEdge>& innerEdges,
950 std::vector<OpenDriveEdge>& outerEdges)
968 if (majorVersion != 1 || minorVersion != 2) {
1022 std::vector<SUMOReal> vals;
1027 std::vector<SUMOReal> vals;
1034 std::vector<SUMOReal> vals;
1040 std::vector<SUMOReal> vals;
1108 const std::string& elementID,
1109 const std::string& contactPoint) {
1112 if (elementType ==
"road") {
1114 }
else if (elementType ==
"junction") {
1118 if (contactPoint ==
"start") {
1120 }
else if (contactPoint ==
"end") {