50 #define DEBUGCOND true 52 #define MIN_TURN_DIAMETER 2.0 64 if (!oc.
isSet(
"opendrive-output")) {
69 const bool origNames = oc.
getBool(
"output.original-names");
70 const bool lefthand = oc.
getBool(
"lefthand");
71 const double straightThresh =
DEG2RAD(oc.
getFloat(
"opendrive-output.straight-threshold"));
74 int edgeID = nc.
size() * 10;
79 device <<
"<?xml version=\"1.0\" encoding=\"utf-8\"?>\n";
82 std::string dstr(ctime(&now));
85 device.openTag(
"header");
86 device.writeAttr(
"revMajor",
"1");
87 device.writeAttr(
"revMinor",
"4");
88 device.writeAttr(
"name",
"");
89 device.writeAttr(
"version",
"1.00");
90 device.writeAttr(
"date", dstr.substr(0, dstr.length() - 1));
91 device.writeAttr(
"north", b.
ymax());
92 device.writeAttr(
"south", b.
ymin());
93 device.writeAttr(
"east", b.
xmax());
94 device.writeAttr(
"west", b.
xmin());
103 for (std::map<std::string, NBEdge*>::const_iterator i = ec.
begin(); i != ec.
end(); ++i) {
104 const NBEdge* e = (*i).second;
109 fromNodeID, toNodeID,
110 origNames, straightThresh);
116 for (std::map<std::string, NBNode*>::const_iterator i = nc.
begin(); i != nc.
end(); ++i) {
118 int connectionID = 0;
119 const int nID =
getID(n->
getID(), nodeMap, nodeID);
121 junctionOSS <<
" <junction name=\"" << n->
getID() <<
"\" id=\"" << nID <<
"\">\n";
123 std::vector<NBEdge*> incoming = (*i).second->getIncomingEdges();
125 std::reverse(incoming.begin(), incoming.end());
127 for (
NBEdge* inEdge : incoming) {
128 std::string centerMark =
"none";
129 const int inEdgeID =
getID(inEdge->getID(), edgeMap, edgeID);
131 const NBEdge* outEdge = 0;
132 bool isOuterEdge =
true;
133 int lastFromLane = -1;
134 std::vector<NBEdge::Connection> parallel;
135 std::vector<NBEdge::Connection> connections = inEdge->
getConnections();
137 std::reverse(connections.begin(), connections.end());
140 assert(c.toEdge != 0);
141 if (outEdge != c.toEdge || c.fromLane == lastFromLane) {
147 getID(parallel.back().getInternalLaneID(), edgeMap, edgeID),
149 getID(outEdge->getID(), edgeMap, edgeID),
151 parallel, isOuterEdge, straightThresh, centerMark);
157 lastFromLane = c.fromLane;
158 parallel.push_back(c);
163 if (!parallel.empty()) {
164 if (!lefthand && (n->
geometryLike() || inEdge->isTurningDirectionAt(outEdge))) {
165 centerMark =
"solid";
168 getID(parallel.back().getInternalLaneID(), edgeMap, edgeID),
170 getID(outEdge->getID(), edgeMap, edgeID),
172 parallel, isOuterEdge, straightThresh, centerMark);
177 junctionOSS <<
" </junction>\n";
184 for (std::map<std::string, NBNode*>::const_iterator i = nc.
begin(); i != nc.
end(); ++i) {
188 int numConnections = 0;
189 for (std::vector<NBEdge*>::const_iterator j = incoming.begin(); j != incoming.end(); ++j) {
190 numConnections += (int)((*j)->getConnections().size());
192 if (numConnections == 0) {
195 for (std::vector<NBEdge*>::const_iterator j = incoming.begin(); j != incoming.end(); ++j) {
196 const NBEdge* inEdge = *j;
197 const std::vector<NBEdge::Connection>& elv = inEdge->
getConnections();
198 for (std::vector<NBEdge::Connection>::const_iterator k = elv.begin(); k != elv.end(); ++k) {
215 int edgeID,
int fromNodeID,
int toNodeID,
216 const bool origNames,
217 const double straightThresh) {
225 planViewOSS.
openTag(
"planView");
227 const std::vector<NBEdge::Lane>& lanes = e->
getLanes();
229 #ifdef DEBUG_SMOOTH_GEOM 231 std::cout <<
"write planview for edge " << e->
getID() <<
"\n";
257 device.
writeAttr(
"elementType",
"junction");
258 device.
writeAttr(
"elementId", fromNodeID);
263 device.
writeAttr(
"elementType",
"junction");
272 device <<
" <lateralProfile/>\n";
273 device <<
" <lanes>\n";
274 device <<
" <laneSection s=\"0\">\n";
277 device <<
" <right>\n";
280 device <<
" <link/>\n";
288 device <<
" <width sOffset=\"0\" a=\"" << e->
getLaneWidth(j) <<
"\" b=\"0\" c=\"0\" d=\"0\"/>\n";
289 std::string markType =
"broken";
300 device <<
" <roadMark sOffset=\"0\" type=\"" << markType <<
"\" weight=\"standard\" color=\"standard\" width=\"0.13\"/>\n";
301 device <<
" <speed sOffset=\"0\" max=\"" << lanes[j].speed <<
"\"/>\n";
302 device <<
" </lane>\n";
304 device <<
" </right>\n";
305 device <<
" </laneSection>\n";
306 device <<
" </lanes>\n";
307 device <<
" <objects/>\n";
308 device <<
" <signals/>\n";
310 device <<
" <userData code=\"sumoId\" value=\"" << e->
getID() <<
"\"/>\n";
324 || parallel.front().fromLane != 0
325 || parallel.front().toLane != 0)) {
326 parallel.insert(parallel.begin(),
NBEdge::Connection(0, const_cast<NBEdge*>(outEdge), 0,
false));
333 int edgeID,
int inEdgeID,
int outEdgeID,
335 const std::vector<NBEdge::Connection>& parallel,
336 const bool isOuterEdge,
337 const double straightThresh,
338 const std::string& centerMark) {
339 assert(parallel.size() != 0);
341 const NBEdge* outEdge = cLeft.toEdge;
347 double laneOffset = 0;
349 fallBackShape.push_back(begShape.back());
350 fallBackShape.push_back(endShape.front());
354 if (init.size() == 0) {
358 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.");
365 if (init.size() != 0) {
366 length =
bezier(init, 12).length2D();
372 length =
bezier(init, 12).length2D();
375 junctionDevice <<
" <connection id=\"" << connectionID <<
"\" incomingRoad=\"" << inEdgeID <<
"\" connectingRoad=\"" << edgeID <<
"\" contactPoint=\"start\">\n";
391 device.
writeAttr(
"elementId", outEdgeID);
392 device.
writeAttr(
"contactPoint",
"start");
400 #ifdef DEBUG_SMOOTH_GEOM 402 std::cout <<
"write planview for internal edge " << cLeft.id <<
" init=" << init <<
" fallback=" << fallBackShape
403 <<
" begShape=" << begShape <<
" endShape=" << endShape
407 if (init.size() == 0) {
415 device <<
" <lateralProfile/>\n";
416 device <<
" <lanes>\n";
417 if (laneOffset != 0) {
418 device <<
" <laneOffset s=\"0\" a=\"" << laneOffset <<
"\" b=\"0\" c=\"0\" d=\"0\"/>\n";
420 device <<
" <laneSection s=\"0\">\n";
422 device <<
" <right>\n";
423 for (
int j = (
int)parallel.size(); --j >= 0;) {
428 device <<
" <link>\n";
429 device <<
" <predecessor id=\"" << fromIndex <<
"\"/>\n";
430 device <<
" <successor id=\"" << toIndex <<
"\"/>\n";
431 device <<
" </link>\n";
432 device <<
" <width sOffset=\"0\" a=\"" << outEdge->
getLaneWidth(c.
toLane) <<
"\" b=\"0\" c=\"0\" d=\"0\"/>\n";
433 std::string markType =
"broken";
439 }
else if (isOuterEdge && j > 0
454 device <<
" <roadMark sOffset=\"0\" type=\"" << markType <<
"\" weight=\"standard\" color=\"standard\" width=\"0.13\"/>\n";
455 device <<
" <speed sOffset=\"0\" max=\"" << c.
vmax <<
"\"/>\n";
456 device <<
" </lane>\n";
458 junctionDevice <<
" <laneLink from=\"" << fromIndex <<
"\" to=\"" << toIndex <<
"\"/>\n";
461 device <<
" </right>\n";
462 device <<
" </laneSection>\n";
463 device <<
" </lanes>\n";
464 device <<
" <objects/>\n";
465 device <<
" <signals/>\n";
467 junctionDevice <<
" </connection>\n";
475 for (
int j = 0; j < (int)shape.size() - 1; ++j) {
488 elevationDevice <<
" <elevation s=\"" << offset <<
"\" a=\"" << p.
z() <<
"\" b=\"" << (p2.
z() - p.
z()) /
MAX2(
POSITION_EPS, length) <<
"\" c=\"0\" d=\"0\"/>\n";
497 device <<
" <center>\n";
498 device <<
" <lane id=\"0\" type=\"none\" level=\"true\">\n";
499 device <<
" <link/>\n";
500 device <<
" <roadMark sOffset=\"0\" type=\"" << mark <<
"\" weight=\"standard\" color=\"standard\" width=\"" << markWidth <<
"\"/>\n";
501 device <<
" </lane>\n";
502 device <<
" </center>\n";
509 return map.
get(origID);
511 map.
insert(origID, lastID++);
518 switch (permissions) {
537 if (permissions ==
SVCAll) {
554 if (laneIndex == -1) {
556 laneIndex = lefthand ? 0 : (int)edge->
getNumLanes() - 1;
565 const int leftmost = lefthand ? 0 : (int)edge->
getNumLanes() - 1;
569 for (
int i = leftmost; i < laneIndex; i++) {
573 for (
int i = leftmost; i > laneIndex; i--) {
597 assert(init.size() == 3 || init.size() == 4);
608 init.
add(-p.
x(), -p.
y(), -p.
z());
612 double aU, bU, cU, dU;
613 double aV, bV, cV, dV;
614 double aZ, bZ, cZ, dZ;
617 if (init.size() == 3) {
620 bU = 2 * init[1].x() - 2 * init[0].x();
621 cU = init[0].x() - 2 * init[1].x() + init[2].x();
625 bV = 2 * init[1].y() - 2 * init[0].y();
626 cV = init[0].y() - 2 * init[1].y() + init[2].y();
631 bZ = (2 * initZ[1].z() - 2 * initZ[0].z()) / length;
632 cZ = (initZ[0].z() - 2 * initZ[1].z() + initZ[2].z()) / (length * length);
638 bU = 3 * init[1].x() - 3 * init[0].x();
639 cU = 3 * init[0].x() - 6 * init[1].x() + 3 * init[2].x();
640 dU = -init[0].x() + 3 * init[1].x() - 3 * init[2].x() + init[3].x();
643 bV = 3 * init[1].y() - 3 * init[0].y();
644 cV = 3 * init[0].y() - 6 * init[1].y() + 3 * init[2].y();
645 dV = -init[0].y() + 3 * init[1].y() - 3 * init[2].y() + init[3].y();
649 bZ = (3 * initZ[1].z() - 3 * initZ[0].z()) / length;
650 cZ = (3 * initZ[0].z() - 6 * initZ[1].z() + 3 * initZ[2].z()) / (length * length);
651 dZ = (-initZ[0].z() + 3 * initZ[1].z() - 3 * initZ[2].z() + initZ[3].z()) / (length * length * length);
674 elevationDevice.
openTag(
"elevation");
682 return offset + length;
688 #ifdef DEBUG_SMOOTH_GEOM 690 std::cout <<
"writeGeomSmooth\n n=" << shape.size() <<
" shape=" <<
toString(shape) <<
"\n";
694 const double longThresh = speed;
695 const double curveCutout = longThresh / 2;
697 assert(longThresh >= 2 * curveCutout);
698 assert(shape.size() > 2);
702 double maxAngleDiff = 0;
704 for (
int j = 1; j < (int)shape.size() - 1; ++j) {
712 maxAngleDiff =
MAX2(maxAngleDiff, dAngle);
713 #ifdef DEBUG_SMOOTH_GEOM 715 std::cout <<
" j=" << j <<
" dAngle=" <<
RAD2DEG(dAngle) <<
" length1=" << length1 <<
" length2=" << length2 <<
"\n";
718 if (dAngle > straightThresh
719 && (length1 > longThresh || j == 1)
720 && (length2 > longThresh || j == (
int)shape.size() - 2)) {
723 shape2.removeClosest(p1);
727 const int numPoints = (int)shape2.size();
728 #ifdef DEBUG_SMOOTH_GEOM 730 std::cout <<
" n=" << numPoints <<
" shape2=" <<
toString(shape2) <<
"\n";
734 if (maxAngleDiff < straightThresh) {
736 #ifdef DEBUG_SMOOTH_GEOM 738 std::cout <<
" special case: all lines. maxAngleDiff=" << maxAngleDiff <<
"\n";
746 for (
int j = 0; j < numPoints - 1; ++j) {
752 const double lineLength = line.
length2D();
753 if (lineLength >= longThresh) {
755 #ifdef DEBUG_SMOOTH_GEOM 757 std::cout <<
" writeLine=" <<
toString(line) <<
"\n";
764 if (j == 0 || j == numPoints - 2) {
767 begShape.
add(p0 - begShape.back());
768 }
else if (j == 1 || p0.
distanceTo2D(shape2[j - 1]) > longThresh) {
770 begShape.push_back(shape2[j - 1]);
771 begShape.push_back(p0);
774 begShape.push_back(shape2[j - 1]);
775 begShape.push_back(p1);
776 begShape.
add(p0 - begShape.back());
779 if (j == 0 || j == numPoints - 2) {
782 endShape.
add(p1 - endShape.front());
783 }
else if (j == numPoints - 3 || p1.
distanceTo2D(shape2[j + 2]) > longThresh) {
785 endShape.push_back(p1);
786 endShape.push_back(shape2[j + 2]);
789 endShape.push_back(p0);
790 endShape.push_back(shape2[j + 2]);
791 endShape.
add(p1 - endShape.front());
793 const double extrapolateLength =
MIN2((
double)25, lineLength / 4);
795 if (init.size() == 0) {
798 #ifdef DEBUG_SMOOTH_GEOM 800 std::cout <<
" writeLine lineLength=" << lineLength <<
" begShape" << j <<
"=" <<
toString(begShape) <<
" endShape" << j <<
"=" <<
toString(endShape) <<
" init" << j <<
"=" <<
toString(init) <<
"\n";
805 const double curveLength =
bezier(init, 12).length2D();
806 offset =
writeGeomPP3(device, elevationDevice, init, curveLength, offset);
807 #ifdef DEBUG_SMOOTH_GEOM 809 std::cout <<
" writeCurve lineLength=" << lineLength <<
" curveLength=" << curveLength <<
" begShape" << j <<
"=" <<
toString(begShape) <<
" endShape" << j <<
"=" <<
toString(endShape) <<
" init" << j <<
"=" <<
toString(init) <<
"\n";
824 double z = shape.size() == 0 ? 0 : shape[0].z();
825 for (
int i = 1; i < (int)shape.size(); ++i) {
831 device <<
" <elevationProfile>\n";
833 device <<
" <elevation s=\"0\" a=\"" << z <<
"\" b=\"0\" c=\"0\" d=\"0\"/>\n";
837 device <<
" </elevationProfile>\n";
847 assert(shape0.size() >= 2);
851 stopLine.push_back(to);
854 for (
int lane = 1; lane < e->
getNumLanes(); ++lane) {
The link is a partial left direction.
double vmax
maximun velocity
OutputDevice & writeAttr(const SumoXMLAttr attr, const T &val)
writes a named attribute
double ymin() const
Returns minimum y-coordinate.
double length2D() const
Returns the length.
double xmax() const
Returns maximum x-coordinate.
A structure which describes a connection between edges or lanes.
int toLane
The lane the connections yields in.
int numNormalConnections() const
return the number of lane-to-lane connections at this junction (excluding crossings) ...
static void writeEmptyCenterLane(OutputDevice &device, const std::string &mark, double markWidth)
std::map< std::string, NBNode * >::const_iterator begin() const
Returns the pointer to the begin of the stored nodes.
double z() const
Returns the z-position.
double distance2D(const Position &p, bool perpendicular=false) const
closest 2D-distance to point p (or -1 if perpendicular is true and the point is beyond this vector) ...
NBEdge * toEdge
The edge the connections yields in.
EdgeVector getIncomingEdges() const
Returns the list of incoming edges unsorted.
const Boundary & getConvBoundary() const
Returns the converted boundary.
std::map< std::string, NBNode * >::const_iterator end() const
Returns the pointer to the end of the stored nodes.
vehicle is a not electrified rail
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.
static void writeNormalEdge(OutputDevice &device, const NBEdge *e, int edgeID, int fromNodeID, int toNodeID, const bool origNames, const double straightThresh)
write normal edge to device
std::string getString() const
Returns the current content as a string.
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.
double x() const
Returns the x-position.
void setPrecision(int precision=gPrecision)
Sets the precison or resets it to default.
double angleTo2D(const Position &other) const
returns the angle in the plane of the vector pointing from here to the other position ...
const std::vector< NBEdge::Lane > & getLanes() const
Returns the lane definitions.
std::map< std::string, NBEdge * >::const_iterator end() const
Returns the pointer to the end of the stored edges.
bool getBool(const std::string &name) const
Returns the boolean-value of the named option (only for Option_Bool)
const std::string & getID() const
Returns the id.
bool isRailway(SVCPermissions permissions)
Returns whether an edge with the given permission is a railway edge.
const SVCPermissions SVCAll
all VClasses are allowed
A class that stores a 2D geometrical boundary.
The link is a (hard) left direction.
vehicle is a (possibly fast moving) electric rail
#define WRITE_WARNING(msg)
static OptionsCont & getOptions()
Retrieves the options.
LinkDirection
The different directions a link between two lanes may take (or a stream between two edges)...
bool isSet(const std::string &name, bool failOnNonExistant=true) const
Returns the information whether the named option is set.
void insert(const std::string str, const T key, bool checkDuplicates=true)
static void writeNetwork(const OptionsCont &oc, NBNetBuilder &nb)
Writes the network into a openDRIVE-file.
static double writeGeomPP3(OutputDevice &device, OutputDevice &elevationDevice, PositionVector init, double length, double offset=0)
write geometry as a single bezier curve (paramPoly3)
std::map< std::string, NBEdge * >::const_iterator begin() const
Returns the pointer to the begin of the stored edges.
std::string toString(const T &t, std::streamsize accuracy=gPrecision)
static bool writeGeomSmooth(const PositionVector &shape, double speed, OutputDevice &device, OutputDevice &elevationDevice, double straightThresh, double &length)
int size() const
Returns the number of nodes stored in this container.
std::string getLaneID(int lane) const
get Lane ID (Secure)
int getNumLanes() const
Returns the number of lanes.
int fromLane
The lane the connections starts at.
A point in 2D or 3D with translation and scaling methods.
NBEdgeCont & getEdgeCont()
static PositionVector getRightLaneBorder(const NBEdge *edge, int laneIndex=-1)
T get(const std::string &str) const
std::string getString(const std::string &name) const
Returns the string-value of the named option (only for Option_String)
bool geometryLike() const
whether this is structurally similar to a geometry node
Storage for edges, including some functionality operating on multiple edges.
double xmin() const
Returns minimum x-coordinate.
The link is a (hard) right direction.
const std::string & getStreetName() const
Returns the street name of this edge.
static PositionVector getLeftLaneBorder(const NBEdge *edge, int laneIndex=-1, double widthOffset=0)
get the left border of the given lane (the leftmost one by default)
static std::string escapeXML(const std::string &orig, const bool maskDoubleHyphen=false)
Replaces the standard escapes by their XML entities.
The link is a partial right direction.
double getFloat(const std::string &name) const
Returns the double-value of the named option (only for Option_Float)
SVCPermissions getPermissions(int lane=-1) const
get the union of allowed classes over all lanes or for a specific lane
void move2side(double amount)
move position vector to side using certain ammount
vehicle is a passenger car (a "normal" car)
LinkDirection getDirection(const NBEdge *const incoming, const NBEdge *const outgoing, bool leftHand=false) const
Returns the representation of the described stream's direction.
double getSpeed() const
Returns the speed allowed on this edge.
double getLaneWidth() const
Returns the default width of lanes of this edge.
void rotate2D(double angle)
static std::string getLaneType(SVCPermissions permissions)
static double writeGeomLines(const PositionVector &shape, OutputDevice &device, OutputDevice &elevationDevice, double offset=0)
write geometry as sequence of lines (sumo style)
const PositionVector & getLaneShape(int i) const
Returns the shape of the nth lane.
const EdgeVector & getIncomingEdges() const
Returns this node's incoming edges (The edges which yield in this node)
const std::vector< Connection > & getConnections() const
Returns the connections.
NBNodeCont & getNodeCont()
Returns a reference to the node container.
Instance responsible for building networks.
static OutputDevice & getDevice(const std::string &name)
Returns the described OutputDevice.
static Position sideOffset(const Position &beg, const Position &end, const double amount)
get a side position of position vector using a offset
A storage for options typed value containers)
double angleAt2D(int pos) const
get angle in certain position of position vector
static const GeoConvHelper & getFinal()
the coordinate transformation for writing the location element and for tracking the original coordina...
static void addPedestrianConnection(const NBEdge *inEdge, const NBEdge *outEdge, std::vector< NBEdge::Connection > ¶llel)
bool isTurningDirectionAt(const NBEdge *const edge) const
Returns whether the given edge is the opposite direction to this edge.
Represents a single node (junction) during network building.
Static storage of an output device and its base (abstract) implementation.
bool closeTag()
Closes the most recently opened tag.
NBNode * getFromNode() const
Returns the origin node of the edge.
Container for nodes during the netbuilding process.
bool hasString(const std::string &str) const
static double angleDiff(const double angle1, const double angle2)
Returns the difference of the second angle to the first angle in radiants.
static void checkLaneGeometries(const NBEdge *e)
check if the lane geometries are compatible with OpenDRIVE assumptions (colinear stop line) ...
void add(double xoff, double yoff, double zoff)
double ymax() const
Returns maximum y-coordinate.
static int writeInternalEdge(OutputDevice &device, OutputDevice &junctionDevice, const NBEdge *inEdge, int nodeID, int edgeID, int inEdgeID, int outEdgeID, int connectionID, const std::vector< NBEdge::Connection > ¶llel, const bool isOuterEdge, const double straightThresh, const std::string ¢erMark)
write internal edge to device, return next connectionID
static int getID(const std::string &origID, StringBijection< int > &map, int &lastID)
static PositionVector bezierControlPoints(const PositionVector &begShape, const PositionVector &endShape, bool isTurnaround, double extrapolateBeg, double extrapolateEnd, bool &ok, NBNode *recordError=0, double straightThresh=DEG2RAD(5))
get bezier control points
NBNode * getToNode() const
Returns the destination node of the edge.
static void writeElevationProfile(const PositionVector &shape, OutputDevice &device, const OutputDevice_String &elevationDevice)
OutputDevice & openTag(const std::string &xmlElement)
Opens an XML tag.
void lf()
writes a line feed if applicable
An output device that encapsulates an ofstream.
void bezier(int npts, double b[], int cpts, double p[])