int NWWriter_OpenDrive::writeInternalEdge(OutputDevice& device, OutputDevice& junctionDevice, const NBEdge* inEdge, int nodeID, int edgeID, int inEdgeID, int outEdgeID, int connectionID, const std::vector<NBEdge::Connection>& parallel, const bool isOuterEdge, const double straightThresh, const std::string& centerMark) { assert(parallel.size() != 0); const NBEdge::Connection& cLeft = parallel.back(); const NBEdge* outEdge = cLeft.toEdge; PositionVector begShape = getLeftLaneBorder(inEdge, cLeft.fromLane); PositionVector endShape = getLeftLaneBorder(outEdge, cLeft.toLane); //std::cout << "computing reference line for internal lane " << cLeft.getInternalLaneID() << " begLane=" << inEdge->getLaneShape(cLeft.fromLane) << " endLane=" << outEdge->getLaneShape(cLeft.toLane) << "\n"; double length; double laneOffset = 0; PositionVector fallBackShape; fallBackShape.push_back(begShape.back()); fallBackShape.push_back(endShape.front()); const bool turnaround = inEdge->isTurningDirectionAt(outEdge); bool ok = true; PositionVector init = NBNode::bezierControlPoints(begShape, endShape, turnaround, 25, 25, ok, 0, straightThresh); if (init.size() == 0) { length = fallBackShape.length2D(); // problem with turnarounds is known, method currently returns 'ok' (#2539) if (!ok) { WRITE_WARNING("Could not compute smooth shape from lane '" + inEdge->getLaneID(cLeft.fromLane) + "' to lane '" + outEdge->getLaneID(cLeft.toLane) + "'. Use option 'junctions.scurve-stretch' or increase radius of junction '" + inEdge->getToNode()->getID() + "' to fix this."); } else if (length <= NUMERICAL_EPS) { // left-curving geometry-like edges must use the right // side as reference line and shift begShape = getRightLaneBorder(inEdge, cLeft.fromLane); endShape = getRightLaneBorder(outEdge, cLeft.toLane); init = NBNode::bezierControlPoints(begShape, endShape, turnaround, 25, 25, ok, 0, straightThresh); if (init.size() != 0) { length = bezier(init, 12).length2D(); laneOffset = outEdge->getLaneWidth(cLeft.toLane); //std::cout << " internalLane=" << cLeft.getInternalLaneID() << " length=" << length << "\n"; } } } else { length = bezier(init, 12).length2D(); } junctionDevice << " <connection id=\"" << connectionID << "\" incomingRoad=\"" << inEdgeID << "\" connectingRoad=\"" << edgeID << "\" contactPoint=\"start\">\n"; device.openTag("road"); device.writeAttr("name", cLeft.id); device.setPrecision(8); // length requires higher precision device.writeAttr("length", MAX2(POSITION_EPS, length)); device.setPrecision(gPrecision); device.writeAttr("id", edgeID); device.writeAttr("junction", nodeID); device.openTag("link"); device.openTag("predecessor"); device.writeAttr("elementType", "road"); device.writeAttr("elementId", inEdgeID); device.writeAttr("contactPoint", "end"); device.closeTag(); device.openTag("successor"); device.writeAttr("elementType", "road"); device.writeAttr("elementId", outEdgeID); device.writeAttr("contactPoint", "start"); device.closeTag(); device.closeTag(); device.openTag("type").writeAttr("s", 0).writeAttr("type", "town").closeTag(); device.openTag("planView"); device.setPrecision(8); // geometry hdg requires higher precision OutputDevice_String elevationOSS(false, 3); elevationOSS.setPrecision(8); #ifdef DEBUG_SMOOTH_GEOM if (DEBUGCOND) { std::cout << "write planview for internal edge " << cLeft.id << " init=" << init << " fallback=" << fallBackShape << " begShape=" << begShape << " endShape=" << endShape << "\n"; } #endif if (init.size() == 0) { writeGeomLines(fallBackShape, device, elevationOSS); } else { writeGeomPP3(device, elevationOSS, init, length); } device.setPrecision(gPrecision); device.closeTag(); writeElevationProfile(fallBackShape, device, elevationOSS); device << " <lateralProfile/>\n"; device << " <lanes>\n"; if (laneOffset != 0) { device << " <laneOffset s=\"0\" a=\"" << laneOffset << "\" b=\"0\" c=\"0\" d=\"0\"/>\n"; } device << " <laneSection s=\"0\">\n"; writeEmptyCenterLane(device, centerMark, 0); device << " <right>\n"; for (int j = (int)parallel.size(); --j >= 0;) { const NBEdge::Connection& c = parallel[j]; const int fromIndex = c.fromLane - inEdge->getNumLanes(); const int toIndex = c.toLane - outEdge->getNumLanes(); device << " <lane id=\"-" << parallel.size() - j << "\" type=\"" << getLaneType(outEdge->getPermissions(c.toLane)) << "\" level=\"true\">\n"; device << " <link>\n"; device << " <predecessor id=\"" << fromIndex << "\"/>\n"; device << " <successor id=\"" << toIndex << "\"/>\n"; device << " </link>\n"; device << " <width sOffset=\"0\" a=\"" << outEdge->getLaneWidth(c.toLane) << "\" b=\"0\" c=\"0\" d=\"0\"/>\n"; std::string markType = "broken"; if (inEdge->isTurningDirectionAt(outEdge)) { markType = "none"; } else if (c.fromLane == 0 && c.toLane == 0 && isOuterEdge) { // solid road mark at the outer border markType = "solid"; } else if (isOuterEdge && j > 0 && (outEdge->getPermissions(parallel[j - 1].toLane) & ~(SVC_PEDESTRIAN | SVC_BICYCLE)) == 0) { // solid road mark to the left of sidewalk or bicycle lane markType = "solid"; } else if (!inEdge->getToNode()->geometryLike()) { // draw shorter road marks to indicate turning paths LinkDirection dir = inEdge->getToNode()->getDirection(inEdge, outEdge, OptionsCont::getOptions().getBool("lefthand")); if (dir == LINKDIR_LEFT || dir == LINKDIR_RIGHT || dir == LINKDIR_PARTLEFT || dir == LINKDIR_PARTRIGHT) { // XXX <type><line/><type> is not rendered by odrViewer so cannot be validated // device << " <type name=\"broken\" width=\"0.13\">\n"; // device << " <line length=\"0.5\" space=\"0.5\" tOffset=\"0\" sOffset=\"0\" rule=\"none\"/>\n"; // device << " </type>\n"; markType = "none"; } } device << " <roadMark sOffset=\"0\" type=\"" << markType << "\" weight=\"standard\" color=\"standard\" width=\"0.13\"/>\n"; device << " <speed sOffset=\"0\" max=\"" << c.vmax << "\"/>\n"; device << " </lane>\n"; junctionDevice << " <laneLink from=\"" << fromIndex << "\" to=\"" << toIndex << "\"/>\n"; connectionID++; } device << " </right>\n"; device << " </laneSection>\n"; device << " </lanes>\n"; device << " <objects/>\n"; device << " <signals/>\n"; device.closeTag(); junctionDevice << " </connection>\n"; return connectionID; }
bool NWWriter_OpenDrive::writeGeomSmooth(const PositionVector& shape, double speed, OutputDevice& device, OutputDevice& elevationDevice, double straightThresh, double& length) { #ifdef DEBUG_SMOOTH_GEOM if (DEBUGCOND) { std::cout << "writeGeomSmooth\n n=" << shape.size() << " shape=" << toString(shape) << "\n"; } #endif bool ok = true; const double longThresh = speed; // 16.0; // make user-configurable (should match the sampling rate of the source data) const double curveCutout = longThresh / 2; // 8.0; // make user-configurable (related to the maximum turning rate) // the length of the segment that is added for cutting a corner can be bounded by 2*curveCutout (prevent the segment to be classified as 'long') assert(longThresh >= 2 * curveCutout); assert(shape.size() > 2); // add intermediate points wherever there is a strong angular change between long segments // assume the geometry is simplified so as not to contain consecutive colinear points PositionVector shape2 = shape; double maxAngleDiff = 0; double offset = 0; for (int j = 1; j < (int)shape.size() - 1; ++j) { //const double hdg = shape.angleAt2D(j); const Position& p0 = shape[j - 1]; const Position& p1 = shape[j]; const Position& p2 = shape[j + 1]; const double dAngle = fabs(GeomHelper::angleDiff(p0.angleTo2D(p1), p1.angleTo2D(p2))); const double length1 = p0.distanceTo2D(p1); const double length2 = p1.distanceTo2D(p2); maxAngleDiff = MAX2(maxAngleDiff, dAngle); #ifdef DEBUG_SMOOTH_GEOM if (DEBUGCOND) { std::cout << " j=" << j << " dAngle=" << RAD2DEG(dAngle) << " length1=" << length1 << " length2=" << length2 << "\n"; } #endif if (dAngle > straightThresh && (length1 > longThresh || j == 1) && (length2 > longThresh || j == (int)shape.size() - 2)) { shape2.insertAtClosest(shape.positionAtOffset2D(offset + length1 - MIN2(length1 - POSITION_EPS, curveCutout))); shape2.insertAtClosest(shape.positionAtOffset2D(offset + length1 + MIN2(length2 - POSITION_EPS, curveCutout))); shape2.removeClosest(p1); } offset += length1; } const int numPoints = (int)shape2.size(); #ifdef DEBUG_SMOOTH_GEOM if (DEBUGCOND) { std::cout << " n=" << numPoints << " shape2=" << toString(shape2) << "\n"; } #endif if (maxAngleDiff < straightThresh) { length = writeGeomLines(shape2, device, elevationDevice, 0); #ifdef DEBUG_SMOOTH_GEOM if (DEBUGCOND) { std::cout << " special case: all lines. maxAngleDiff=" << maxAngleDiff << "\n"; } #endif return ok; } // write the long segments as lines, short segments as curves offset = 0; for (int j = 0; j < numPoints - 1; ++j) { const Position& p0 = shape2[j]; const Position& p1 = shape2[j + 1]; PositionVector line; line.push_back(p0); line.push_back(p1); const double lineLength = line.length2D(); if (lineLength >= longThresh) { offset = writeGeomLines(line, device, elevationDevice, offset); #ifdef DEBUG_SMOOTH_GEOM if (DEBUGCOND) { std::cout << " writeLine=" << toString(line) << "\n"; } #endif } else { // find control points PositionVector begShape; PositionVector endShape; if (j == 0 || j == numPoints - 2) { // keep the angle of the first/last segment but end at the front of the shape begShape = line; begShape.add(p0 - begShape.back()); } else if (j == 1 || p0.distanceTo2D(shape2[j - 1]) > longThresh) { // use the previous segment if it is long or the first one begShape.push_back(shape2[j - 1]); begShape.push_back(p0); } else { // end at p0 with mean angle of the previous and current segment begShape.push_back(shape2[j - 1]); begShape.push_back(p1); begShape.add(p0 - begShape.back()); } if (j == 0 || j == numPoints - 2) { // keep the angle of the first/last segment but start at the end of the shape endShape = line; endShape.add(p1 - endShape.front()); } else if (j == numPoints - 3 || p1.distanceTo2D(shape2[j + 2]) > longThresh) { // use the next segment if it is long or the final one endShape.push_back(p1); endShape.push_back(shape2[j + 2]); } else { // start at p1 with mean angle of the current and next segment endShape.push_back(p0); endShape.push_back(shape2[j + 2]); endShape.add(p1 - endShape.front()); } const double extrapolateLength = MIN2((double)25, lineLength / 4); PositionVector init = NBNode::bezierControlPoints(begShape, endShape, false, extrapolateLength, extrapolateLength, ok, 0, straightThresh); if (init.size() == 0) { // could not compute control points, write line offset = writeGeomLines(line, device, elevationDevice, offset); #ifdef DEBUG_SMOOTH_GEOM if (DEBUGCOND) { std::cout << " writeLine lineLength=" << lineLength << " begShape" << j << "=" << toString(begShape) << " endShape" << j << "=" << toString(endShape) << " init" << j << "=" << toString(init) << "\n"; } #endif } else { // write bezier const double curveLength = bezier(init, 12).length2D(); offset = writeGeomPP3(device, elevationDevice, init, curveLength, offset); #ifdef DEBUG_SMOOTH_GEOM if (DEBUGCOND) { std::cout << " writeCurve lineLength=" << lineLength << " curveLength=" << curveLength << " begShape" << j << "=" << toString(begShape) << " endShape" << j << "=" << toString(endShape) << " init" << j << "=" << toString(init) << "\n"; } #endif } } } length = offset; return ok; }
// =========================================================================== // method definitions // =========================================================================== // --------------------------------------------------------------------------- // static methods // --------------------------------------------------------------------------- void NWWriter_OpenDrive::writeNetwork(const OptionsCont& oc, NBNetBuilder& nb) { // check whether an opendrive-file shall be generated if (!oc.isSet("opendrive-output")) { return; } const NBNodeCont& nc = nb.getNodeCont(); const NBEdgeCont& ec = nb.getEdgeCont(); const bool origNames = oc.getBool("output.original-names"); const SUMOReal straightThresh = DEG2RAD(oc.getFloat("opendrive-output.straight-threshold")); // some internal mapping containers int nodeID = 1; int edgeID = nc.size() * 10; // distinct from node ids StringBijection<int> edgeMap; StringBijection<int> nodeMap; // OutputDevice& device = OutputDevice::getDevice(oc.getString("opendrive-output")); device << "<?xml version=\"1.0\" encoding=\"utf-8\"?>\n"; device.openTag("OpenDRIVE"); time_t now = time(0); std::string dstr(ctime(&now)); const Boundary& b = GeoConvHelper::getFinal().getConvBoundary(); // write header device.openTag("header"); device.writeAttr("revMajor", "1"); device.writeAttr("revMinor", "4"); device.writeAttr("name", ""); device.writeAttr("version", "1.00"); device.writeAttr("date", dstr.substr(0, dstr.length() - 1)); device.writeAttr("north", b.ymax()); device.writeAttr("south", b.ymin()); device.writeAttr("east", b.xmax()); device.writeAttr("west", b.xmin()); /* @note obsolete in 1.4 device.writeAttr("maxRoad", ec.size()); device.writeAttr("maxJunc", nc.size()); device.writeAttr("maxPrg", 0); */ device.closeTag(); // write normal edges (road) for (std::map<std::string, NBEdge*>::const_iterator i = ec.begin(); i != ec.end(); ++i) { const NBEdge* e = (*i).second; // buffer output because some fields are computed out of order OutputDevice_String elevationOSS(false, 3); elevationOSS.setPrecision(8); OutputDevice_String planViewOSS(false, 2); planViewOSS.setPrecision(8); SUMOReal length = 0; planViewOSS.openTag("planView"); planViewOSS.setPrecision(8); // geometry hdg requires higher precision // for the shape we need to use the leftmost border of the leftmost lane const std::vector<NBEdge::Lane>& lanes = e->getLanes(); PositionVector ls = getLeftLaneBorder(e); #ifdef DEBUG_SMOOTH_GEOM if (DEBUGCOND) { std::cout << "write planview for edge " << e->getID() << "\n"; } #endif if (ls.size() == 2 || e->getPermissions() == SVC_PEDESTRIAN) { // foot paths may contain sharp angles length = writeGeomLines(ls, planViewOSS, elevationOSS); } else { bool ok = writeGeomSmooth(ls, e->getSpeed(), planViewOSS, elevationOSS, straightThresh, length); if (!ok) { WRITE_WARNING("Could not compute smooth shape for edge '" + e->getID() + "'."); } } planViewOSS.closeTag(); device.openTag("road"); device.writeAttr("name", StringUtils::escapeXML(e->getStreetName())); device.setPrecision(8); // length requires higher precision device.writeAttr("length", MAX2(POSITION_EPS, length)); device.setPrecision(OUTPUT_ACCURACY); device.writeAttr("id", getID(e->getID(), edgeMap, edgeID)); device.writeAttr("junction", -1); const bool hasSucc = e->getConnections().size() > 0; const bool hasPred = e->getIncomingEdges().size() > 0; if (hasPred || hasSucc) { device.openTag("link"); if (hasPred) { device.openTag("predecessor"); device.writeAttr("elementType", "junction"); device.writeAttr("elementId", getID(e->getFromNode()->getID(), nodeMap, nodeID)); device.closeTag(); } if (hasSucc) { device.openTag("successor"); device.writeAttr("elementType", "junction"); device.writeAttr("elementId", getID(e->getToNode()->getID(), nodeMap, nodeID)); device.closeTag(); } device.closeTag(); } device.openTag("type").writeAttr("s", 0).writeAttr("type", "town").closeTag(); device << planViewOSS.getString(); writeElevationProfile(ls, device, elevationOSS); device << " <lateralProfile/>\n"; device << " <lanes>\n"; device << " <laneSection s=\"0\">\n"; writeEmptyCenterLane(device, "solid", 0.13); device << " <right>\n"; for (int j = e->getNumLanes(); --j >= 0;) { device << " <lane id=\"-" << e->getNumLanes() - j << "\" type=\"" << getLaneType(e->getPermissions(j)) << "\" level=\"true\">\n"; device << " <link/>\n"; // this could be used for geometry-link junctions without u-turn, // predecessor and sucessors would be lane indices, // road predecessor / succesfors would be of type 'road' rather than // 'junction' //device << " <predecessor id=\"-1\"/>\n"; //device << " <successor id=\"-1\"/>\n"; //device << " </link>\n"; device << " <width sOffset=\"0\" a=\"" << e->getLaneWidth(j) << "\" b=\"0\" c=\"0\" d=\"0\"/>\n"; std::string markType = "broken"; if (j == 0) { markType = "solid"; } device << " <roadMark sOffset=\"0\" type=\"" << markType << "\" weight=\"standard\" color=\"standard\" width=\"0.13\"/>\n"; device << " <speed sOffset=\"0\" max=\"" << lanes[j].speed << "\"/>\n"; device << " </lane>\n"; } device << " </right>\n"; device << " </laneSection>\n"; device << " </lanes>\n"; device << " <objects/>\n"; device << " <signals/>\n"; if (origNames) { device << " <userData code=\"sumoId\" value=\"" << e->getID() << "\"/>\n"; } device.closeTag(); checkLaneGeometries(e); } device.lf(); // write junction-internal edges (road). In OpenDRIVE these are called 'paths' or 'connecting roads' for (std::map<std::string, NBNode*>::const_iterator i = nc.begin(); i != nc.end(); ++i) { NBNode* n = (*i).second; const std::vector<NBEdge*>& incoming = (*i).second->getIncomingEdges(); for (std::vector<NBEdge*>::const_iterator j = incoming.begin(); j != incoming.end(); ++j) { const NBEdge* inEdge = *j; const std::vector<NBEdge::Connection>& elv = inEdge->getConnections(); for (std::vector<NBEdge::Connection>::const_iterator k = elv.begin(); k != elv.end(); ++k) { const NBEdge::Connection& c = *k; const NBEdge* outEdge = c.toEdge; if (outEdge == 0) { continue; } const SUMOReal width = c.toEdge->getLaneWidth(c.toLane); const PositionVector begShape = getLeftLaneBorder(inEdge, c.fromLane); const PositionVector endShape = getLeftLaneBorder(outEdge, c.toLane); //std::cout << "computing reference line for internal lane " << c.getInternalLaneID() << " begLane=" << inEdge->getLaneShape(c.fromLane) << " endLane=" << outEdge->getLaneShape(c.toLane) << "\n"; SUMOReal length; PositionVector fallBackShape; fallBackShape.push_back(begShape.back()); fallBackShape.push_back(endShape.front()); const bool turnaround = inEdge->isTurningDirectionAt(outEdge); bool ok = true; PositionVector init = NBNode::bezierControlPoints(begShape, endShape, turnaround, 25, 25, ok, 0, straightThresh); if (init.size() == 0) { length = fallBackShape.length2D(); // problem with turnarounds is known, method currently returns 'ok' (#2539) if (!ok) { WRITE_WARNING("Could not compute smooth shape from lane '" + inEdge->getLaneID(c.fromLane) + "' to lane '" + outEdge->getLaneID(c.toLane) + "'. Use option 'junctions.scurve-stretch' or increase radius of junction '" + inEdge->getToNode()->getID() + "' to fix this."); } } else { length = bezier(init, 12).length2D(); } device.openTag("road"); device.writeAttr("name", c.getInternalLaneID()); device.setPrecision(8); // length requires higher precision device.writeAttr("length", MAX2(POSITION_EPS, length)); device.setPrecision(OUTPUT_ACCURACY); device.writeAttr("id", getID(c.getInternalLaneID(), edgeMap, edgeID)); device.writeAttr("junction", getID(n->getID(), nodeMap, nodeID)); device.openTag("link"); device.openTag("predecessor"); device.writeAttr("elementType", "road"); device.writeAttr("elementId", getID(inEdge->getID(), edgeMap, edgeID)); device.writeAttr("contactPoint", "end"); device.closeTag(); device.openTag("successor"); device.writeAttr("elementType", "road"); device.writeAttr("elementId", getID(outEdge->getID(), edgeMap, edgeID)); device.writeAttr("contactPoint", "start"); device.closeTag(); device.closeTag(); device.openTag("type").writeAttr("s", 0).writeAttr("type", "town").closeTag(); device.openTag("planView"); device.setPrecision(8); // geometry hdg requires higher precision OutputDevice_String elevationOSS(false, 3); #ifdef DEBUG_SMOOTH_GEOM if (DEBUGCOND) { std::cout << "write planview for internal edge " << c.getInternalLaneID() << " init=" << init << " fallback=" << fallBackShape << "\n"; } #endif if (init.size() == 0) { writeGeomLines(fallBackShape, device, elevationOSS); } else { writeGeomPP3(device, elevationOSS, init, length); } device.setPrecision(OUTPUT_ACCURACY); device.closeTag(); writeElevationProfile(fallBackShape, device, elevationOSS); device << " <lateralProfile/>\n"; device << " <lanes>\n"; device << " <laneSection s=\"0\">\n"; writeEmptyCenterLane(device, "none", 0); device << " <right>\n"; device << " <lane id=\"-1\" type=\"" << getLaneType(outEdge->getPermissions(c.toLane)) << "\" level=\"true\">\n"; device << " <link>\n"; device << " <predecessor id=\"-" << inEdge->getNumLanes() - c.fromLane << "\"/>\n"; device << " <successor id=\"-" << outEdge->getNumLanes() - c.toLane << "\"/>\n"; device << " </link>\n"; device << " <width sOffset=\"0\" a=\"" << width << "\" b=\"0\" c=\"0\" d=\"0\"/>\n"; device << " <roadMark sOffset=\"0\" type=\"none\" weight=\"standard\" color=\"standard\" width=\"0.13\"/>\n"; device << " </lane>\n"; device << " </right>\n"; device << " </laneSection>\n"; device << " </lanes>\n"; device << " <objects/>\n"; device << " <signals/>\n"; device.closeTag(); } } } // write junctions (junction) for (std::map<std::string, NBNode*>::const_iterator i = nc.begin(); i != nc.end(); ++i) { NBNode* n = (*i).second; const std::vector<NBEdge*>& incoming = n->getIncomingEdges(); // check if any connections must be written int numConnections = 0; for (std::vector<NBEdge*>::const_iterator j = incoming.begin(); j != incoming.end(); ++j) { numConnections += (int)((*j)->getConnections().size()); } if (numConnections == 0) { continue; } device << " <junction name=\"" << n->getID() << "\" id=\"" << getID(n->getID(), nodeMap, nodeID) << "\">\n"; int index = 0; for (std::vector<NBEdge*>::const_iterator j = incoming.begin(); j != incoming.end(); ++j) { const NBEdge* inEdge = *j; const std::vector<NBEdge::Connection>& elv = inEdge->getConnections(); for (std::vector<NBEdge::Connection>::const_iterator k = elv.begin(); k != elv.end(); ++k) { const NBEdge::Connection& c = *k; const NBEdge* outEdge = c.toEdge; if (outEdge == 0) { continue; } device << " <connection id=\"" << index << "\" incomingRoad=\"" << getID(inEdge->getID(), edgeMap, edgeID) << "\" connectingRoad=\"" << getID(c.getInternalLaneID(), edgeMap, edgeID) << "\" contactPoint=\"start\">\n"; device << " <laneLink from=\"-" << inEdge->getNumLanes() - c.fromLane << "\" to=\"-1" // every connection has its own edge << "\"/>\n"; device << " </connection>\n"; ++index; } } device << " </junction>\n"; } device.closeTag(); device.close(); }