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 1689 : NWWriter_OpenDrive::writeNetwork(const OptionsCont& oc, NBNetBuilder& nb) {
64 : // check whether an opendrive-file shall be generated
65 3378 : if (!oc.isSet("opendrive-output")) {
66 1658 : return;
67 : }
68 : const NBNodeCont& nc = nb.getNodeCont();
69 : const NBEdgeCont& ec = nb.getEdgeCont();
70 31 : const bool origNames = oc.getBool("output.original-names");
71 31 : lefthand = oc.getBool("lefthand");
72 31 : LHLL = lefthand && oc.getBool("opendrive-output.lefthand-left");
73 31 : LHRL = lefthand && !LHLL;
74 31 : const double straightThresh = DEG2RAD(oc.getFloat("opendrive-output.straight-threshold"));
75 : // some internal mapping containers
76 31 : int nodeID = 1;
77 31 : int edgeID = nc.size() * 10; // distinct from node ids
78 : StringBijection<int> edgeMap;
79 : StringBijection<int> nodeMap;
80 : //
81 31 : OutputDevice& device = OutputDevice::getDevice(oc.getString("opendrive-output"));
82 62 : OutputDevice::createDeviceByOption("opendrive-output", "OpenDRIVE");
83 31 : time_t now = time(nullptr);
84 31 : std::string dstr(ctime(&now));
85 31 : const Boundary& b = GeoConvHelper::getFinal().getConvBoundary();
86 : // write header
87 31 : device.openTag("header");
88 31 : device.writeAttr("revMajor", "1");
89 31 : device.writeAttr("revMinor", "4");
90 31 : device.writeAttr("name", "");
91 62 : device.writeAttr("version", "1.00");
92 62 : device.writeAttr("date", dstr.substr(0, dstr.length() - 1));
93 31 : device.writeAttr("north", b.ymax());
94 31 : device.writeAttr("south", b.ymin());
95 31 : device.writeAttr("east", b.xmax());
96 31 : 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 31 : if (gch.usingGeoProjection()) {
105 9 : device.openTag("geoReference");
106 : device.writePreformattedTag(" <![CDATA[\n "
107 9 : + gch.getProjString()
108 9 : + "\n]]>\n");
109 9 : device.closeTag();
110 9 : if (gch.getOffsetBase() != Position(0, 0)) {
111 9 : device.openTag("offset");
112 18 : device.writeAttr("x", -gch.getOffsetBase().x());
113 18 : device.writeAttr("y", -gch.getOffsetBase().y());
114 18 : device.writeAttr("z", -gch.getOffsetBase().z());
115 9 : device.writeAttr("hdg", 0);
116 18 : device.closeTag();
117 : }
118 : }
119 62 : device.closeTag();
120 :
121 : SignalLanes signalLanes;
122 :
123 31 : mapmatchRoadObjects(nb.getShapeCont(), ec);
124 :
125 31 : PositionVector crosswalk_shape;
126 : std::map<std::string, std::vector<std::string>> crosswalksByEdge;
127 395 : for (auto it = nc.begin(); it != nc.end(); ++it) {
128 364 : NBNode* n = it->second;
129 364 : auto crosswalks = n->getCrossings();
130 403 : 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 403 : }
150 364 : }
151 :
152 : // write normal edges (road)
153 658 : for (std::map<std::string, NBEdge*>::const_iterator i = ec.begin(); i != ec.end(); ++i) {
154 627 : const NBEdge* e = (*i).second;
155 627 : const int fromNodeID = e->getIncomingEdges().size() > 0 ? getID(e->getFromNode()->getID(), nodeMap, nodeID) : INVALID_ID;
156 627 : const int toNodeID = e->getConnections().size() > 0 ? getID(e->getToNode()->getID(), nodeMap, nodeID) : INVALID_ID;
157 627 : writeNormalEdge(device, e,
158 627 : getID(e->getID(), edgeMap, edgeID),
159 : fromNodeID, toNodeID,
160 : origNames, straightThresh,
161 : nb.getShapeCont(),
162 : signalLanes,
163 627 : crosswalksByEdge[e->getID()]);
164 : }
165 31 : device.lf();
166 :
167 : // write junction-internal edges (road). In OpenDRIVE these are called 'paths' or 'connecting roads'
168 31 : OutputDevice_String junctionOSS(3);
169 395 : for (std::map<std::string, NBNode*>::const_iterator i = nc.begin(); i != nc.end(); ++i) {
170 364 : NBNode* n = (*i).second;
171 : int connectionID = 0; // unique within a junction
172 364 : const int nID = getID(n->getID(), nodeMap, nodeID);
173 364 : if (n->numNormalConnections() > 0) {
174 223 : junctionOSS << " <junction name=\"" << n->getID() << "\" id=\"" << nID << "\">\n";
175 : }
176 364 : std::vector<NBEdge*> incoming = (*i).second->getIncomingEdges();
177 364 : if (lefthand) {
178 : std::reverse(incoming.begin(), incoming.end());
179 : }
180 991 : for (NBEdge* inEdge : incoming) {
181 627 : std::string centerMark = "none";
182 627 : 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 627 : std::vector<NBEdge::Connection> connections = inEdge->getConnections();
189 1914 : for (const NBEdge::Connection& c : connections) {
190 : assert(c.toEdge != 0);
191 1287 : if (outEdge != c.toEdge || c.fromLane == lastFromLane) {
192 1186 : if (outEdge != nullptr) {
193 714 : if (isOuterEdge) {
194 337 : addPedestrianConnection(inEdge, outEdge, parallel);
195 : }
196 1428 : connectionID = writeInternalEdge(device, junctionOSS, inEdge, nID,
197 1428 : getID(parallel.back().getInternalLaneID(), edgeMap, edgeID),
198 : inEdgeID,
199 714 : getID(outEdge->getID(), edgeMap, edgeID),
200 : connectionID,
201 : parallel, isOuterEdge, straightThresh, centerMark,
202 : signalLanes);
203 : parallel.clear();
204 : isOuterEdge = false;
205 : }
206 1186 : outEdge = c.toEdge;
207 : }
208 1287 : lastFromLane = c.fromLane;
209 1287 : parallel.push_back(c);
210 : }
211 627 : if (isOuterEdge) {
212 290 : addPedestrianConnection(inEdge, outEdge, parallel);
213 : }
214 627 : if (!parallel.empty()) {
215 472 : if (!lefthand && (n->geometryLike() || inEdge->isTurningDirectionAt(outEdge))) {
216 : centerMark = "solid";
217 : }
218 944 : connectionID = writeInternalEdge(device, junctionOSS, inEdge, nID,
219 944 : getID(parallel.back().getInternalLaneID(), edgeMap, edgeID),
220 : inEdgeID,
221 472 : getID(outEdge->getID(), edgeMap, edgeID),
222 : connectionID,
223 : parallel, isOuterEdge, straightThresh, centerMark,
224 : signalLanes);
225 : parallel.clear();
226 : }
227 627 : }
228 364 : if (n->numNormalConnections() > 0) {
229 223 : junctionOSS << " </junction>\n";
230 : }
231 364 : }
232 31 : device.lf();
233 : // write controllers
234 395 : for (std::map<std::string, NBNode*>::const_iterator i = nc.begin(); i != nc.end(); ++i) {
235 364 : NBNode* n = (*i).second;
236 364 : if (n->isTLControlled()) {
237 18 : NBTrafficLightDefinition* tl = *n->getControllingTLS().begin();
238 : std::set<std::string> ids;
239 36 : device.openTag("controller");
240 36 : device.writeAttr("id", tl->getID());
241 271 : for (const NBConnection& c : tl->getControlledLinks()) {
242 506 : const std::string id = tl->getID() + "_" + toString(c.getTLIndex());
243 : if (ids.count(id) == 0) {
244 : ids.insert(id);
245 241 : device.openTag("control");
246 241 : device.writeAttr("signalId", id);
247 482 : device.closeTag();
248 : }
249 : }
250 36 : device.closeTag();
251 : }
252 : }
253 : // write junctions (junction)
254 31 : device << junctionOSS.getString();
255 :
256 31 : device.closeTag();
257 31 : device.close();
258 93 : }
259 :
260 :
261 : std::string
262 623 : NWWriter_OpenDrive::getDividerType(const NBEdge* e) {
263 : std::map<std::string, std::string> dividerTypeMapping;
264 623 : dividerTypeMapping["solid_line"] = "solid";
265 623 : dividerTypeMapping["dashed_line"] = "broken";
266 623 : dividerTypeMapping["double_solid_line"] = "solid solid";
267 623 : dividerTypeMapping["no"] = "none";
268 :
269 : // defaulting to solid as in the original code
270 623 : std::string dividerType = "solid";
271 :
272 1246 : 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 623 : return dividerType;
279 : }
280 :
281 :
282 : void
283 627 : 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 627 : OutputDevice_String elevationOSS(3);
292 627 : elevationOSS.setPrecision(8);
293 627 : OutputDevice_String planViewOSS(2);
294 627 : planViewOSS.setPrecision(8);
295 627 : double length = 0;
296 :
297 627 : 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 627 : 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 627 : if (ls.size() == 2 || e->getPermissions() == SVC_PEDESTRIAN) {
308 : // foot paths may contain sharp angles
309 482 : length = writeGeomLines(ls, planViewOSS, elevationOSS);
310 : } else {
311 145 : bool ok = writeGeomSmooth(ls, e->getSpeed(), planViewOSS, elevationOSS, straightThresh, length);
312 145 : if (!ok) {
313 3 : WRITE_WARNINGF(TL("Could not compute smooth shape for edge '%'."), e->getID());
314 : }
315 : }
316 627 : planViewOSS.closeTag();
317 :
318 1254 : device.openTag("road");
319 1254 : device.writeAttr("name", StringUtils::escapeXML(e->getStreetName()));
320 627 : device.setPrecision(8); // length requires higher precision
321 1254 : device.writeAttr("length", MAX2(POSITION_EPS, length));
322 627 : device.setPrecision(gPrecision);
323 627 : device.writeAttr("id", edgeID);
324 627 : device.writeAttr("junction", -1);
325 627 : if (fromNodeID != INVALID_ID || toNodeID != INVALID_ID) {
326 516 : device.openTag("link");
327 516 : if (fromNodeID != INVALID_ID) {
328 501 : device.openTag("predecessor");
329 501 : device.writeAttr("elementType", "junction");
330 501 : device.writeAttr("elementId", fromNodeID);
331 1002 : device.closeTag();
332 : }
333 516 : if (toNodeID != INVALID_ID) {
334 472 : device.openTag("successor");
335 472 : device.writeAttr("elementType", "junction");
336 472 : device.writeAttr("elementId", toNodeID);
337 944 : device.closeTag();
338 : }
339 1032 : device.closeTag();
340 : }
341 2508 : device.openTag("type").writeAttr("s", 0).writeAttr("type", "town").closeTag();
342 627 : device << planViewOSS.getString();
343 627 : writeElevationProfile(ls, device, elevationOSS);
344 627 : device << " <lateralProfile/>\n";
345 627 : device << " <lanes>\n";
346 627 : device << " <laneSection s=\"0\">\n";
347 627 : const std::string centerMark = e->getPermissions(e->getNumLanes() - 1) == 0 ? "none" : getDividerType(e);
348 627 : if (!LHLL) {
349 609 : writeEmptyCenterLane(device, centerMark, 0.13);
350 : }
351 1236 : const std::string side = LHLL ? "left" : "right";
352 627 : device << " <" << side << ">\n";
353 : const int numLanes = e->getNumLanes();
354 1492 : 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 865 : const int j = lefthand ? numLanes - 1 - jRH : jRH;
358 : std::string laneType = e->getLaneStruct(j).type;
359 865 : if (laneType == "") {
360 1668 : laneType = getLaneType(e->getPermissions(j));
361 : }
362 865 : device << " <lane id=\"" << s2x(j, numLanes) << "\" type=\"" << laneType << "\" level=\"true\">\n";
363 865 : 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 865 : device << " <width sOffset=\"0\" a=\"" << e->getLaneWidth(j) << "\" b=\"0\" c=\"0\" d=\"0\"/>\n";
372 865 : std::string markType = "broken";
373 865 : 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 831 : if (j == 0) {
383 : markType = "solid";
384 : } else if (j > 0
385 222 : && (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 132 : } else if (e->getPermissions(j) == 0) {
389 : // solid road mark to the right of a forbidden lane
390 : markType = "solid";
391 : }
392 : }
393 865 : device << " <roadMark sOffset=\"0\" type=\"" << markType << "\" weight=\"standard\" color=\"standard\" width=\"0.13\"/>\n";
394 865 : device << " <speed sOffset=\"0\" max=\"" << lanes[j].speed << "\"/>\n";
395 865 : device << " </lane>\n";
396 : }
397 627 : device << " </" << side << ">\n";
398 627 : if (LHLL) {
399 18 : writeEmptyCenterLane(device, centerMark, 0.13);
400 : }
401 627 : device << " </laneSection>\n";
402 627 : device << " </lanes>\n";
403 627 : writeRoadObjects(device, e, shc, crossings);
404 627 : writeSignals(device, e, length, signalLanes, shc);
405 627 : if (origNames) {
406 404 : device << " <userData code=\"sumoId\" value=\"" << e->getID() << "\"/>\n";
407 : }
408 627 : device.closeTag();
409 627 : checkLaneGeometries(e);
410 627 : }
411 :
412 : void
413 627 : 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 472 : && inEdge->getPermissions(0) == SVC_PEDESTRIAN
418 90 : && outEdge->getPermissions(0) == SVC_PEDESTRIAN
419 704 : && (parallel.empty()
420 77 : || parallel.front().fromLane != 0
421 32 : || 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 627 : }
426 :
427 :
428 : int
429 1186 : 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 1186 : const NBEdge::Connection& cLeft = LHRL ? parallel.front() : parallel.back();
439 1186 : const NBEdge* outEdge = cLeft.toEdge;
440 1186 : PositionVector begShape = getInnerLaneBorder(inEdge, cLeft.fromLane);
441 1186 : 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 1186 : double laneOffset = 0;
446 1186 : PositionVector fallBackShape;
447 1186 : fallBackShape.push_back(begShape.back());
448 1186 : fallBackShape.push_back(endShape.front());
449 1186 : const bool turnaround = inEdge->isTurningDirectionAt(outEdge);
450 1186 : bool ok = true;
451 1186 : PositionVector init = NBNode::bezierControlPoints(begShape, endShape, turnaround, 25, 25, ok, nullptr, straightThresh);
452 1186 : if (init.size() == 0) {
453 363 : length = fallBackShape.length2D();
454 : // problem with turnarounds is known, method currently returns 'ok' (#2539)
455 363 : if (!ok) {
456 12 : 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 359 : } else if (length <= NUMERICAL_EPS) {
458 : // left-curving geometry-like edges must use the right
459 : // side as reference line and shift
460 624 : begShape = getOuterLaneBorder(inEdge, cLeft.fromLane);
461 624 : endShape = getOuterLaneBorder(outEdge, cLeft.toLane);
462 624 : init = NBNode::bezierControlPoints(begShape, endShape, turnaround, 25, 25, ok, nullptr, straightThresh);
463 312 : if (init.size() != 0) {
464 312 : length = init.bezier(12).length2D();
465 312 : laneOffset = outEdge->getLaneWidth(cLeft.toLane);
466 : //std::cout << " internalLane=" << cLeft.getInternalLaneID() << " length=" << length << "\n";
467 : }
468 : }
469 : } else {
470 823 : length = init.bezier(12).length2D();
471 : }
472 1186 : double roadLength = MAX2(POSITION_EPS, length);
473 :
474 1186 : junctionDevice << " <connection id=\"" << connectionID << "\" incomingRoad=\"" << inEdgeID << "\" connectingRoad=\"" << edgeID << "\" contactPoint=\"start\">\n";
475 1186 : device.openTag("road");
476 1186 : device.writeAttr("name", cLeft.id);
477 1186 : device.setPrecision(8); // length requires higher precision
478 1186 : device.writeAttr("length", roadLength);
479 1186 : device.setPrecision(gPrecision);
480 1186 : device.writeAttr("id", edgeID);
481 1186 : device.writeAttr("junction", nodeID);
482 1186 : device.openTag("link");
483 1186 : device.openTag("predecessor");
484 1186 : device.writeAttr("elementType", "road");
485 1186 : device.writeAttr("elementId", inEdgeID);
486 1186 : device.writeAttr("contactPoint", "end");
487 1186 : device.closeTag();
488 1186 : device.openTag("successor");
489 1186 : device.writeAttr("elementType", "road");
490 1186 : device.writeAttr("elementId", outEdgeID);
491 1186 : device.writeAttr("contactPoint", "start");
492 1186 : device.closeTag();
493 1186 : device.closeTag();
494 4744 : device.openTag("type").writeAttr("s", 0).writeAttr("type", "town").closeTag();
495 1186 : device.openTag("planView");
496 1186 : device.setPrecision(8); // geometry hdg requires higher precision
497 1186 : OutputDevice_String elevationOSS(3);
498 1186 : 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 1186 : if (init.size() == 0) {
507 51 : writeGeomLines(fallBackShape, device, elevationOSS);
508 : } else {
509 1135 : writeGeomPP3(device, elevationOSS, init, length);
510 : }
511 1186 : device.setPrecision(gPrecision);
512 1186 : device.closeTag();
513 1186 : writeElevationProfile(fallBackShape, device, elevationOSS);
514 1186 : device << " <lateralProfile/>\n";
515 1186 : device << " <lanes>\n";
516 1186 : if (laneOffset != 0) {
517 312 : device << " <laneOffset s=\"0\" a=\"" << laneOffset << "\" b=\"0\" c=\"0\" d=\"0\"/>\n";
518 : }
519 1186 : device << " <laneSection s=\"0\">\n";
520 1186 : if (!lefthand) {
521 1122 : writeEmptyCenterLane(device, centerMark, 0);
522 : }
523 2308 : const std::string side = lefthand ? "left" : "right";
524 1186 : device << " <" << side << ">\n";
525 1186 : const int numLanes = (int)parallel.size();
526 2518 : for (int jRH = numLanes; --jRH >= 0;) {
527 1332 : const int j = lefthand ? numLanes - 1 - jRH : jRH;
528 1332 : const int xJ = s2x(j, numLanes);
529 1332 : const NBEdge::Connection& c = parallel[j];
530 1332 : const int fromIndex = s2x(c.fromLane, inEdge->getNumLanes());
531 1332 : const int toIndex = s2x(c.toLane, outEdge->getNumLanes());
532 :
533 1332 : double inEdgeWidth = inEdge->getLaneWidth(c.fromLane);
534 1332 : 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 1332 : double bCoefficient = (outEdgeWidth - inEdgeWidth) / roadLength;
539 1332 : double cCoefficient = 0;
540 1332 : double dCoefficient = 0;
541 1332 : device << " <lane id=\"" << xJ << "\" type=\"" << getLaneType(outEdge->getPermissions(c.toLane)) << "\" level=\"true\">\n";
542 1332 : device << " <link>\n";
543 1332 : device << " <predecessor id=\"" << fromIndex << "\"/>\n";
544 1332 : device << " <successor id=\"" << toIndex << "\"/>\n";
545 1332 : device << " </link>\n";
546 1332 : device << " <width sOffset=\"0\" a=\"" << inEdgeWidth << "\" b=\"" << bCoefficient << "\" c=\"" << cCoefficient << "\" d=\"" << dCoefficient << "\"/>\n";
547 1332 : std::string markType = "broken";
548 1332 : if (inEdge->isTurningDirectionAt(outEdge)) {
549 : markType = "none";
550 996 : } else if (c.fromLane == 0 && c.toLane == 0 && isOuterEdge) {
551 : // solid road mark at the outer border
552 : markType = "solid";
553 585 : } else if (isOuterEdge && j > 0
554 585 : && (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 533 : } else if (!inEdge->getToNode()->geometryLike()) {
558 : // draw shorter road marks to indicate turning paths
559 496 : LinkDirection dir = inEdge->getToNode()->getDirection(inEdge, outEdge, lefthand);
560 496 : 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 1332 : device << " <roadMark sOffset=\"0\" type=\"" << markType << "\" weight=\"standard\" color=\"standard\" width=\"0.13\"/>\n";
569 1332 : device << " <speed sOffset=\"0\" max=\"" << c.vmax << "\"/>\n";
570 1332 : device << " </lane>\n";
571 :
572 1332 : junctionDevice << " <laneLink from=\"" << fromIndex << "\" to=\"" << xJ << "\"/>\n";
573 1332 : connectionID++;
574 : }
575 1186 : device << " </" << side << ">\n";
576 1186 : if (lefthand) {
577 64 : writeEmptyCenterLane(device, centerMark, 0);
578 : }
579 1186 : device << " </laneSection>\n";
580 1186 : device << " </lanes>\n";
581 1186 : device << " <objects/>\n";
582 : UNUSED_PARAMETER(signalLanes);
583 1186 : device << " <signals/>\n";
584 1186 : device.closeTag();
585 1186 : junctionDevice << " </connection>\n";
586 :
587 2372 : return connectionID;
588 1186 : }
589 :
590 :
591 : double
592 913 : NWWriter_OpenDrive::writeGeomLines(const PositionVector& shape, OutputDevice& device, OutputDevice& elevationDevice, double offset) {
593 1894 : for (int j = 0; j < (int)shape.size() - 1; ++j) {
594 981 : const Position& p = shape[j];
595 981 : const Position& p2 = shape[j + 1];
596 981 : const double hdg = shape.angleAt2D(j);
597 : const double length = p.distanceTo2D(p2);
598 981 : device.openTag("geometry");
599 1962 : device.writeAttr("s", offset);
600 1962 : device.writeAttr("x", p.x());
601 981 : device.writeAttr("y", p.y());
602 981 : device.writeAttr("hdg", hdg);
603 1962 : device.writeAttr("length", length < 1e-8 ? 1e-8 : length);
604 1962 : device.openTag("line").closeTag();
605 981 : device.closeTag();
606 1960 : elevationDevice << " <elevation s=\"" << offset << "\" a=\"" << p.z() << "\" b=\"" << (p2.z() - p.z()) / MAX2(POSITION_EPS, length) << "\" c=\"0\" d=\"0\"/>\n";
607 981 : offset += length;
608 : }
609 913 : return offset;
610 : }
611 :
612 :
613 : void
614 1813 : NWWriter_OpenDrive::writeEmptyCenterLane(OutputDevice& device, const std::string& mark, double markWidth) {
615 1813 : device << " <center>\n";
616 1813 : device << " <lane id=\"0\" type=\"none\" level=\"true\">\n";
617 1813 : device << " <link/>\n";
618 1813 : device << " <roadMark sOffset=\"0\" type=\"" << mark << "\" weight=\"standard\" color=\"standard\" width=\"" << markWidth << "\"/>\n";
619 1813 : device << " </lane>\n";
620 1813 : device << " </center>\n";
621 1813 : }
622 :
623 :
624 : int
625 4963 : NWWriter_OpenDrive::getID(const std::string& origID, StringBijection<int>& map, int& lastID) {
626 : if (map.hasString(origID)) {
627 2786 : return map.get(origID);
628 : }
629 2177 : map.insert(origID, lastID++);
630 2177 : return lastID - 1;
631 : }
632 :
633 :
634 : std::string
635 2166 : NWWriter_OpenDrive::getLaneType(SVCPermissions permissions) {
636 2166 : switch (permissions) {
637 : case SVC_PEDESTRIAN:
638 405 : 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 73 : 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 1654 : default: {
655 : // complex permissions
656 1654 : if (permissions == SVCAll) {
657 689 : return "driving";
658 965 : } else if (isRailway(permissions)) {
659 0 : return "rail";
660 965 : } else if ((permissions & SVC_PASSENGER) != 0) {
661 714 : return "driving";
662 : } else {
663 251 : return "restricted";
664 : }
665 : }
666 : }
667 : }
668 :
669 :
670 : PositionVector
671 3757 : NWWriter_OpenDrive::getInnerLaneBorder(const NBEdge* edge, int laneIndex, double widthOffset) {
672 3757 : if (laneIndex == -1) {
673 : // innermost lane
674 761 : laneIndex = (int)edge->getNumLanes() - 1;
675 761 : if (LHRL) {
676 : laneIndex = 0;
677 : }
678 : }
679 3757 : PositionVector result = edge->getLaneShape(laneIndex);
680 7410 : widthOffset -= (LHLL ? -1 : 1) * edge->getLaneWidth(laneIndex) / 2;
681 : try {
682 3757 : result.move2side(widthOffset);
683 0 : } catch (InvalidArgument&) { }
684 3757 : return result;
685 0 : }
686 :
687 :
688 : PositionVector
689 624 : NWWriter_OpenDrive::getOuterLaneBorder(const NBEdge* edge, int laneIndex) {
690 624 : return getInnerLaneBorder(edge, laneIndex, edge->getLaneWidth(laneIndex));
691 : }
692 :
693 :
694 : double
695 1563 : 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 3126 : length = MAX2(POSITION_EPS, length);
705 :
706 1563 : const Position p = init.front();
707 1563 : const double hdg = init.angleAt2D(0);
708 :
709 : // backup elevation values
710 : const PositionVector initZ = init;
711 : // translate to u,v coordinates
712 1563 : init.add(-p.x(), -p.y(), -p.z());
713 1563 : 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 1563 : if (init.size() == 3) {
722 : //f(x, a, b ,c) = a + (2*b - 2*a)*x + (a - 2*b + c)*x*x
723 862 : aU = init[0].x();
724 862 : bU = 2 * init[1].x() - 2 * init[0].x();
725 862 : cU = init[0].x() - 2 * init[1].x() + init[2].x();
726 862 : dU = 0;
727 :
728 862 : aV = init[0].y();
729 862 : bV = 2 * init[1].y() - 2 * init[0].y();
730 862 : cV = init[0].y() - 2 * init[1].y() + init[2].y();
731 862 : dV = 0;
732 :
733 : // elevation is not parameteric on [0:1] but on [0:length]
734 862 : aZ = initZ[0].z();
735 862 : bZ = (2 * initZ[1].z() - 2 * initZ[0].z()) / length;
736 862 : cZ = (initZ[0].z() - 2 * initZ[1].z() + initZ[2].z()) / (length * length);
737 862 : 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 701 : aU = init[0].x();
742 701 : bU = 3 * init[1].x() - 3 * init[0].x();
743 701 : cU = 3 * init[0].x() - 6 * init[1].x() + 3 * init[2].x();
744 701 : dU = -init[0].x() + 3 * init[1].x() - 3 * init[2].x() + init[3].x();
745 :
746 701 : aV = init[0].y();
747 701 : bV = 3 * init[1].y() - 3 * init[0].y();
748 701 : cV = 3 * init[0].y() - 6 * init[1].y() + 3 * init[2].y();
749 701 : 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 701 : aZ = initZ[0].z();
753 701 : bZ = (3 * initZ[1].z() - 3 * initZ[0].z()) / length;
754 701 : cZ = (3 * initZ[0].z() - 6 * initZ[1].z() + 3 * initZ[2].z()) / (length * length);
755 701 : dZ = (-initZ[0].z() + 3 * initZ[1].z() - 3 * initZ[2].z() + initZ[3].z()) / (length * length * length);
756 : }
757 :
758 1563 : device.openTag("geometry");
759 1563 : device.writeAttr("s", offset);
760 1563 : device.writeAttr("x", p.x());
761 1563 : device.writeAttr("y", p.y());
762 1563 : device.writeAttr("hdg", hdg);
763 1563 : device.writeAttr("length", length);
764 :
765 1563 : device.openTag("paramPoly3");
766 1563 : device.writeAttr("aU", aU);
767 1563 : device.writeAttr("bU", bU);
768 1563 : device.writeAttr("cU", cU);
769 1563 : device.writeAttr("dU", dU);
770 1563 : device.writeAttr("aV", aV);
771 1563 : device.writeAttr("bV", bV);
772 1563 : device.writeAttr("cV", cV);
773 1563 : device.writeAttr("dV", dV);
774 1563 : device.writeAttr("pRange", "normalized");
775 1563 : device.closeTag();
776 1563 : device.closeTag();
777 :
778 : // write elevation
779 1563 : elevationDevice.openTag("elevation");
780 1563 : elevationDevice.writeAttr("s", offset);
781 1563 : elevationDevice.writeAttr("a", aZ);
782 1563 : elevationDevice.writeAttr("b", bZ);
783 1563 : elevationDevice.writeAttr("c", cZ);
784 1563 : elevationDevice.writeAttr("d", dZ);
785 1563 : elevationDevice.closeTag();
786 :
787 1563 : return offset + length;
788 1563 : }
789 :
790 :
791 : bool
792 145 : 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 145 : bool ok = true;
799 : const double longThresh = speed; // 16.0; // make user-configurable (should match the sampling rate of the source data)
800 145 : 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 557 : for (int j = 1; j < (int)shape.size() - 1; ++j) {
810 : //const double hdg = shape.angleAt2D(j);
811 412 : const Position& p0 = shape[j - 1];
812 412 : const Position& p1 = shape[j];
813 412 : const Position& p2 = shape[j + 1];
814 412 : 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 412 : && (length1 > longThresh || j == 1)
825 700 : && (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 489 : shape2.insertAtClosest(shape.positionAtOffset2D(offset + length1 - MIN2(length1 - 2 * POSITION_EPS, curveCutout)), false);
828 490 : shape2.insertAtClosest(shape.positionAtOffset2D(offset + length1 + MIN2(length2 - 2 * POSITION_EPS, curveCutout)), false);
829 251 : shape2.removeClosest(p1);
830 : }
831 412 : offset += length1;
832 : }
833 145 : 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 145 : 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 953 : for (int j = 0; j < numPoints - 1; ++j) {
853 808 : const Position& p0 = shape2[j];
854 808 : const Position& p1 = shape2[j + 1];
855 808 : PositionVector line;
856 808 : line.push_back(p0);
857 808 : line.push_back(p1);
858 808 : const double lineLength = line.length2D();
859 808 : if (lineLength >= longThresh) {
860 290 : 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 518 : PositionVector begShape;
869 518 : PositionVector endShape;
870 518 : 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 89 : begShape.add(p0 - begShape.back());
874 429 : } else if (j == 1 || p0.distanceTo2D(shape2[j - 1]) > longThresh) {
875 : // use the previous segment if it is long or the first one
876 234 : begShape.push_back(shape2[j - 1]);
877 234 : begShape.push_back(p0);
878 : } else {
879 : // end at p0 with mean angle of the previous and current segment
880 195 : begShape.push_back(shape2[j - 1]);
881 195 : begShape.push_back(p1);
882 195 : begShape.add(p0 - begShape.back());
883 : }
884 :
885 518 : 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 89 : endShape.add(p1 - endShape.front());
889 429 : } 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 234 : endShape.push_back(p1);
892 234 : endShape.push_back(shape2[j + 2]);
893 : } else {
894 : // start at p1 with mean angle of the current and next segment
895 195 : endShape.push_back(p0);
896 195 : endShape.push_back(shape2[j + 2]);
897 195 : endShape.add(p1 - endShape.front());
898 : }
899 518 : const double extrapolateLength = MIN2((double)25, lineLength / 4);
900 518 : PositionVector init = NBNode::bezierControlPoints(begShape, endShape, false, extrapolateLength, extrapolateLength, ok, nullptr, straightThresh);
901 518 : if (init.size() == 0) {
902 : // could not compute control points, write line
903 90 : 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 428 : const double curveLength = init.bezier(12).length2D();
912 428 : 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 518 : }
920 808 : }
921 145 : length = offset;
922 145 : return ok;
923 145 : }
924 :
925 :
926 : void
927 1813 : NWWriter_OpenDrive::writeElevationProfile(const PositionVector& shape, OutputDevice& device, const OutputDevice_String& elevationDevice) {
928 : // check if the shape is flat
929 : bool flat = true;
930 1813 : double z = shape.size() == 0 ? 0 : shape[0].z();
931 4093 : for (int i = 1; i < (int)shape.size(); ++i) {
932 2282 : if (fabs(shape[i].z() - z) > NUMERICAL_EPS) {
933 : flat = false;
934 : break;
935 : }
936 : }
937 1813 : device << " <elevationProfile>\n";
938 1813 : if (flat) {
939 1811 : device << " <elevation s=\"0\" a=\"" << z << "\" b=\"0\" c=\"0\" d=\"0\"/>\n";
940 : } else {
941 4 : device << elevationDevice.getString();
942 : }
943 1813 : device << " </elevationProfile>\n";
944 :
945 1813 : }
946 :
947 :
948 : void
949 627 : NWWriter_OpenDrive::checkLaneGeometries(const NBEdge* e) {
950 627 : if (e->getNumLanes() > 1) {
951 : // compute 'stop line' of rightmost lane
952 130 : const PositionVector shape0 = e->getLaneShape(0);
953 : assert(shape0.size() >= 2);
954 130 : const Position& from = shape0[-2];
955 130 : const Position& to = shape0[-1];
956 130 : PositionVector stopLine;
957 130 : stopLine.push_back(to);
958 228 : stopLine.push_back(to - PositionVector::sideOffset(from, to, lefthand ? 1000 : -1000));
959 : // endpoints of all other lanes should be on the stop line
960 368 : for (int lane = 1; lane < e->getNumLanes(); ++lane) {
961 238 : const double dist = stopLine.distance2D(e->getLaneShape(lane)[-1]);
962 238 : 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 130 : }
967 627 : }
968 :
969 : void
970 627 : NWWriter_OpenDrive::writeRoadObjects(OutputDevice& device, const NBEdge* e, const ShapeContainer& shc, const std::vector<std::string>& crossings) {
971 627 : device.openTag("objects");
972 1254 : if (e->hasParameter(ROAD_OBJECTS)) {
973 52 : device.setPrecision(8); // geometry hdg requires higher precision
974 52 : PositionVector road = getInnerLaneBorder(e);
975 224 : 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 627 : if (crossings.size() > 0) {
991 34 : device.setPrecision(8); // geometry hdg requires higher precision
992 34 : PositionVector road = getInnerLaneBorder(e);
993 39 : 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 627 : device.closeTag();
1002 627 : }
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 9 : 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 3 : std::regex_match(trafficSign, match, re);
1062 3 : if (match.size() == 5) {
1063 : std::string value;
1064 3 : 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 627 : NWWriter_OpenDrive::writeSignals(OutputDevice& device, const NBEdge* e, double length,
1078 : SignalLanes& signalLanes, const ShapeContainer& shc) {
1079 1254 : device.openTag("signals");
1080 627 : 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 58 : NBTrafficLightDefinition* tl = *e->getToNode()->getControllingTLS().begin();
1085 : std::map<std::string, bool> toWrite;
1086 1116 : for (const NBConnection& c : tl->getControlledLinks()) {
1087 1058 : if (c.getFrom() == e) {
1088 426 : const std::string id = tl->getID() + "_" + toString(c.getTLIndex());
1089 : if (toWrite.count(id) == 0) {
1090 201 : toWrite[id] = signalLanes.count(id) == 0;
1091 : }
1092 213 : signalLanes[id].first.insert(c.getFromLane());
1093 213 : signalLanes[id].second.insert(e->getToNode()->getDirection(e, c.getTo()));
1094 : }
1095 : }
1096 259 : for (auto item : toWrite) {
1097 : const std::string id = item.first;
1098 201 : const bool isNew = item.second;
1099 201 : 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 201 : const bool s = dirs.count(LinkDirection::STRAIGHT) != 0;
1103 201 : const std::string tag = isNew ? "signal" : "signalReference";
1104 201 : int firstLane = *signalLanes[id].first.begin();
1105 201 : double t = e->getLaneWidth(firstLane) * 0.5;
1106 321 : for (int i = firstLane + 1; i < e->getNumLanes(); i++) {
1107 120 : t += e->getLaneWidth(i);
1108 : }
1109 201 : device.openTag(tag);
1110 201 : device.writeAttr("id", id);
1111 201 : device.setPrecision(8);
1112 201 : device.writeAttr("s", length);
1113 201 : device.writeAttr("t", -t);
1114 201 : device.writeAttr("orientation", "+");
1115 201 : if (isNew) {
1116 201 : int type = 1000001;
1117 201 : int subType = -1;
1118 201 : if (l && !s && !r) {
1119 43 : type = 1000011;
1120 43 : subType = 10;
1121 158 : } else if (!l && !s && r) {
1122 44 : type = 1000011;
1123 44 : subType = 20;
1124 114 : } else if (!l && s && !r) {
1125 84 : type = 1000011;
1126 84 : subType = 30;
1127 30 : } else if (l && s && !r) {
1128 0 : type = 1000011;
1129 0 : subType = 40;
1130 30 : } else if (!l && s && r) {
1131 4 : type = 1000011;
1132 4 : subType = 50;
1133 : }
1134 201 : device.writeAttr("dynamic", "yes");
1135 201 : device.writeAttr("zOffset", 5);
1136 201 : device.writeAttr("country", "OpenDRIVE");
1137 201 : device.writeAttr("type", type);
1138 201 : device.writeAttr("subtype", subType);
1139 201 : device.writeAttr("height", 0.78);
1140 402 : device.writeAttr("width", 0.26);
1141 : }
1142 406 : for (int lane : signalLanes[id].first) {
1143 410 : device.openTag("validity");
1144 410 : device.writeAttr("fromLane", s2x(lane, e->getNumLanes()));
1145 205 : device.writeAttr("toLane", s2x(lane, e->getNumLanes()));
1146 410 : device.closeTag();
1147 : }
1148 402 : device.closeTag();
1149 : }
1150 1138 : } else if (e->hasParameter(ROAD_OBJECTS)) {
1151 48 : PositionVector roadShape = getInnerLaneBorder(e);
1152 210 : for (std::string id : StringTokenizer(e->getParameter(ROAD_OBJECTS, "")).getVector()) {
1153 : PointOfInterest* poi = shc.getPOIs().get(id);
1154 53 : if (poi != nullptr) {
1155 57 : if (poi->getShapeType() == "traffic_sign" && poi->hasParameter("traffic_sign")) {
1156 8 : std::string traffic_sign_type = poi->getParameter("traffic_sign");
1157 :
1158 4 : std::vector<TrafficSign> trafficSigns = parseTrafficSign(traffic_sign_type, poi);
1159 :
1160 4 : auto distance = roadShape.nearest_offset_to_point2D(*poi, true);
1161 4 : double t = getRoadSideOffset(e);
1162 4 : double calculatedZOffset = 3.0;
1163 10 : for (auto it = trafficSigns.rbegin(); it != trafficSigns.rend(); ++it) {
1164 6 : TrafficSign trafficSign = *it;
1165 6 : device.openTag("signal");
1166 6 : device.writeAttr("id", id);
1167 6 : device.writeAttr("s", distance);
1168 6 : device.writeAttr("t", t);
1169 6 : device.writeAttr("orientation", "-");
1170 6 : device.writeAttr("dynamic", "no");
1171 6 : device.writeAttr("zOffset", calculatedZOffset);
1172 6 : device.writeAttr("country", trafficSign.country);
1173 6 : device.writeAttr("type", trafficSign.type);
1174 6 : device.writeAttr("subtype", trafficSign.subtype);
1175 6 : device.writeAttr("value", trafficSign.value);
1176 6 : device.writeAttr("height", 0.78);
1177 6 : device.writeAttr("width", 0.78);
1178 6 : device.closeTag();
1179 6 : calculatedZOffset += 0.78;
1180 6 : }
1181 4 : }
1182 : }
1183 48 : }
1184 48 : }
1185 627 : device.closeTag();
1186 627 : }
1187 :
1188 :
1189 : double
1190 8 : NWWriter_OpenDrive::getRoadSideOffset(const NBEdge* e) {
1191 : double t = 0.30;
1192 8 : if (!lefthand || LHLL) {
1193 12 : for (int i = 0; i < e->getNumLanes(); i++) {
1194 6 : t += e->getPermissions(i) == SVC_PEDESTRIAN ? e->getLaneWidth(i) * 0.2 : e->getLaneWidth(i);
1195 : }
1196 : }
1197 8 : t = LHRL ? t : -t;
1198 8 : return t;
1199 : }
1200 :
1201 :
1202 : int
1203 5271 : NWWriter_OpenDrive::s2x(int sumoIndex, int numLanes) {
1204 : // sumo lanes: 0, 1, 2 (0 being the outermost lane)
1205 : // XODR: -3,-2,-1 (written in reverse order)
1206 : // LHLL: 3, 2, 1 (written in reverse order)
1207 : // lefthand (old):-1,-2,-3
1208 : return (lefthand
1209 5271 : ? (LHLL
1210 436 : ? numLanes - sumoIndex
1211 : : - sumoIndex - 1)
1212 5271 : : sumoIndex - numLanes);
1213 : }
1214 :
1215 :
1216 : void
1217 31 : NWWriter_OpenDrive::mapmatchRoadObjects(const ShapeContainer& shc, const NBEdgeCont& ec) {
1218 31 : if (shc.getPolygons().size() == 0 && shc.getPOIs().size() == 0) {
1219 26 : return;
1220 : }
1221 5 : const double maxDist = OptionsCont::getOptions().getFloat("opendrive-output.shape-match-dist");
1222 5 : if (maxDist < 0) {
1223 : return;
1224 : }
1225 : // register custom assignements
1226 : std::set<std::string> assigned;
1227 149 : for (auto it = ec.begin(); it != ec.end(); ++it) {
1228 144 : NBEdge* e = it->second;
1229 288 : if (e->hasParameter(ROAD_OBJECTS)) {
1230 0 : for (std::string id : StringTokenizer(e->getParameter(ROAD_OBJECTS, "")).getVector()) {
1231 : assigned.insert(id);
1232 0 : }
1233 : }
1234 : }
1235 : // build rtree for edges
1236 : NamedRTree r;
1237 149 : for (auto it = ec.begin(); it != ec.end(); ++it) {
1238 144 : NBEdge* edge = it->second;
1239 144 : Boundary bound = edge->getGeometry().getBoxBoundary();
1240 144 : bound.grow(maxDist);
1241 144 : float min[2] = { static_cast<float>(bound.xmin()), static_cast<float>(bound.ymin()) };
1242 144 : float max[2] = { static_cast<float>(bound.xmax()), static_cast<float>(bound.ymax()) };
1243 144 : r.Insert(min, max, edge);
1244 144 : }
1245 :
1246 67 : for (auto itPoly = shc.getPolygons().begin(); itPoly != shc.getPolygons().end(); itPoly++) {
1247 62 : SUMOPolygon* p = itPoly->second;
1248 62 : Boundary bound = p->getShape().getBoxBoundary();
1249 62 : float min[2] = { static_cast<float>(bound.xmin()), static_cast<float>(bound.ymin()) };
1250 62 : float max[2] = { static_cast<float>(bound.xmax()), static_cast<float>(bound.ymax()) };
1251 : std::set<const Named*> edges;
1252 : Named::StoringVisitor visitor(edges);
1253 : r.Search(min, max, visitor);
1254 : std::vector<std::pair<double, std::string> > nearby;
1255 2714 : for (const Named* namedEdge : edges) {
1256 2652 : NBEdge* e = const_cast<NBEdge*>(dynamic_cast<const NBEdge*>(namedEdge));
1257 5304 : const double distance = VectorHelper<double>::minValue(p->getShape().distances(e->getLaneShape(0), true));
1258 2652 : if (distance <= maxDist) {
1259 : // sort by distance and ID to stabilize results
1260 3562 : nearby.push_back(std::make_pair(distance, e->getID()));
1261 : //std::cout << " poly=" << p->getID() << " e=" << e->getID() << " dist=" << distance << "\n";
1262 : }
1263 : }
1264 62 : if (nearby.size() > 0) {
1265 62 : std::sort(nearby.begin(), nearby.end());
1266 62 : NBEdge* closest = ec.retrieve(nearby.front().second);
1267 124 : std::string objects = closest->getParameter(ROAD_OBJECTS, "");
1268 62 : if (objects != "") {
1269 : objects += " ";
1270 : }
1271 : objects += p->getID();
1272 124 : closest->setParameter(ROAD_OBJECTS, objects);
1273 : //std::cout << "poly=" << p->getID() << " closest=" << closest->getID() << "\n";
1274 : }
1275 124 : }
1276 63 : for (auto itPoi = shc.getPOIs().begin(); itPoi != shc.getPOIs().end(); itPoi++) {
1277 58 : PointOfInterest* p = itPoi->second;
1278 58 : float min[2] = { static_cast<float>(p->x()), static_cast<float>(p->y()) };
1279 58 : float max[2] = { static_cast<float>(p->x()), static_cast<float>(p->y()) };
1280 : std::set<const Named*> edges;
1281 : Named::StoringVisitor visitor(edges);
1282 : r.Search(min, max, visitor);
1283 : std::vector<std::pair<double, std::string> > nearby;
1284 1774 : for (const Named* namedEdge : edges) {
1285 1716 : NBEdge* e = const_cast<NBEdge*>(dynamic_cast<const NBEdge*>(namedEdge));
1286 1716 : const double distance = e->getLaneShape(0).distance2D(*p, true);
1287 1716 : if (distance != GeomHelper::INVALID_OFFSET && distance <= maxDist) {
1288 : // sort by distance and ID to stabilize results
1289 766 : nearby.push_back(std::make_pair(distance, e->getID()));
1290 : //if (p->getID() == "1275468911") {
1291 : // std::cout << " poly=" << p->getID() << " e=" << e->getID() << " dist=" << distance << "\n";
1292 : //}
1293 : }
1294 : }
1295 58 : if (nearby.size() > 0) {
1296 58 : std::sort(nearby.begin(), nearby.end());
1297 58 : NBEdge* closest = ec.retrieve(nearby.front().second);
1298 116 : std::string objects = closest->getParameter(ROAD_OBJECTS, "");
1299 58 : if (objects != "") {
1300 : objects += " ";
1301 : }
1302 : objects += p->getID();
1303 116 : closest->setParameter(ROAD_OBJECTS, objects);
1304 : //std::cout << "poly=" << p->getID() << " closest=" << closest->getID() << "\n";
1305 : }
1306 58 : }
1307 : }
1308 :
1309 :
1310 : void
1311 58 : NWWriter_OpenDrive::writeRoadObjectPOI(OutputDevice& device, const NBEdge* e, const PositionVector& roadShape, const PointOfInterest* poi) {
1312 58 : Position center = *poi;
1313 58 : const double edgeOffset = roadShape.nearest_offset_to_point2D(center, false);
1314 58 : if (edgeOffset == GeomHelper::INVALID_OFFSET) {
1315 0 : WRITE_WARNINGF("Cannot map road object POI '%' with center % onto edge '%'", poi->getID(), center, e->getID());
1316 : return;
1317 : }
1318 :
1319 58 : Position edgePos = roadShape.positionAtOffset2D(edgeOffset);
1320 58 : double sideOffset = center.distanceTo2D(edgePos);
1321 : // determine sign of sideOffset
1322 174 : PositionVector tmp = roadShape.getSubpart2D(MAX2(0.0, edgeOffset - 1), MIN2(roadShape.length2D(), edgeOffset + 1));
1323 58 : tmp.move2side(sideOffset);
1324 58 : if (tmp.distance2D(center) < sideOffset) {
1325 45 : sideOffset *= -1;
1326 : }
1327 : // place traffic signs on appropriate side of the road
1328 : std::string type = poi->getShapeType();
1329 116 : std::string name = StringUtils::escapeXML(poi->getParameter("name", ""), true);
1330 58 : if (poi->getShapeType() == "traffic_sign") {
1331 4 : sideOffset = getRoadSideOffset(e);
1332 : type = "pole";
1333 : name = "pole";
1334 : }
1335 :
1336 116 : device.openTag("object");
1337 58 : device.writeAttr("id", poi->getID());
1338 58 : device.writeAttr("type", type);
1339 58 : device.writeAttr("name", name);
1340 58 : device.writeAttr("s", edgeOffset);
1341 58 : device.writeAttr("t", sideOffset);
1342 58 : device.writeAttr("hdg", 0);
1343 116 : device.closeTag();
1344 58 : }
1345 :
1346 : void
1347 101 : NWWriter_OpenDrive::writeRoadObjectPoly(OutputDevice& device, const NBEdge* e, const PositionVector& roadShape, const SUMOPolygon* p) {
1348 101 : PositionVector shape = p->getShape();
1349 101 : Position center = shape.getPolygonCenter();
1350 :
1351 101 : const double edgeOffset = roadShape.nearest_offset_to_point2D(center, false);
1352 101 : if (edgeOffset == GeomHelper::INVALID_OFFSET) {
1353 0 : WRITE_WARNINGF("Cannot map road object polygon '%' with center % onto edge '%'", p->getID(), center, e->getID());
1354 : return;
1355 : }
1356 101 : Position edgePos = roadShape.positionAtOffset2D(edgeOffset);
1357 101 : const double edgeAngle = roadShape.rotationAtOffset(edgeOffset);
1358 : double sideOffset = center.distanceTo2D(edgePos);
1359 : // determine sign of sideOffset
1360 269 : PositionVector tmp = roadShape.getSubpart2D(MAX2(0.0, edgeOffset - 1), MIN2(roadShape.length2D(), edgeOffset + 1));
1361 101 : tmp.move2side(sideOffset);
1362 101 : if (tmp.distance2D(center) < sideOffset) {
1363 62 : sideOffset *= -1;
1364 : }
1365 : //std::cout << " id=" << id
1366 : // << " shape=" << shape
1367 : // << " center=" << center
1368 : // << " edgeOffset=" << edgeOffset
1369 : // << "\n";
1370 : auto shapeType = p->getShapeType();
1371 202 : device.openTag("object");
1372 101 : device.writeAttr("id", p->getID());
1373 101 : device.writeAttr("type", shapeType);
1374 202 : if (p->hasParameter("name")) {
1375 36 : device.writeAttr("name", StringUtils::escapeXML(p->getParameter("name", ""), true));
1376 : }
1377 101 : device.writeAttr("s", edgeOffset);
1378 171 : device.writeAttr("t", shapeType == "crosswalk" && !lefthand ? 0 : sideOffset);
1379 101 : double hdg = -edgeAngle;
1380 202 : if (p->hasParameter("hdg")) {
1381 : try {
1382 78 : hdg = StringUtils::toDoubleSecure(p->getParameter("hdg", ""), 0);
1383 0 : } catch (NumberFormatException&) {}
1384 : }
1385 101 : device.writeAttr("hdg", hdg);
1386 202 : if (p->hasParameter("length")) {
1387 : try {
1388 117 : device.writeAttr("length", StringUtils::toDoubleSecure(p->getParameter("length", ""), 0));
1389 0 : } catch (NumberFormatException&) {}
1390 : }
1391 202 : if (p->hasParameter("width")) {
1392 : try {
1393 117 : device.writeAttr("width", StringUtils::toDoubleSecure(p->getParameter("width", ""), 0));
1394 0 : } catch (NumberFormatException&) {}
1395 : }
1396 101 : double height = 0;
1397 202 : if (p->hasParameter("height")) {
1398 : try {
1399 6 : height = StringUtils::toDoubleSecure(p->getParameter("height", ""), 0);
1400 6 : device.writeAttr("height", height);
1401 0 : } catch (NumberFormatException&) {}
1402 : }
1403 : //device.openTag("outlines");
1404 101 : device.openTag("outline");
1405 101 : device.writeAttr("id", 0);
1406 101 : device.writeAttr("fillType", "concrete");
1407 101 : device.writeAttr("outer", "true");
1408 142 : device.writeAttr("closed", p->getShape().isClosed() ? "true" : "false");
1409 101 : device.writeAttr("laneType", "border");
1410 :
1411 101 : shape.sub(center);
1412 101 : if (hdg != -edgeAngle) {
1413 36 : shape.rotate2D(-edgeAngle - hdg);
1414 : }
1415 : int i = 0;
1416 1244 : for (Position pos : shape) {
1417 1143 : device.openTag("cornerLocal");
1418 1143 : device.writeAttr("u", pos.x());
1419 2286 : device.writeAttr("v", pos.y());
1420 1143 : device.writeAttr("z", MAX2(0.0, p->getShapeLayer()));
1421 1143 : device.writeAttr("height", height);
1422 1143 : device.writeAttr("id", i++);
1423 2286 : device.closeTag();
1424 : }
1425 :
1426 :
1427 : //device.closeTag();
1428 101 : device.closeTag();
1429 202 : device.closeTag();
1430 101 : }
1431 :
1432 : /****************************************************************************/
|
