Line data Source code
1 : /****************************************************************************/
2 : // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.dev/sumo
3 : // Copyright (C) 2011-2024 German Aerospace Center (DLR) and others.
4 : // This program and the accompanying materials are made available under the
5 : // terms of the Eclipse Public License 2.0 which is available at
6 : // https://www.eclipse.org/legal/epl-2.0/
7 : // This Source Code may also be made available under the following Secondary
8 : // Licenses when the conditions for such availability set forth in the Eclipse
9 : // Public License 2.0 are satisfied: GNU General Public License, version 2
10 : // or later which is available at
11 : // https://www.gnu.org/licenses/old-licenses/gpl-2.0-standalone.html
12 : // SPDX-License-Identifier: EPL-2.0 OR GPL-2.0-or-later
13 : /****************************************************************************/
14 : /// @file NWWriter_OpenDrive.cpp
15 : /// @author Daniel Krajzewicz
16 : /// @author Jakob Erdmann
17 : /// @date Tue, 04.05.2011
18 : ///
19 : // Exporter writing networks using the openDRIVE format
20 : /****************************************************************************/
21 : #include <config.h>
22 :
23 : #include <ctime>
24 : #include "NWWriter_OpenDrive.h"
25 : #include <utils/iodevices/OutputDevice_String.h>
26 : #include <utils/common/MsgHandler.h>
27 : #include <netbuild/NBEdgeCont.h>
28 : #include <netbuild/NBNode.h>
29 : #include <netbuild/NBNodeCont.h>
30 : #include <netbuild/NBNetBuilder.h>
31 : #include <utils/options/OptionsCont.h>
32 : #include <utils/iodevices/OutputDevice.h>
33 : #include <utils/common/MsgHandler.h>
34 : #include <utils/common/StdDefs.h>
35 : #include <utils/common/StringUtils.h>
36 : #include <utils/common/StringTokenizer.h>
37 : #include <utils/geom/GeoConvHelper.h>
38 : #include <regex>
39 :
40 : #define INVALID_ID -1
41 :
42 : //#define DEBUG_SMOOTH_GEOM
43 : #define DEBUGCOND true
44 :
45 : #define MIN_TURN_DIAMETER 2.0
46 :
47 : #define ROAD_OBJECTS "roadObjects"
48 :
49 : // ===========================================================================
50 : // static members
51 : // ===========================================================================
52 : bool NWWriter_OpenDrive::lefthand(false);
53 : bool NWWriter_OpenDrive::LHLL(false);
54 : bool NWWriter_OpenDrive::LHRL(false);
55 :
56 : // ===========================================================================
57 : // method definitions
58 : // ===========================================================================
59 : // ---------------------------------------------------------------------------
60 : // static methods
61 : // ---------------------------------------------------------------------------
62 : void
63 1830 : NWWriter_OpenDrive::writeNetwork(const OptionsCont& oc, NBNetBuilder& nb) {
64 : // check whether an opendrive-file shall be generated
65 3660 : if (!oc.isSet("opendrive-output")) {
66 1798 : return;
67 : }
68 : const NBNodeCont& nc = nb.getNodeCont();
69 : const NBEdgeCont& ec = nb.getEdgeCont();
70 32 : const bool origNames = oc.getBool("output.original-names");
71 32 : lefthand = oc.getBool("lefthand");
72 35 : LHLL = lefthand && oc.getBool("opendrive-output.lefthand-left");
73 32 : LHRL = lefthand && !LHLL;
74 32 : const double straightThresh = DEG2RAD(oc.getFloat("opendrive-output.straight-threshold"));
75 : // some internal mapping containers
76 32 : int nodeID = 1;
77 32 : int edgeID = nc.size() * 10; // distinct from node ids
78 : StringBijection<int> edgeMap;
79 : StringBijection<int> nodeMap;
80 : //
81 64 : OutputDevice& device = OutputDevice::getDevice(oc.getString("opendrive-output"));
82 64 : OutputDevice::createDeviceByOption("opendrive-output", "OpenDRIVE");
83 32 : time_t now = time(nullptr);
84 32 : std::string dstr(ctime(&now));
85 32 : const Boundary& b = GeoConvHelper::getFinal().getConvBoundary();
86 : // write header
87 32 : device.openTag("header");
88 32 : device.writeAttr("revMajor", "1");
89 32 : device.writeAttr("revMinor", "4");
90 32 : device.writeAttr("name", "");
91 64 : device.writeAttr("version", "1.00");
92 64 : device.writeAttr("date", dstr.substr(0, dstr.length() - 1));
93 32 : device.writeAttr("north", b.ymax());
94 32 : device.writeAttr("south", b.ymin());
95 32 : device.writeAttr("east", b.xmax());
96 32 : device.writeAttr("west", b.xmin());
97 : /* @note obsolete in 1.4
98 : device.writeAttr("maxRoad", ec.size());
99 : device.writeAttr("maxJunc", nc.size());
100 : device.writeAttr("maxPrg", 0);
101 : */
102 : // write optional geo reference
103 : const GeoConvHelper& gch = GeoConvHelper::getFinal();
104 32 : if (gch.usingGeoProjection()) {
105 10 : device.openTag("geoReference");
106 : device.writePreformattedTag(" <![CDATA[\n "
107 10 : + gch.getProjString()
108 20 : + "\n]]>\n");
109 10 : device.closeTag();
110 20 : if (gch.getOffsetBase() != Position(0, 0)) {
111 10 : device.openTag("offset");
112 20 : device.writeAttr("x", gch.getOffsetBase().x());
113 20 : device.writeAttr("y", gch.getOffsetBase().y());
114 20 : device.writeAttr("z", gch.getOffsetBase().z());
115 10 : device.writeAttr("hdg", 0);
116 20 : device.closeTag();
117 : }
118 : }
119 64 : device.closeTag();
120 :
121 : SignalLanes signalLanes;
122 :
123 32 : mapmatchRoadObjects(nb.getShapeCont(), ec);
124 :
125 32 : PositionVector crosswalk_shape;
126 : std::map<std::string, std::vector<std::string>> crosswalksByEdge;
127 471 : for (auto it = nc.begin(); it != nc.end(); ++it) {
128 439 : NBNode* n = it->second;
129 439 : auto crosswalks = n->getCrossings();
130 478 : for (const auto& cw : n->getCrossings()) {
131 : // getting from crosswalk line to a full shape
132 39 : crosswalk_shape = cw->shape;
133 39 : PositionVector rightside = cw->shape;
134 : try {
135 39 : crosswalk_shape.move2side(cw->width / 2);
136 39 : rightside.move2side(cw->width / -2);
137 0 : } catch (InvalidArgument&) { }
138 78 : rightside = rightside.reverse();
139 39 : crosswalk_shape.append(rightside);
140 39 : auto crosswalkId = cw->id;
141 78 : nb.getShapeCont().addPolygon(crosswalkId, "crosswalk", RGBColor::DEFAULT_COLOR, 0,
142 : Shape::DEFAULT_ANGLE, Shape::DEFAULT_IMG_FILE, Shape::DEFAULT_RELATIVEPATH,
143 : crosswalk_shape, false, true, 1, false, crosswalkId);
144 : SUMOPolygon* cwp = nb.getShapeCont().getPolygons().get(crosswalkId);
145 78 : cwp->setParameter("length", toString(cw->width));
146 78 : cwp->setParameter("width", toString(cw->shape.length2D()));
147 78 : cwp->setParameter("hdg", "0");
148 39 : crosswalksByEdge[cw->edges[0]->getID()].push_back(crosswalkId);
149 39 : }
150 : }
151 :
152 : // write normal edges (road)
153 850 : for (std::map<std::string, NBEdge*>::const_iterator i = ec.begin(); i != ec.end(); ++i) {
154 818 : const NBEdge* e = (*i).second;
155 1636 : const int fromNodeID = e->getIncomingEdges().size() > 0 ? getID(e->getFromNode()->getID(), nodeMap, nodeID) : INVALID_ID;
156 818 : const int toNodeID = e->getConnections().size() > 0 ? getID(e->getToNode()->getID(), nodeMap, nodeID) : INVALID_ID;
157 818 : writeNormalEdge(device, e,
158 818 : getID(e->getID(), edgeMap, edgeID),
159 : fromNodeID, toNodeID,
160 : origNames, straightThresh,
161 : nb.getShapeCont(),
162 : signalLanes,
163 818 : crosswalksByEdge[e->getID()]);
164 : }
165 32 : device.lf();
166 :
167 : // write junction-internal edges (road). In OpenDRIVE these are called 'paths' or 'connecting roads'
168 32 : OutputDevice_String junctionOSS(3);
169 471 : for (std::map<std::string, NBNode*>::const_iterator i = nc.begin(); i != nc.end(); ++i) {
170 439 : NBNode* n = (*i).second;
171 : int connectionID = 0; // unique within a junction
172 439 : const int nID = getID(n->getID(), nodeMap, nodeID);
173 439 : if (n->numNormalConnections() > 0) {
174 294 : junctionOSS << " <junction name=\"" << n->getID() << "\" id=\"" << nID << "\">\n";
175 : }
176 439 : std::vector<NBEdge*> incoming = (*i).second->getIncomingEdges();
177 439 : if (lefthand) {
178 : std::reverse(incoming.begin(), incoming.end());
179 : }
180 1257 : for (NBEdge* inEdge : incoming) {
181 818 : std::string centerMark = "none";
182 818 : const int inEdgeID = getID(inEdge->getID(), edgeMap, edgeID);
183 : // group parallel edges
184 : const NBEdge* outEdge = nullptr;
185 : bool isOuterEdge = true; // determine where a solid outer border should be drawn
186 : int lastFromLane = -1;
187 : std::vector<NBEdge::Connection> parallel;
188 818 : std::vector<NBEdge::Connection> connections = inEdge->getConnections();
189 2682 : for (const NBEdge::Connection& c : connections) {
190 : assert(c.toEdge != 0);
191 1864 : if (outEdge != c.toEdge || c.fromLane == lastFromLane) {
192 1753 : if (outEdge != nullptr) {
193 1092 : if (isOuterEdge) {
194 512 : addPedestrianConnection(inEdge, outEdge, parallel);
195 : }
196 2184 : connectionID = writeInternalEdge(device, junctionOSS, inEdge, nID,
197 2184 : getID(parallel.back().getInternalLaneID(), edgeMap, edgeID),
198 : inEdgeID,
199 1092 : getID(outEdge->getID(), edgeMap, edgeID),
200 : connectionID,
201 : parallel, isOuterEdge, straightThresh, centerMark,
202 : signalLanes);
203 : parallel.clear();
204 : isOuterEdge = false;
205 : }
206 1753 : outEdge = c.toEdge;
207 : }
208 1864 : lastFromLane = c.fromLane;
209 1864 : parallel.push_back(c);
210 : }
211 818 : if (isOuterEdge) {
212 306 : addPedestrianConnection(inEdge, outEdge, parallel);
213 : }
214 818 : if (!parallel.empty()) {
215 661 : if (!lefthand && (n->geometryLike() || inEdge->isTurningDirectionAt(outEdge))) {
216 : centerMark = "solid";
217 : }
218 1322 : connectionID = writeInternalEdge(device, junctionOSS, inEdge, nID,
219 1322 : getID(parallel.back().getInternalLaneID(), edgeMap, edgeID),
220 : inEdgeID,
221 661 : getID(outEdge->getID(), edgeMap, edgeID),
222 : connectionID,
223 : parallel, isOuterEdge, straightThresh, centerMark,
224 : signalLanes);
225 : parallel.clear();
226 : }
227 818 : }
228 439 : if (n->numNormalConnections() > 0) {
229 294 : junctionOSS << " </junction>\n";
230 : }
231 : }
232 32 : device.lf();
233 : // write controllers
234 471 : for (std::map<std::string, NBNode*>::const_iterator i = nc.begin(); i != nc.end(); ++i) {
235 439 : NBNode* n = (*i).second;
236 439 : if (n->isTLControlled()) {
237 21 : NBTrafficLightDefinition* tl = *n->getControllingTLS().begin();
238 : std::set<std::string> ids;
239 42 : device.openTag("controller");
240 42 : device.writeAttr("id", tl->getID());
241 299 : for (const NBConnection& c : tl->getControlledLinks()) {
242 556 : const std::string id = tl->getID() + "_" + toString(c.getTLIndex());
243 : if (ids.count(id) == 0) {
244 : ids.insert(id);
245 266 : device.openTag("control");
246 266 : device.writeAttr("signalId", id);
247 532 : device.closeTag();
248 : }
249 : }
250 42 : device.closeTag();
251 : }
252 : }
253 : // write junctions (junction)
254 32 : device << junctionOSS.getString();
255 :
256 32 : device.closeTag();
257 32 : device.close();
258 96 : }
259 :
260 :
261 : std::string
262 814 : NWWriter_OpenDrive::getDividerType(const NBEdge* e) {
263 : std::map<std::string, std::string> dividerTypeMapping;
264 814 : dividerTypeMapping["solid_line"] = "solid";
265 814 : dividerTypeMapping["dashed_line"] = "broken";
266 814 : dividerTypeMapping["double_solid_line"] = "solid solid";
267 814 : dividerTypeMapping["no"] = "none";
268 :
269 : // defaulting to solid as in the original code
270 814 : std::string dividerType = "solid";
271 :
272 1628 : if (e->getParametersMap().count("divider") > 0) {
273 4 : std::string divider = e->getParametersMap().find("divider")->second;
274 : if (dividerTypeMapping.count(divider) > 0) {
275 2 : dividerType = dividerTypeMapping.find(divider)->second;
276 : }
277 : }
278 814 : return dividerType;
279 : }
280 :
281 :
282 : void
283 818 : NWWriter_OpenDrive::writeNormalEdge(OutputDevice& device, const NBEdge* e,
284 : int edgeID, int fromNodeID, int toNodeID,
285 : const bool origNames,
286 : const double straightThresh,
287 : const ShapeContainer& shc,
288 : SignalLanes& signalLanes,
289 : const std::vector<std::string>& crossings) {
290 : // buffer output because some fields are computed out of order
291 818 : OutputDevice_String elevationOSS(3);
292 818 : elevationOSS.setPrecision(8);
293 818 : OutputDevice_String planViewOSS(2);
294 818 : planViewOSS.setPrecision(8);
295 818 : double length = 0;
296 :
297 818 : planViewOSS.openTag("planView");
298 : // for the shape we need to use the leftmost border of the leftmost lane
299 : const std::vector<NBEdge::Lane>& lanes = e->getLanes();
300 818 : PositionVector ls = getInnerLaneBorder(e);
301 : #ifdef DEBUG_SMOOTH_GEOM
302 : if (DEBUGCOND) {
303 : std::cout << "write planview for edge " << e->getID() << "\n";
304 : }
305 : #endif
306 :
307 818 : if (ls.size() == 2 || e->getPermissions() == SVC_PEDESTRIAN) {
308 : // foot paths may contain sharp angles
309 621 : length = writeGeomLines(ls, planViewOSS, elevationOSS);
310 : } else {
311 197 : bool ok = writeGeomSmooth(ls, e->getSpeed(), planViewOSS, elevationOSS, straightThresh, length);
312 197 : if (!ok) {
313 3 : WRITE_WARNINGF(TL("Could not compute smooth shape for edge '%'."), e->getID());
314 : }
315 : }
316 818 : planViewOSS.closeTag();
317 :
318 1636 : device.openTag("road");
319 1636 : device.writeAttr("name", StringUtils::escapeXML(e->getStreetName()));
320 818 : device.setPrecision(8); // length requires higher precision
321 1636 : device.writeAttr("length", MAX2(POSITION_EPS, length));
322 818 : device.setPrecision(gPrecision);
323 818 : device.writeAttr("id", edgeID);
324 818 : device.writeAttr("junction", -1);
325 818 : if (fromNodeID != INVALID_ID || toNodeID != INVALID_ID) {
326 707 : device.openTag("link");
327 707 : if (fromNodeID != INVALID_ID) {
328 690 : device.openTag("predecessor");
329 690 : device.writeAttr("elementType", "junction");
330 690 : device.writeAttr("elementId", fromNodeID);
331 1380 : device.closeTag();
332 : }
333 707 : if (toNodeID != INVALID_ID) {
334 661 : device.openTag("successor");
335 661 : device.writeAttr("elementType", "junction");
336 661 : device.writeAttr("elementId", toNodeID);
337 1322 : device.closeTag();
338 : }
339 1414 : device.closeTag();
340 : }
341 3272 : device.openTag("type").writeAttr("s", 0).writeAttr("type", "town").closeTag();
342 818 : device << planViewOSS.getString();
343 818 : writeElevationProfile(ls, device, elevationOSS);
344 818 : device << " <lateralProfile/>\n";
345 818 : device << " <lanes>\n";
346 818 : device << " <laneSection s=\"0\">\n";
347 818 : const std::string centerMark = e->getPermissions(e->getNumLanes() - 1) == 0 ? "none" : getDividerType(e);
348 818 : if (!LHLL) {
349 800 : writeEmptyCenterLane(device, centerMark, 0.13);
350 : }
351 1618 : const std::string side = LHLL ? "left" : "right";
352 818 : device << " <" << side << ">\n";
353 : const int numLanes = e->getNumLanes();
354 1886 : for (int jRH = numLanes; --jRH >= 0;) {
355 : // XODR always has the lanes left to right (by default this is
356 : // inner-to-outer but in LH networks its outer-to-inner)
357 1068 : const int j = lefthand ? numLanes - 1 - jRH : jRH;
358 : std::string laneType = e->getLaneStruct(j).type;
359 1068 : if (laneType == "") {
360 2074 : laneType = getLaneType(e->getPermissions(j));
361 : }
362 1068 : device << " <lane id=\"" << s2x(j, numLanes) << "\" type=\"" << laneType << "\" level=\"true\">\n";
363 1068 : device << " <link/>\n";
364 : // this could be used for geometry-link junctions without u-turn,
365 : // predecessor and sucessors would be lane indices,
366 : // road predecessor / succesfors would be of type 'road' rather than
367 : // 'junction'
368 : //device << " <predecessor id=\"-1\"/>\n";
369 : //device << " <successor id=\"-1\"/>\n";
370 : //device << " </link>\n";
371 1068 : device << " <width sOffset=\"0\" a=\"" << e->getLaneWidth(j) << "\" b=\"0\" c=\"0\" d=\"0\"/>\n";
372 1068 : std::string markType = "broken";
373 1068 : if (LHRL) {
374 34 : if (j == numLanes - 1) {
375 : // solid road mark in the middle of the road
376 : markType = "solid";
377 16 : } else if ((e->getPermissions(j) & ~(SVC_PEDESTRIAN | SVC_BICYCLE)) == 0) {
378 : // solid road mark to the right of sidewalk or bicycle lane
379 : markType = "solid";
380 : }
381 : } else {
382 1034 : if (j == 0) {
383 : markType = "solid";
384 : } else if (j > 0
385 234 : && (e->getPermissions(j - 1) & ~(SVC_PEDESTRIAN | SVC_BICYCLE)) == 0) {
386 : // solid road mark to the left of sidewalk or bicycle lane
387 : markType = "solid";
388 144 : } else if (e->getPermissions(j) == 0) {
389 : // solid road mark to the right of a forbidden lane
390 : markType = "solid";
391 : }
392 : }
393 1068 : device << " <roadMark sOffset=\"0\" type=\"" << markType << "\" weight=\"standard\" color=\"standard\" width=\"0.13\"/>\n";
394 1068 : device << " <speed sOffset=\"0\" max=\"" << lanes[j].speed << "\"/>\n";
395 1068 : device << " </lane>\n";
396 : }
397 818 : device << " </" << side << ">\n";
398 818 : if (LHLL) {
399 18 : writeEmptyCenterLane(device, centerMark, 0.13);
400 : }
401 818 : device << " </laneSection>\n";
402 818 : device << " </lanes>\n";
403 818 : writeRoadObjects(device, e, shc, crossings);
404 818 : writeSignals(device, e, length, signalLanes, shc);
405 818 : if (origNames) {
406 404 : device << " <userData code=\"sumoId\" value=\"" << e->getID() << "\"/>\n";
407 : }
408 818 : device.closeTag();
409 818 : checkLaneGeometries(e);
410 818 : }
411 :
412 : void
413 818 : NWWriter_OpenDrive::addPedestrianConnection(const NBEdge* inEdge, const NBEdge* outEdge, std::vector<NBEdge::Connection>& parallel) {
414 : // by default there are no internal lanes for pedestrians. Determine if
415 : // one is feasible and does not exist yet.
416 : if (outEdge != nullptr
417 661 : && inEdge->getPermissions(0) == SVC_PEDESTRIAN
418 134 : && outEdge->getPermissions(0) == SVC_PEDESTRIAN
419 927 : && (parallel.empty()
420 109 : || parallel.front().fromLane != 0
421 64 : || parallel.front().toLane != 0)) {
422 90 : parallel.insert(parallel.begin(), NBEdge::Connection(0, const_cast<NBEdge*>(outEdge), 0));
423 45 : parallel.front().vmax = (inEdge->getLanes()[0].speed + outEdge->getLanes()[0].speed) / (double) 2.0;
424 : }
425 818 : }
426 :
427 :
428 : int
429 1753 : NWWriter_OpenDrive::writeInternalEdge(OutputDevice& device, OutputDevice& junctionDevice, const NBEdge* inEdge, int nodeID,
430 : int edgeID, int inEdgeID, int outEdgeID,
431 : int connectionID,
432 : const std::vector<NBEdge::Connection>& parallel,
433 : const bool isOuterEdge,
434 : const double straightThresh,
435 : const std::string& centerMark,
436 : SignalLanes& signalLanes) {
437 : assert(parallel.size() != 0);
438 1753 : const NBEdge::Connection& cLeft = LHRL ? parallel.front() : parallel.back();
439 1753 : const NBEdge* outEdge = cLeft.toEdge;
440 1753 : PositionVector begShape = getInnerLaneBorder(inEdge, cLeft.fromLane);
441 1753 : PositionVector endShape = getInnerLaneBorder(outEdge, cLeft.toLane);
442 : //std::cout << "computing reference line for internal lane " << cLeft.getInternalLaneID() << " begLane=" << inEdge->getLaneShape(cLeft.fromLane) << " endLane=" << outEdge->getLaneShape(cLeft.toLane) << "\n";
443 :
444 : double length;
445 1753 : double laneOffset = 0;
446 1753 : PositionVector fallBackShape;
447 1753 : fallBackShape.push_back(begShape.back());
448 1753 : fallBackShape.push_back(endShape.front());
449 1753 : const bool turnaround = inEdge->isTurningDirectionAt(outEdge);
450 1753 : bool ok = true;
451 1753 : PositionVector init = NBNode::bezierControlPoints(begShape, endShape, turnaround, 25, 25, ok, nullptr, straightThresh);
452 1753 : if (init.size() == 0) {
453 533 : length = fallBackShape.length2D();
454 : // problem with turnarounds is known, method currently returns 'ok' (#2539)
455 533 : if (!ok) {
456 17 : WRITE_WARNINGF(TL("Could not compute smooth shape from lane '%' to lane '%'. Use option 'junctions.scurve-stretch' or increase radius of junction '%' to fix this."), inEdge->getLaneID(cLeft.fromLane), outEdge->getLaneID(cLeft.toLane), inEdge->getToNode()->getID());
457 528 : } else if (length <= NUMERICAL_EPS) {
458 : // left-curving geometry-like edges must use the right
459 : // side as reference line and shift
460 960 : begShape = getOuterLaneBorder(inEdge, cLeft.fromLane);
461 960 : endShape = getOuterLaneBorder(outEdge, cLeft.toLane);
462 960 : init = NBNode::bezierControlPoints(begShape, endShape, turnaround, 25, 25, ok, nullptr, straightThresh);
463 480 : if (init.size() != 0) {
464 480 : length = init.bezier(12).length2D();
465 480 : laneOffset = outEdge->getLaneWidth(cLeft.toLane);
466 : //std::cout << " internalLane=" << cLeft.getInternalLaneID() << " length=" << length << "\n";
467 : }
468 : }
469 : } else {
470 1220 : length = init.bezier(12).length2D();
471 : }
472 1753 : double roadLength = MAX2(POSITION_EPS, length);
473 :
474 1753 : junctionDevice << " <connection id=\"" << connectionID << "\" incomingRoad=\"" << inEdgeID << "\" connectingRoad=\"" << edgeID << "\" contactPoint=\"start\">\n";
475 1753 : device.openTag("road");
476 1753 : device.writeAttr("name", cLeft.id);
477 1753 : device.setPrecision(8); // length requires higher precision
478 1753 : device.writeAttr("length", roadLength);
479 1753 : device.setPrecision(gPrecision);
480 1753 : device.writeAttr("id", edgeID);
481 1753 : device.writeAttr("junction", nodeID);
482 1753 : device.openTag("link");
483 1753 : device.openTag("predecessor");
484 1753 : device.writeAttr("elementType", "road");
485 1753 : device.writeAttr("elementId", inEdgeID);
486 1753 : device.writeAttr("contactPoint", "end");
487 1753 : device.closeTag();
488 1753 : device.openTag("successor");
489 1753 : device.writeAttr("elementType", "road");
490 1753 : device.writeAttr("elementId", outEdgeID);
491 1753 : device.writeAttr("contactPoint", "start");
492 1753 : device.closeTag();
493 1753 : device.closeTag();
494 7012 : device.openTag("type").writeAttr("s", 0).writeAttr("type", "town").closeTag();
495 1753 : device.openTag("planView");
496 1753 : device.setPrecision(8); // geometry hdg requires higher precision
497 1753 : OutputDevice_String elevationOSS(3);
498 1753 : elevationOSS.setPrecision(8);
499 : #ifdef DEBUG_SMOOTH_GEOM
500 : if (DEBUGCOND) {
501 : std::cout << "write planview for internal edge " << cLeft.id << " init=" << init << " fallback=" << fallBackShape
502 : << " begShape=" << begShape << " endShape=" << endShape
503 : << "\n";
504 : }
505 : #endif
506 1753 : if (init.size() == 0) {
507 53 : writeGeomLines(fallBackShape, device, elevationOSS);
508 : } else {
509 1700 : writeGeomPP3(device, elevationOSS, init, length);
510 : }
511 1753 : device.setPrecision(gPrecision);
512 1753 : device.closeTag();
513 1753 : writeElevationProfile(fallBackShape, device, elevationOSS);
514 1753 : device << " <lateralProfile/>\n";
515 1753 : device << " <lanes>\n";
516 1753 : if (laneOffset != 0) {
517 480 : device << " <laneOffset s=\"0\" a=\"" << laneOffset << "\" b=\"0\" c=\"0\" d=\"0\"/>\n";
518 : }
519 1753 : device << " <laneSection s=\"0\">\n";
520 1753 : if (!lefthand) {
521 1689 : writeEmptyCenterLane(device, centerMark, 0);
522 : }
523 3442 : const std::string side = lefthand ? "left" : "right";
524 1753 : device << " <" << side << ">\n";
525 1753 : const int numLanes = (int)parallel.size();
526 3662 : for (int jRH = numLanes; --jRH >= 0;) {
527 1909 : const int j = lefthand ? numLanes - 1 - jRH : jRH;
528 1909 : const int xJ = s2x(j, numLanes);
529 1909 : const NBEdge::Connection& c = parallel[j];
530 1909 : const int fromIndex = s2x(c.fromLane, inEdge->getNumLanes());
531 1909 : const int toIndex = s2x(c.toLane, outEdge->getNumLanes());
532 :
533 1909 : double inEdgeWidth = inEdge->getLaneWidth(c.fromLane);
534 1909 : double outEdgeWidth = outEdge->getLaneWidth(c.toLane);
535 : // Ideally a polynomial function of third order would be needed for more precision.
536 : // This is obtained by specifying c and d coefficients, keeping it simple and linear
537 : // as we only know final and initial width.
538 1909 : double bCoefficient = (outEdgeWidth - inEdgeWidth) / roadLength;
539 1909 : double cCoefficient = 0;
540 1909 : double dCoefficient = 0;
541 1909 : device << " <lane id=\"" << xJ << "\" type=\"" << getLaneType(outEdge->getPermissions(c.toLane)) << "\" level=\"true\">\n";
542 1909 : device << " <link>\n";
543 1909 : device << " <predecessor id=\"" << fromIndex << "\"/>\n";
544 1909 : device << " <successor id=\"" << toIndex << "\"/>\n";
545 1909 : device << " </link>\n";
546 1909 : device << " <width sOffset=\"0\" a=\"" << inEdgeWidth << "\" b=\"" << bCoefficient << "\" c=\"" << cCoefficient << "\" d=\"" << dCoefficient << "\"/>\n";
547 1909 : std::string markType = "broken";
548 1909 : if (inEdge->isTurningDirectionAt(outEdge)) {
549 : markType = "none";
550 1396 : } else if (c.fromLane == 0 && c.toLane == 0 && isOuterEdge) {
551 : // solid road mark at the outer border
552 : markType = "solid";
553 808 : } else if (isOuterEdge && j > 0
554 808 : && (outEdge->getPermissions(parallel[j - 1].toLane) & ~(SVC_PEDESTRIAN | SVC_BICYCLE)) == 0) {
555 : // solid road mark to the left of sidewalk or bicycle lane
556 : markType = "solid";
557 756 : } else if (!inEdge->getToNode()->geometryLike()) {
558 : // draw shorter road marks to indicate turning paths
559 717 : LinkDirection dir = inEdge->getToNode()->getDirection(inEdge, outEdge, lefthand);
560 717 : if (dir == LinkDirection::LEFT || dir == LinkDirection::RIGHT || dir == LinkDirection::PARTLEFT || dir == LinkDirection::PARTRIGHT) {
561 : // XXX <type><line/><type> is not rendered by odrViewer so cannot be validated
562 : // device << " <type name=\"broken\" width=\"0.13\">\n";
563 : // device << " <line length=\"0.5\" space=\"0.5\" tOffset=\"0\" sOffset=\"0\" rule=\"none\"/>\n";
564 : // device << " </type>\n";
565 : markType = "none";
566 : }
567 : }
568 1909 : device << " <roadMark sOffset=\"0\" type=\"" << markType << "\" weight=\"standard\" color=\"standard\" width=\"0.13\"/>\n";
569 1909 : device << " <speed sOffset=\"0\" max=\"" << c.vmax << "\"/>\n";
570 1909 : device << " </lane>\n";
571 :
572 1909 : junctionDevice << " <laneLink from=\"" << fromIndex << "\" to=\"" << xJ << "\"/>\n";
573 1909 : connectionID++;
574 : }
575 1753 : device << " </" << side << ">\n";
576 1753 : if (lefthand) {
577 64 : writeEmptyCenterLane(device, centerMark, 0);
578 : }
579 1753 : device << " </laneSection>\n";
580 1753 : device << " </lanes>\n";
581 1753 : device << " <objects/>\n";
582 : UNUSED_PARAMETER(signalLanes);
583 1753 : device << " <signals/>\n";
584 1753 : device.closeTag();
585 1753 : junctionDevice << " </connection>\n";
586 :
587 3506 : return connectionID;
588 1753 : }
589 :
590 :
591 : double
592 1330 : NWWriter_OpenDrive::writeGeomLines(const PositionVector& shape, OutputDevice& device, OutputDevice& elevationDevice, double offset) {
593 2738 : for (int j = 0; j < (int)shape.size() - 1; ++j) {
594 1408 : const Position& p = shape[j];
595 1408 : const Position& p2 = shape[j + 1];
596 1408 : const double hdg = shape.angleAt2D(j);
597 1408 : const double length = p.distanceTo2D(p2);
598 1408 : device.openTag("geometry");
599 2816 : device.writeAttr("s", offset);
600 2816 : device.writeAttr("x", p.x());
601 1408 : device.writeAttr("y", p.y());
602 1408 : device.writeAttr("hdg", hdg);
603 1408 : device.writeAttr("length", length);
604 2816 : device.openTag("line").closeTag();
605 1408 : device.closeTag();
606 2813 : elevationDevice << " <elevation s=\"" << offset << "\" a=\"" << p.z() << "\" b=\"" << (p2.z() - p.z()) / MAX2(POSITION_EPS, length) << "\" c=\"0\" d=\"0\"/>\n";
607 1408 : offset += length;
608 : }
609 1330 : return offset;
610 : }
611 :
612 :
613 : void
614 2571 : NWWriter_OpenDrive::writeEmptyCenterLane(OutputDevice& device, const std::string& mark, double markWidth) {
615 2571 : device << " <center>\n";
616 2571 : device << " <lane id=\"0\" type=\"none\" level=\"true\">\n";
617 2571 : device << " <link/>\n";
618 2571 : device << " <roadMark sOffset=\"0\" type=\"" << mark << "\" weight=\"standard\" color=\"standard\" width=\"" << markWidth << "\"/>\n";
619 2571 : device << " </lane>\n";
620 2571 : device << " </center>\n";
621 2571 : }
622 :
623 :
624 : int
625 6932 : NWWriter_OpenDrive::getID(const std::string& origID, StringBijection<int>& map, int& lastID) {
626 6932 : if (map.hasString(origID)) {
627 3922 : return map.get(origID);
628 : }
629 3010 : map.insert(origID, lastID++);
630 3010 : return lastID - 1;
631 : }
632 :
633 :
634 : std::string
635 2946 : NWWriter_OpenDrive::getLaneType(SVCPermissions permissions) {
636 2946 : switch (permissions) {
637 : case SVC_PEDESTRIAN:
638 603 : return "sidewalk";
639 : //case (SVC_BICYCLE | SVC_PEDESTRIAN):
640 : // WRITE_WARNING("Ambiguous lane type (biking+driving) for road '" + roadID + "'");
641 : // return "sidewalk";
642 : case SVC_BICYCLE:
643 79 : return "biking";
644 : case 0:
645 : // ambiguous
646 12 : return "none";
647 : case SVC_RAIL:
648 : case SVC_RAIL_URBAN:
649 : case SVC_RAIL_ELECTRIC:
650 : case SVC_RAIL_FAST:
651 4 : return "rail";
652 : case SVC_TRAM:
653 18 : return "tram";
654 2230 : default: {
655 : // complex permissions
656 2230 : if (permissions == SVCAll) {
657 1155 : return "driving";
658 1075 : } else if (isRailway(permissions)) {
659 0 : return "rail";
660 1075 : } else if ((permissions & SVC_PASSENGER) != 0) {
661 714 : return "driving";
662 : } else {
663 361 : return "restricted";
664 : }
665 : }
666 : }
667 : }
668 :
669 :
670 : PositionVector
671 5418 : NWWriter_OpenDrive::getInnerLaneBorder(const NBEdge* edge, int laneIndex, double widthOffset) {
672 5418 : if (laneIndex == -1) {
673 : // innermost lane
674 952 : laneIndex = (int)edge->getNumLanes() - 1;
675 952 : if (LHRL) {
676 : laneIndex = 0;
677 : }
678 : }
679 5418 : PositionVector result = edge->getLaneShape(laneIndex);
680 10732 : widthOffset -= (LHLL ? -1 : 1) * edge->getLaneWidth(laneIndex) / 2;
681 : try {
682 5418 : result.move2side(widthOffset);
683 0 : } catch (InvalidArgument&) { }
684 5418 : return result;
685 0 : }
686 :
687 :
688 : PositionVector
689 960 : NWWriter_OpenDrive::getOuterLaneBorder(const NBEdge* edge, int laneIndex) {
690 960 : return getInnerLaneBorder(edge, laneIndex, edge->getLaneWidth(laneIndex));
691 : }
692 :
693 :
694 : double
695 2332 : NWWriter_OpenDrive::writeGeomPP3(
696 : OutputDevice& device,
697 : OutputDevice& elevationDevice,
698 : PositionVector init,
699 : double length,
700 : double offset) {
701 : assert(init.size() == 3 || init.size() == 4);
702 :
703 : // avoid division by 0
704 4664 : length = MAX2(POSITION_EPS, length);
705 :
706 2332 : const Position p = init.front();
707 2332 : const double hdg = init.angleAt2D(0);
708 :
709 : // backup elevation values
710 : const PositionVector initZ = init;
711 : // translate to u,v coordinates
712 2332 : init.add(-p.x(), -p.y(), -p.z());
713 2332 : init.rotate2D(-hdg);
714 :
715 : // parametric coefficients
716 : double aU, bU, cU, dU;
717 : double aV, bV, cV, dV;
718 : double aZ, bZ, cZ, dZ;
719 :
720 : // unfactor the Bernstein polynomials of degree 2 (or 3) and collect the coefficients
721 2332 : if (init.size() == 3) {
722 : //f(x, a, b ,c) = a + (2*b - 2*a)*x + (a - 2*b + c)*x*x
723 1306 : aU = init[0].x();
724 1306 : bU = 2 * init[1].x() - 2 * init[0].x();
725 1306 : cU = init[0].x() - 2 * init[1].x() + init[2].x();
726 1306 : dU = 0;
727 :
728 1306 : aV = init[0].y();
729 1306 : bV = 2 * init[1].y() - 2 * init[0].y();
730 1306 : cV = init[0].y() - 2 * init[1].y() + init[2].y();
731 1306 : dV = 0;
732 :
733 : // elevation is not parameteric on [0:1] but on [0:length]
734 1306 : aZ = initZ[0].z();
735 1306 : bZ = (2 * initZ[1].z() - 2 * initZ[0].z()) / length;
736 1306 : cZ = (initZ[0].z() - 2 * initZ[1].z() + initZ[2].z()) / (length * length);
737 1306 : dZ = 0;
738 :
739 : } else {
740 : // f(x, a, b, c, d) = a + (x*((3*b) - (3*a))) + ((x*x)*((3*a) + (3*c) - (6*b))) + ((x*x*x)*((3*b) - (3*c) - a + d))
741 1026 : aU = init[0].x();
742 1026 : bU = 3 * init[1].x() - 3 * init[0].x();
743 1026 : cU = 3 * init[0].x() - 6 * init[1].x() + 3 * init[2].x();
744 1026 : dU = -init[0].x() + 3 * init[1].x() - 3 * init[2].x() + init[3].x();
745 :
746 1026 : aV = init[0].y();
747 1026 : bV = 3 * init[1].y() - 3 * init[0].y();
748 1026 : cV = 3 * init[0].y() - 6 * init[1].y() + 3 * init[2].y();
749 1026 : dV = -init[0].y() + 3 * init[1].y() - 3 * init[2].y() + init[3].y();
750 :
751 : // elevation is not parameteric on [0:1] but on [0:length]
752 1026 : aZ = initZ[0].z();
753 1026 : bZ = (3 * initZ[1].z() - 3 * initZ[0].z()) / length;
754 1026 : cZ = (3 * initZ[0].z() - 6 * initZ[1].z() + 3 * initZ[2].z()) / (length * length);
755 1026 : dZ = (-initZ[0].z() + 3 * initZ[1].z() - 3 * initZ[2].z() + initZ[3].z()) / (length * length * length);
756 : }
757 :
758 2332 : device.openTag("geometry");
759 2332 : device.writeAttr("s", offset);
760 2332 : device.writeAttr("x", p.x());
761 2332 : device.writeAttr("y", p.y());
762 2332 : device.writeAttr("hdg", hdg);
763 2332 : device.writeAttr("length", length);
764 :
765 2332 : device.openTag("paramPoly3");
766 2332 : device.writeAttr("aU", aU);
767 2332 : device.writeAttr("bU", bU);
768 2332 : device.writeAttr("cU", cU);
769 2332 : device.writeAttr("dU", dU);
770 2332 : device.writeAttr("aV", aV);
771 2332 : device.writeAttr("bV", bV);
772 2332 : device.writeAttr("cV", cV);
773 2332 : device.writeAttr("dV", dV);
774 2332 : device.writeAttr("pRange", "normalized");
775 2332 : device.closeTag();
776 2332 : device.closeTag();
777 :
778 : // write elevation
779 2332 : elevationDevice.openTag("elevation");
780 2332 : elevationDevice.writeAttr("s", offset);
781 2332 : elevationDevice.writeAttr("a", aZ);
782 2332 : elevationDevice.writeAttr("b", bZ);
783 2332 : elevationDevice.writeAttr("c", cZ);
784 2332 : elevationDevice.writeAttr("d", dZ);
785 2332 : elevationDevice.closeTag();
786 :
787 2332 : return offset + length;
788 2332 : }
789 :
790 :
791 : bool
792 197 : NWWriter_OpenDrive::writeGeomSmooth(const PositionVector& shape, double speed, OutputDevice& device, OutputDevice& elevationDevice, double straightThresh, double& length) {
793 : #ifdef DEBUG_SMOOTH_GEOM
794 : if (DEBUGCOND) {
795 : std::cout << "writeGeomSmooth\n n=" << shape.size() << " shape=" << toString(shape) << "\n";
796 : }
797 : #endif
798 197 : bool ok = true;
799 : const double longThresh = speed; // 16.0; // make user-configurable (should match the sampling rate of the source data)
800 197 : const double curveCutout = longThresh / 2; // 8.0; // make user-configurable (related to the maximum turning rate)
801 : // 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')
802 : assert(longThresh >= 2 * curveCutout);
803 : assert(shape.size() > 2);
804 : // add intermediate points wherever there is a strong angular change between long segments
805 : // assume the geometry is simplified so as not to contain consecutive colinear points
806 : PositionVector shape2 = shape;
807 : double maxAngleDiff = 0;
808 : double offset = 0;
809 948 : for (int j = 1; j < (int)shape.size() - 1; ++j) {
810 : //const double hdg = shape.angleAt2D(j);
811 751 : const Position& p0 = shape[j - 1];
812 751 : const Position& p1 = shape[j];
813 751 : const Position& p2 = shape[j + 1];
814 751 : const double dAngle = fabs(GeomHelper::angleDiff(p0.angleTo2D(p1), p1.angleTo2D(p2)));
815 : const double length1 = p0.distanceTo2D(p1);
816 : const double length2 = p1.distanceTo2D(p2);
817 : maxAngleDiff = MAX2(maxAngleDiff, dAngle);
818 : #ifdef DEBUG_SMOOTH_GEOM
819 : if (DEBUGCOND) {
820 : std::cout << " j=" << j << " dAngle=" << RAD2DEG(dAngle) << " length1=" << length1 << " length2=" << length2 << "\n";
821 : }
822 : #endif
823 : if (dAngle > straightThresh
824 599 : && (length1 > longThresh || j == 1)
825 1139 : && (length2 > longThresh || j == (int)shape.size() - 2)) {
826 : // NBNode::bezierControlPoints checks for minimum length of POSITION_EPS so we make sure there is no instability
827 663 : shape2.insertAtClosest(shape.positionAtOffset2D(offset + length1 - MIN2(length1 - 2 * POSITION_EPS, curveCutout)), false);
828 665 : shape2.insertAtClosest(shape.positionAtOffset2D(offset + length1 + MIN2(length2 - 2 * POSITION_EPS, curveCutout)), false);
829 340 : shape2.removeClosest(p1);
830 : }
831 751 : offset += length1;
832 : }
833 197 : const int numPoints = (int)shape2.size();
834 : #ifdef DEBUG_SMOOTH_GEOM
835 : if (DEBUGCOND) {
836 : std::cout << " n=" << numPoints << " shape2=" << toString(shape2) << "\n";
837 : }
838 : #endif
839 :
840 197 : if (maxAngleDiff < straightThresh) {
841 0 : length = writeGeomLines(shape2, device, elevationDevice, 0);
842 : #ifdef DEBUG_SMOOTH_GEOM
843 : if (DEBUGCOND) {
844 : std::cout << " special case: all lines. maxAngleDiff=" << maxAngleDiff << "\n";
845 : }
846 : #endif
847 0 : return ok;
848 : }
849 :
850 : // write the long segments as lines, short segments as curves
851 : offset = 0;
852 1485 : for (int j = 0; j < numPoints - 1; ++j) {
853 1288 : const Position& p0 = shape2[j];
854 1288 : const Position& p1 = shape2[j + 1];
855 1288 : PositionVector line;
856 1288 : line.push_back(p0);
857 1288 : line.push_back(p1);
858 1288 : const double lineLength = line.length2D();
859 1288 : if (lineLength >= longThresh) {
860 394 : offset = writeGeomLines(line, device, elevationDevice, offset);
861 : #ifdef DEBUG_SMOOTH_GEOM
862 : if (DEBUGCOND) {
863 : std::cout << " writeLine=" << toString(line) << "\n";
864 : }
865 : #endif
866 : } else {
867 : // find control points
868 894 : PositionVector begShape;
869 894 : PositionVector endShape;
870 894 : if (j == 0 || j == numPoints - 2) {
871 : // keep the angle of the first/last segment but end at the front of the shape
872 : begShape = line;
873 122 : begShape.add(p0 - begShape.back());
874 772 : } else if (j == 1 || p0.distanceTo2D(shape2[j - 1]) > longThresh) {
875 : // use the previous segment if it is long or the first one
876 319 : begShape.push_back(shape2[j - 1]);
877 319 : begShape.push_back(p0);
878 : } else {
879 : // end at p0 with mean angle of the previous and current segment
880 453 : begShape.push_back(shape2[j - 1]);
881 453 : begShape.push_back(p1);
882 453 : begShape.add(p0 - begShape.back());
883 : }
884 :
885 894 : if (j == 0 || j == numPoints - 2) {
886 : // keep the angle of the first/last segment but start at the end of the shape
887 : endShape = line;
888 122 : endShape.add(p1 - endShape.front());
889 772 : } else if (j == numPoints - 3 || p1.distanceTo2D(shape2[j + 2]) > longThresh) {
890 : // use the next segment if it is long or the final one
891 319 : endShape.push_back(p1);
892 319 : endShape.push_back(shape2[j + 2]);
893 : } else {
894 : // start at p1 with mean angle of the current and next segment
895 453 : endShape.push_back(p0);
896 453 : endShape.push_back(shape2[j + 2]);
897 453 : endShape.add(p1 - endShape.front());
898 : }
899 894 : const double extrapolateLength = MIN2((double)25, lineLength / 4);
900 894 : PositionVector init = NBNode::bezierControlPoints(begShape, endShape, false, extrapolateLength, extrapolateLength, ok, nullptr, straightThresh);
901 894 : if (init.size() == 0) {
902 : // could not compute control points, write line
903 262 : offset = writeGeomLines(line, device, elevationDevice, offset);
904 : #ifdef DEBUG_SMOOTH_GEOM
905 : if (DEBUGCOND) {
906 : std::cout << " writeLine lineLength=" << lineLength << " begShape" << j << "=" << toString(begShape) << " endShape" << j << "=" << toString(endShape) << " init" << j << "=" << toString(init) << "\n";
907 : }
908 : #endif
909 : } else {
910 : // write bezier
911 632 : const double curveLength = init.bezier(12).length2D();
912 632 : offset = writeGeomPP3(device, elevationDevice, init, curveLength, offset);
913 : #ifdef DEBUG_SMOOTH_GEOM
914 : if (DEBUGCOND) {
915 : std::cout << " writeCurve lineLength=" << lineLength << " curveLength=" << curveLength << " begShape" << j << "=" << toString(begShape) << " endShape" << j << "=" << toString(endShape) << " init" << j << "=" << toString(init) << "\n";
916 : }
917 : #endif
918 : }
919 894 : }
920 1288 : }
921 197 : length = offset;
922 197 : return ok;
923 197 : }
924 :
925 :
926 : void
927 2571 : NWWriter_OpenDrive::writeElevationProfile(const PositionVector& shape, OutputDevice& device, const OutputDevice_String& elevationDevice) {
928 : // check if the shape is flat
929 : bool flat = true;
930 2571 : double z = shape.size() == 0 ? 0 : shape[0].z();
931 5739 : for (int i = 1; i < (int)shape.size(); ++i) {
932 3170 : if (fabs(shape[i].z() - z) > NUMERICAL_EPS) {
933 : flat = false;
934 : break;
935 : }
936 : }
937 2571 : device << " <elevationProfile>\n";
938 2571 : if (flat) {
939 2569 : device << " <elevation s=\"0\" a=\"" << z << "\" b=\"0\" c=\"0\" d=\"0\"/>\n";
940 : } else {
941 4 : device << elevationDevice.getString();
942 : }
943 2571 : device << " </elevationProfile>\n";
944 :
945 2571 : }
946 :
947 :
948 : void
949 818 : NWWriter_OpenDrive::checkLaneGeometries(const NBEdge* e) {
950 818 : if (e->getNumLanes() > 1) {
951 : // compute 'stop line' of rightmost lane
952 142 : const PositionVector shape0 = e->getLaneShape(0);
953 : assert(shape0.size() >= 2);
954 142 : const Position& from = shape0[-2];
955 142 : const Position& to = shape0[-1];
956 142 : PositionVector stopLine;
957 142 : stopLine.push_back(to);
958 252 : stopLine.push_back(to - PositionVector::sideOffset(from, to, lefthand ? 1000 : -1000));
959 : // endpoints of all other lanes should be on the stop line
960 392 : for (int lane = 1; lane < e->getNumLanes(); ++lane) {
961 250 : const double dist = stopLine.distance2D(e->getLaneShape(lane)[-1]);
962 250 : if (dist > NUMERICAL_EPS) {
963 0 : WRITE_WARNINGF(TL("Uneven stop line at lane '%' (dist=%) cannot be represented in OpenDRIVE."), e->getLaneID(lane), toString(dist));
964 : }
965 : }
966 142 : }
967 818 : }
968 :
969 : void
970 818 : NWWriter_OpenDrive::writeRoadObjects(OutputDevice& device, const NBEdge* e, const ShapeContainer& shc, const std::vector<std::string>& crossings) {
971 818 : device.openTag("objects");
972 1636 : if (e->hasParameter(ROAD_OBJECTS)) {
973 52 : device.setPrecision(8); // geometry hdg requires higher precision
974 52 : PositionVector road = getInnerLaneBorder(e);
975 194 : for (std::string id : StringTokenizer(e->getParameter(ROAD_OBJECTS, "")).getVector()) {
976 : SUMOPolygon* p = shc.getPolygons().get(id);
977 62 : if (p == nullptr) {
978 : PointOfInterest* poi = shc.getPOIs().get(id);
979 58 : if (poi == nullptr) {
980 0 : WRITE_WARNINGF("Road object polygon or POI '%' not found for edge '%'", id, e->getID());
981 : } else {
982 58 : writeRoadObjectPOI(device, e, road, poi);
983 : }
984 : } else {
985 62 : writeRoadObjectPoly(device, e, road, p);
986 : }
987 52 : }
988 52 : device.setPrecision(gPrecision);
989 52 : }
990 818 : if (crossings.size() > 0) {
991 34 : device.setPrecision(8); // geometry hdg requires higher precision
992 34 : PositionVector road = getInnerLaneBorder(e);
993 73 : for (size_t ic = 0; ic < crossings.size(); ic++) {
994 : SUMOPolygon* p = shc.getPolygons().get(crossings[ic]);
995 39 : if (p != 0) {
996 39 : writeRoadObjectPoly(device, e, road, p);
997 : }
998 : }
999 34 : device.setPrecision(gPrecision);
1000 34 : }
1001 818 : device.closeTag();
1002 818 : }
1003 :
1004 :
1005 : std::vector<NWWriter_OpenDrive::TrafficSign>
1006 4 : NWWriter_OpenDrive::parseTrafficSign(const std::string& trafficSign, PointOfInterest* poi) {
1007 : std::vector<TrafficSign> result;
1008 : // check for maxspeed, stop, give_way and hazard
1009 7 : if (trafficSign == "maxspeed" && poi->hasParameter("maxspeed")) {
1010 6 : result.push_back(TrafficSign{ "OpenDrive", "maxspeed", "", poi->getParameter("maxspeed")});
1011 1 : } else if (trafficSign == "stop") {
1012 0 : result.push_back(TrafficSign{ "OpenDrive", trafficSign, "", "" });
1013 1 : } else if (trafficSign == "give_way") {
1014 0 : result.push_back(TrafficSign{ "OpenDrive", trafficSign, "", "" });
1015 1 : } else if (trafficSign == "hazard" && poi->hasParameter("hazard")) {
1016 0 : result.push_back(TrafficSign{ "OpenDrive", trafficSign, poi->getParameter("hazard"), "" });
1017 : } else {
1018 1 : if (trafficSign.find_first_of(",;") != std::string::npos) {
1019 1 : std::string::size_type colon = trafficSign.find(':');
1020 1 : const std::string country = trafficSign.substr(0, colon);
1021 1 : const std::string remaining = trafficSign.substr(colon + 1);
1022 :
1023 : std::vector<std::string> tokens;
1024 : std::string::size_type lastPos = 0;
1025 : std::string::size_type pos = remaining.find_first_of(",;");
1026 3 : while (pos != std::string::npos) {
1027 : // add country and colon before pushing the token
1028 4 : tokens.push_back(country + ":" + remaining.substr(lastPos, pos - lastPos));
1029 2 : lastPos = pos + 1;
1030 : pos = remaining.find_first_of(",;", lastPos);
1031 : }
1032 2 : tokens.push_back(country + ":" + remaining.substr(lastPos));
1033 : // call for each token the parseTrafficSignId function and fill the result vector
1034 4 : for (std::string token : tokens) {
1035 6 : result.push_back(parseTrafficSignId(token));
1036 : }
1037 1 : } else {
1038 0 : result.push_back(parseTrafficSignId(trafficSign));
1039 : }
1040 : }
1041 4 : return result;
1042 0 : }
1043 :
1044 :
1045 : NWWriter_OpenDrive::TrafficSign
1046 3 : NWWriter_OpenDrive::parseTrafficSignId(const std::string& trafficSign) {
1047 : // for OpenDrive 1.4 the country code is specified as ISO 3166-1, alpha-3,
1048 : // but OSM uses ISO 3166-1, alpha-2. In order to be OpenDrive 1.4 compliant
1049 : // we would need to convert it back to alpha-3. However, newer OpenDrive
1050 : // versions support alpha-2, so we use that instead.
1051 :
1052 : //std::regex re("([A-Z]{2}):([0-9]{1,3})(?:\\[([0-9]{1,3})\\])?");
1053 :
1054 : // This regex is used to parse the traffic sign id. It matches the following groups:
1055 : // 1. country code (2 letters)
1056 : // 2. sign id (1-4 digits) e.g. 1020 or 274.1
1057 : // 3. value in brackets e.g. 1020[50] or 274.1[50] -> 50
1058 : // 4. value after the hyphen 1020-50 or 274.1-50 -> 50
1059 3 : std::regex re("([A-Z]{2}):([0-9.]{1,6}|[A-Z]{1}[0-9.]{2,6})(?:\\[(.*)\\])?(?:-(.*))?");
1060 : std::smatch match;
1061 : std::regex_match(trafficSign, match, re);
1062 3 : if (match.size() == 5) {
1063 : std::string value;
1064 : if (match[3].matched) {
1065 2 : value = match[3].str();
1066 2 : } else if (match[4].matched) {
1067 2 : value = match[4].str();
1068 : }
1069 6 : return TrafficSign{ match[1], match[2], "", value};
1070 : } else {
1071 0 : return TrafficSign{ "OpenDrive", trafficSign, "", ""};
1072 : }
1073 3 : }
1074 :
1075 :
1076 : void
1077 818 : NWWriter_OpenDrive::writeSignals(OutputDevice& device, const NBEdge* e, double length,
1078 : SignalLanes& signalLanes, const ShapeContainer& shc) {
1079 1636 : device.openTag("signals");
1080 818 : if (e->getToNode()->isTLControlled()) {
1081 : // try to faithfully represent the SUMO signal layout
1082 : // (if a realistic number of signals is needed, the user should set
1083 : // option --tls.group-signals)
1084 65 : NBTrafficLightDefinition* tl = *e->getToNode()->getControllingTLS().begin();
1085 : std::map<std::string, bool> toWrite;
1086 1208 : for (const NBConnection& c : tl->getControlledLinks()) {
1087 1143 : if (c.getFrom() == e) {
1088 476 : const std::string id = tl->getID() + "_" + toString(c.getTLIndex());
1089 : if (toWrite.count(id) == 0) {
1090 226 : toWrite[id] = signalLanes.count(id) == 0;
1091 : }
1092 238 : signalLanes[id].first.insert(c.getFromLane());
1093 238 : signalLanes[id].second.insert(e->getToNode()->getDirection(e, c.getTo()));
1094 : }
1095 : }
1096 291 : for (auto item : toWrite) {
1097 : const std::string id = item.first;
1098 226 : const bool isNew = item.second;
1099 226 : const std::set<LinkDirection>& dirs = signalLanes[id].second;
1100 : const bool l = dirs.count(LinkDirection::LEFT) != 0 || dirs.count(LinkDirection::PARTLEFT) != 0;
1101 : const bool r = dirs.count(LinkDirection::RIGHT) != 0 || dirs.count(LinkDirection::PARTRIGHT) != 0;
1102 226 : const bool s = dirs.count(LinkDirection::STRAIGHT) != 0;
1103 226 : const std::string tag = isNew ? "signal" : "signalReference";
1104 226 : int firstLane = *signalLanes[id].first.begin();
1105 226 : double t = e->getLaneWidth(firstLane) * 0.5;
1106 352 : for (int i = firstLane + 1; i < e->getNumLanes(); i++) {
1107 126 : t += e->getLaneWidth(i);
1108 : }
1109 226 : device.openTag(tag);
1110 226 : device.writeAttr("id", id);
1111 226 : device.writeAttr("s", length);
1112 226 : device.writeAttr("t", -t);
1113 226 : device.writeAttr("orientation", "+");
1114 226 : if (isNew) {
1115 226 : int type = 1000001;
1116 226 : int subType = -1;
1117 226 : if (l && !s && !r) {
1118 47 : type = 1000011;
1119 47 : subType = 10;
1120 179 : } else if (!l && !s && r) {
1121 48 : type = 1000011;
1122 48 : subType = 20;
1123 131 : } else if (!l && s && !r) {
1124 94 : type = 1000011;
1125 94 : subType = 30;
1126 37 : } else if (l && s && !r) {
1127 0 : type = 1000011;
1128 0 : subType = 40;
1129 37 : } else if (!l && s && r) {
1130 4 : type = 1000011;
1131 4 : subType = 50;
1132 : }
1133 226 : device.writeAttr("dynamic", "yes");
1134 226 : device.writeAttr("zOffset", 5);
1135 226 : device.writeAttr("country", "OpenDRIVE");
1136 226 : device.writeAttr("type", type);
1137 226 : device.writeAttr("subtype", subType);
1138 226 : device.writeAttr("height", 0.78);
1139 452 : device.writeAttr("width", 0.26);
1140 : }
1141 456 : for (int lane : signalLanes[id].first) {
1142 230 : device.openTag("validity");
1143 230 : device.writeAttr("fromLane", lane);
1144 230 : device.writeAttr("toLane", lane);
1145 460 : device.closeTag();
1146 : }
1147 452 : device.closeTag();
1148 : }
1149 1506 : } else if (e->hasParameter(ROAD_OBJECTS)) {
1150 48 : PositionVector roadShape = getInnerLaneBorder(e);
1151 182 : for (std::string id : StringTokenizer(e->getParameter(ROAD_OBJECTS, "")).getVector()) {
1152 : PointOfInterest* poi = shc.getPOIs().get(id);
1153 53 : if (poi != nullptr) {
1154 57 : if (poi->getShapeType() == "traffic_sign" && poi->hasParameter("traffic_sign")) {
1155 8 : std::string traffic_sign_type = poi->getParameter("traffic_sign");
1156 :
1157 4 : std::vector<TrafficSign> trafficSigns = parseTrafficSign(traffic_sign_type, poi);
1158 :
1159 4 : auto distance = roadShape.nearest_offset_to_point2D(*poi, true);
1160 4 : double t = getRoadSideOffset(e);
1161 4 : double calculatedZOffset = 3.0;
1162 10 : for (auto it = trafficSigns.rbegin(); it != trafficSigns.rend(); ++it) {
1163 6 : TrafficSign trafficSign = *it;
1164 6 : device.openTag("signal");
1165 6 : device.writeAttr("id", id);
1166 6 : device.writeAttr("s", distance);
1167 6 : device.writeAttr("t", t);
1168 6 : device.writeAttr("orientation", "-");
1169 6 : device.writeAttr("dynamic", "no");
1170 6 : device.writeAttr("zOffset", calculatedZOffset);
1171 6 : device.writeAttr("country", trafficSign.country);
1172 6 : device.writeAttr("type", trafficSign.type);
1173 6 : device.writeAttr("subtype", trafficSign.subtype);
1174 6 : device.writeAttr("value", trafficSign.value);
1175 6 : device.writeAttr("height", 0.78);
1176 6 : device.writeAttr("width", 0.78);
1177 6 : device.closeTag();
1178 6 : calculatedZOffset += 0.78;
1179 6 : }
1180 4 : }
1181 : }
1182 48 : }
1183 48 : }
1184 818 : device.closeTag();
1185 818 : }
1186 :
1187 :
1188 : double
1189 8 : NWWriter_OpenDrive::getRoadSideOffset(const NBEdge* e) {
1190 : double t = 0.30;
1191 8 : if (!lefthand || LHLL) {
1192 12 : for (int i = 0; i < e->getNumLanes(); i++) {
1193 6 : t += e->getPermissions(i) == SVC_PEDESTRIAN ? e->getLaneWidth(i) * 0.2 : e->getLaneWidth(i);
1194 : }
1195 : }
1196 8 : t = LHRL ? t : -t;
1197 8 : return t;
1198 : }
1199 :
1200 :
1201 : int
1202 6795 : NWWriter_OpenDrive::s2x(int sumoIndex, int numLanes) {
1203 : // sumo lanes: 0, 1, 2 (0 being the outermost lane)
1204 : // XODR: -3,-2,-1 (written in reverse order)
1205 : // LHLL: 3, 2, 1 (written in reverse order)
1206 : // lefthand (old):-1,-2,-3
1207 : return (lefthand
1208 6795 : ? (LHLL
1209 308 : ? numLanes - sumoIndex
1210 : : - sumoIndex - 1)
1211 6795 : : sumoIndex - numLanes);
1212 : }
1213 :
1214 :
1215 : void
1216 32 : NWWriter_OpenDrive::mapmatchRoadObjects(const ShapeContainer& shc, const NBEdgeCont& ec) {
1217 32 : if (shc.getPolygons().size() == 0 && shc.getPOIs().size() == 0) {
1218 27 : return;
1219 : }
1220 5 : const double maxDist = OptionsCont::getOptions().getFloat("opendrive-output.shape-match-dist");
1221 5 : if (maxDist < 0) {
1222 : return;
1223 : }
1224 : // register custom assignements
1225 : std::set<std::string> assigned;
1226 149 : for (auto it = ec.begin(); it != ec.end(); ++it) {
1227 144 : NBEdge* e = it->second;
1228 288 : if (e->hasParameter(ROAD_OBJECTS)) {
1229 0 : for (std::string id : StringTokenizer(e->getParameter(ROAD_OBJECTS, "")).getVector()) {
1230 : assigned.insert(id);
1231 0 : }
1232 : }
1233 : }
1234 : // build rtree for edges
1235 : NamedRTree r;
1236 149 : for (auto it = ec.begin(); it != ec.end(); ++it) {
1237 144 : NBEdge* edge = it->second;
1238 144 : Boundary bound = edge->getGeometry().getBoxBoundary();
1239 144 : bound.grow(maxDist);
1240 144 : float min[2] = { static_cast<float>(bound.xmin()), static_cast<float>(bound.ymin()) };
1241 144 : float max[2] = { static_cast<float>(bound.xmax()), static_cast<float>(bound.ymax()) };
1242 144 : r.Insert(min, max, edge);
1243 144 : }
1244 :
1245 67 : for (auto itPoly = shc.getPolygons().begin(); itPoly != shc.getPolygons().end(); itPoly++) {
1246 62 : SUMOPolygon* p = itPoly->second;
1247 62 : Boundary bound = p->getShape().getBoxBoundary();
1248 62 : float min[2] = { static_cast<float>(bound.xmin()), static_cast<float>(bound.ymin()) };
1249 62 : float max[2] = { static_cast<float>(bound.xmax()), static_cast<float>(bound.ymax()) };
1250 : std::set<const Named*> edges;
1251 : Named::StoringVisitor visitor(edges);
1252 : r.Search(min, max, visitor);
1253 : std::vector<std::pair<double, std::string> > nearby;
1254 2714 : for (const Named* namedEdge : edges) {
1255 2652 : NBEdge* e = const_cast<NBEdge*>(dynamic_cast<const NBEdge*>(namedEdge));
1256 2652 : const double distance = VectorHelper<double>::minValue(p->getShape().distances(e->getLaneShape(0), true));
1257 2652 : if (distance <= maxDist) {
1258 : // sort by distance and ID to stabilize results
1259 3562 : nearby.push_back(std::make_pair(distance, e->getID()));
1260 : //std::cout << " poly=" << p->getID() << " e=" << e->getID() << " dist=" << distance << "\n";
1261 : }
1262 : }
1263 62 : if (nearby.size() > 0) {
1264 62 : std::sort(nearby.begin(), nearby.end());
1265 62 : NBEdge* closest = ec.retrieve(nearby.front().second);
1266 124 : std::string objects = closest->getParameter(ROAD_OBJECTS, "");
1267 62 : if (objects != "") {
1268 : objects += " ";
1269 : }
1270 : objects += p->getID();
1271 124 : closest->setParameter(ROAD_OBJECTS, objects);
1272 : //std::cout << "poly=" << p->getID() << " closest=" << closest->getID() << "\n";
1273 : }
1274 124 : }
1275 63 : for (auto itPoi = shc.getPOIs().begin(); itPoi != shc.getPOIs().end(); itPoi++) {
1276 58 : PointOfInterest* p = itPoi->second;
1277 58 : float min[2] = { static_cast<float>(p->x()), static_cast<float>(p->y()) };
1278 58 : float max[2] = { static_cast<float>(p->x()), static_cast<float>(p->y()) };
1279 : std::set<const Named*> edges;
1280 : Named::StoringVisitor visitor(edges);
1281 : r.Search(min, max, visitor);
1282 : std::vector<std::pair<double, std::string> > nearby;
1283 1774 : for (const Named* namedEdge : edges) {
1284 1716 : NBEdge* e = const_cast<NBEdge*>(dynamic_cast<const NBEdge*>(namedEdge));
1285 1716 : const double distance = e->getLaneShape(0).distance2D(*p, true);
1286 1716 : if (distance != GeomHelper::INVALID_OFFSET && distance <= maxDist) {
1287 : // sort by distance and ID to stabilize results
1288 766 : nearby.push_back(std::make_pair(distance, e->getID()));
1289 : //if (p->getID() == "1275468911") {
1290 : // std::cout << " poly=" << p->getID() << " e=" << e->getID() << " dist=" << distance << "\n";
1291 : //}
1292 : }
1293 : }
1294 58 : if (nearby.size() > 0) {
1295 58 : std::sort(nearby.begin(), nearby.end());
1296 58 : NBEdge* closest = ec.retrieve(nearby.front().second);
1297 116 : std::string objects = closest->getParameter(ROAD_OBJECTS, "");
1298 58 : if (objects != "") {
1299 : objects += " ";
1300 : }
1301 : objects += p->getID();
1302 116 : closest->setParameter(ROAD_OBJECTS, objects);
1303 : //std::cout << "poly=" << p->getID() << " closest=" << closest->getID() << "\n";
1304 : }
1305 58 : }
1306 : }
1307 :
1308 :
1309 : void
1310 58 : NWWriter_OpenDrive::writeRoadObjectPOI(OutputDevice& device, const NBEdge* e, const PositionVector& roadShape, const PointOfInterest* poi) {
1311 58 : Position center = *poi;
1312 58 : const double edgeOffset = roadShape.nearest_offset_to_point2D(center, false);
1313 58 : if (edgeOffset == GeomHelper::INVALID_OFFSET) {
1314 0 : WRITE_WARNINGF("Cannot map road object POI '%' with center % onto edge '%'", poi->getID(), center, e->getID());
1315 : return;
1316 : }
1317 :
1318 58 : Position edgePos = roadShape.positionAtOffset2D(edgeOffset);
1319 58 : double sideOffset = center.distanceTo2D(edgePos);
1320 : // determine sign of sideOffset
1321 174 : PositionVector tmp = roadShape.getSubpart2D(MAX2(0.0, edgeOffset - 1), MIN2(roadShape.length2D(), edgeOffset + 1));
1322 58 : tmp.move2side(sideOffset);
1323 58 : if (tmp.distance2D(center) < sideOffset) {
1324 45 : sideOffset *= -1;
1325 : }
1326 : // place traffic signs on appropriate side of the road
1327 : std::string type = poi->getShapeType();
1328 116 : std::string name = StringUtils::escapeXML(poi->getParameter("name", ""), true);
1329 58 : if (poi->getShapeType() == "traffic_sign") {
1330 4 : sideOffset = getRoadSideOffset(e);
1331 : type = "pole";
1332 : name = "pole";
1333 : }
1334 :
1335 116 : device.openTag("object");
1336 58 : device.writeAttr("id", poi->getID());
1337 58 : device.writeAttr("type", type);
1338 58 : device.writeAttr("name", name);
1339 58 : device.writeAttr("s", edgeOffset);
1340 58 : device.writeAttr("t", sideOffset);
1341 58 : device.writeAttr("hdg", 0);
1342 116 : device.closeTag();
1343 58 : }
1344 :
1345 : void
1346 101 : NWWriter_OpenDrive::writeRoadObjectPoly(OutputDevice& device, const NBEdge* e, const PositionVector& roadShape, const SUMOPolygon* p) {
1347 101 : PositionVector shape = p->getShape();
1348 101 : Position center = shape.getPolygonCenter();
1349 :
1350 101 : const double edgeOffset = roadShape.nearest_offset_to_point2D(center, false);
1351 101 : if (edgeOffset == GeomHelper::INVALID_OFFSET) {
1352 0 : WRITE_WARNINGF("Cannot map road object polygon '%' with center % onto edge '%'", p->getID(), center, e->getID());
1353 : return;
1354 : }
1355 101 : Position edgePos = roadShape.positionAtOffset2D(edgeOffset);
1356 101 : const double edgeAngle = roadShape.rotationAtOffset(edgeOffset);
1357 : double sideOffset = center.distanceTo2D(edgePos);
1358 : // determine sign of sideOffset
1359 269 : PositionVector tmp = roadShape.getSubpart2D(MAX2(0.0, edgeOffset - 1), MIN2(roadShape.length2D(), edgeOffset + 1));
1360 101 : tmp.move2side(sideOffset);
1361 101 : if (tmp.distance2D(center) < sideOffset) {
1362 62 : sideOffset *= -1;
1363 : }
1364 : //std::cout << " id=" << id
1365 : // << " shape=" << shape
1366 : // << " center=" << center
1367 : // << " edgeOffset=" << edgeOffset
1368 : // << "\n";
1369 : auto shapeType = p->getShapeType();
1370 202 : device.openTag("object");
1371 101 : device.writeAttr("id", p->getID());
1372 101 : device.writeAttr("type", shapeType);
1373 202 : if (p->hasParameter("name")) {
1374 36 : device.writeAttr("name", StringUtils::escapeXML(p->getParameter("name", ""), true));
1375 : }
1376 202 : device.writeAttr("s", edgeOffset);
1377 132 : device.writeAttr("t", shapeType == "crosswalk" && !lefthand ? 0 : sideOffset);
1378 101 : double hdg = -edgeAngle;
1379 202 : if (p->hasParameter("hdg")) {
1380 : try {
1381 39 : hdg = StringUtils::toDoubleSecure(p->getParameter("hdg", ""), 0);
1382 0 : } catch (NumberFormatException&) {}
1383 : }
1384 101 : device.writeAttr("hdg", hdg);
1385 202 : if (p->hasParameter("length")) {
1386 : try {
1387 78 : device.writeAttr("length", StringUtils::toDoubleSecure(p->getParameter("length", ""), 0));
1388 0 : } catch (NumberFormatException&) {}
1389 : }
1390 202 : if (p->hasParameter("width")) {
1391 : try {
1392 78 : device.writeAttr("width", StringUtils::toDoubleSecure(p->getParameter("width", ""), 0));
1393 0 : } catch (NumberFormatException&) {}
1394 : }
1395 101 : double height = 0;
1396 202 : if (p->hasParameter("height")) {
1397 : try {
1398 3 : height = StringUtils::toDoubleSecure(p->getParameter("height", ""), 0);
1399 6 : device.writeAttr("height", height);
1400 0 : } catch (NumberFormatException&) {}
1401 : }
1402 : //device.openTag("outlines");
1403 101 : device.openTag("outline");
1404 101 : device.writeAttr("id", 0);
1405 101 : device.writeAttr("fillType", "concrete");
1406 101 : device.writeAttr("outer", "true");
1407 142 : device.writeAttr("closed", p->getShape().isClosed() ? "true" : "false");
1408 101 : device.writeAttr("laneType", "border");
1409 :
1410 101 : shape.sub(center);
1411 101 : if (hdg != -edgeAngle) {
1412 36 : shape.rotate2D(-edgeAngle - hdg);
1413 : }
1414 : int i = 0;
1415 1244 : for (Position pos : shape) {
1416 1143 : device.openTag("cornerLocal");
1417 1143 : device.writeAttr("u", pos.x());
1418 2286 : device.writeAttr("v", pos.y());
1419 1143 : device.writeAttr("z", MAX2(0.0, p->getShapeLayer()));
1420 1143 : device.writeAttr("height", height);
1421 1143 : device.writeAttr("id", i++);
1422 2286 : device.closeTag();
1423 : }
1424 :
1425 :
1426 : //device.closeTag();
1427 101 : device.closeTag();
1428 202 : device.closeTag();
1429 101 : }
1430 :
1431 : /****************************************************************************/
|
