NIImporter_OpenDrive.h

Go to the documentation of this file.

/****************************************************************************/
// Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.dev/sumo
// Copyright (C) 2001-2024 German Aerospace Center (DLR) and others.
// This program and the accompanying materials are made available under the
// terms of the Eclipse Public License 2.0 which is available at
// https://www.eclipse.org/legal/epl-2.0/
// This Source Code may also be made available under the following Secondary
// Licenses when the conditions for such availability set forth in the Eclipse
// Public License 2.0 are satisfied: GNU General Public License, version 2
// or later which is available at
// https://www.gnu.org/licenses/old-licenses/gpl-2.0-standalone.html
// SPDX-License-Identifier: EPL-2.0 OR GPL-2.0-or-later
/****************************************************************************/
// Importer for networks stored in openDrive format
/****************************************************************************/
#pragma once
#include <config.h>
 
#include <string>
#include <map>
#include <utils/xml/GenericSAXHandler.h>
#include <utils/geom/PositionVector.h>
 
 
// ===========================================================================
// class declarations
// ===========================================================================
class NBNetBuilder;
class NBEdge;
class OptionsCont;
class NBNode;
class NBNodeCont;
 
 
#define UNSET_CONNECTION 100000
#define UNSET_LANEVALIDITY 100000
 
// ===========================================================================
// class definitions
// ===========================================================================
class NIImporter_OpenDrive : public GenericSAXHandler {
public:
    static void loadNetwork(const OptionsCont& oc, NBNetBuilder& nb);
 
 
protected:
 
    enum OpenDriveXMLTag {
        OPENDRIVE_TAG_NOTHING,
        OPENDRIVE_TAG_HEADER,
        OPENDRIVE_TAG_ROAD,
        OPENDRIVE_TAG_PREDECESSOR,
        OPENDRIVE_TAG_SUCCESSOR,
        OPENDRIVE_TAG_GEOMETRY,
        OPENDRIVE_TAG_LINE,
        OPENDRIVE_TAG_SPIRAL,
        OPENDRIVE_TAG_ARC,
        OPENDRIVE_TAG_POLY3,
        OPENDRIVE_TAG_PARAMPOLY3,
        OPENDRIVE_TAG_LANESECTION,
        OPENDRIVE_TAG_LANEOFFSET,
        OPENDRIVE_TAG_ACCESS,
        OPENDRIVE_TAG_LEFT,
        OPENDRIVE_TAG_CENTER,
        OPENDRIVE_TAG_RIGHT,
        OPENDRIVE_TAG_LANE,
        OPENDRIVE_TAG_SIGNAL,
        OPENDRIVE_TAG_SIGNALREFERENCE,
        OPENDRIVE_TAG_CONTROLLER,
        OPENDRIVE_TAG_CONTROL,
        OPENDRIVE_TAG_VALIDITY,
        OPENDRIVE_TAG_JUNCTION,
        OPENDRIVE_TAG_CONNECTION,
        OPENDRIVE_TAG_LANELINK,
        OPENDRIVE_TAG_WIDTH,
        OPENDRIVE_TAG_SPEED,
        OPENDRIVE_TAG_ELEVATION,
        OPENDRIVE_TAG_GEOREFERENCE,
        OPENDRIVE_TAG_OFFSET,
        OPENDRIVE_TAG_OBJECT,
        OPENDRIVE_TAG_REPEAT,
        OPENDRIVE_TAG_INCLUDE
    };
 
 
    enum OpenDriveXMLAttr {
        OPENDRIVE_ATTR_NOTHING,
        OPENDRIVE_ATTR_REVMAJOR,
        OPENDRIVE_ATTR_REVMINOR,
        OPENDRIVE_ATTR_ID,
        OPENDRIVE_ATTR_LENGTH,
        OPENDRIVE_ATTR_WIDTH,
        OPENDRIVE_ATTR_RADIUS,
        OPENDRIVE_ATTR_DISTANCE,
        OPENDRIVE_ATTR_TSTART,
        OPENDRIVE_ATTR_TEND,
        OPENDRIVE_ATTR_WIDTHSTART,
        OPENDRIVE_ATTR_WIDTHEND,
        OPENDRIVE_ATTR_JUNCTION,
        OPENDRIVE_ATTR_ELEMENTTYPE,
        OPENDRIVE_ATTR_ELEMENTID,
        OPENDRIVE_ATTR_CONTACTPOINT,
        OPENDRIVE_ATTR_S,
        OPENDRIVE_ATTR_T,
        OPENDRIVE_ATTR_X,
        OPENDRIVE_ATTR_Y,
        OPENDRIVE_ATTR_Z,
        OPENDRIVE_ATTR_HDG,
        OPENDRIVE_ATTR_CURVSTART,
        OPENDRIVE_ATTR_CURVEND,
        OPENDRIVE_ATTR_CURVATURE,
        OPENDRIVE_ATTR_A,
        OPENDRIVE_ATTR_B,
        OPENDRIVE_ATTR_C,
        OPENDRIVE_ATTR_D,
        OPENDRIVE_ATTR_AU,
        OPENDRIVE_ATTR_BU,
        OPENDRIVE_ATTR_CU,
        OPENDRIVE_ATTR_DU,
        OPENDRIVE_ATTR_AV,
        OPENDRIVE_ATTR_BV,
        OPENDRIVE_ATTR_CV,
        OPENDRIVE_ATTR_DV,
        OPENDRIVE_ATTR_PRANGE,
        OPENDRIVE_ATTR_TYPE,
        OPENDRIVE_ATTR_LEVEL,
        OPENDRIVE_ATTR_ORIENTATION,
        OPENDRIVE_ATTR_DYNAMIC,
        OPENDRIVE_ATTR_INCOMINGROAD,
        OPENDRIVE_ATTR_CONNECTINGROAD,
        OPENDRIVE_ATTR_FROM,
        OPENDRIVE_ATTR_TO,
        OPENDRIVE_ATTR_FROMLANE,
        OPENDRIVE_ATTR_TOLANE,
        OPENDRIVE_ATTR_MAX,
        OPENDRIVE_ATTR_SOFFSET,
        OPENDRIVE_ATTR_RULE,
        OPENDRIVE_ATTR_RESTRICTION,
        OPENDRIVE_ATTR_NAME,
        OPENDRIVE_ATTR_UNIT,    // xodr v1.4
        OPENDRIVE_ATTR_SIGNALID,
        OPENDRIVE_ATTR_FILE
    };
 
 
    enum LinkType {
        OPENDRIVE_LT_SUCCESSOR,
        OPENDRIVE_LT_PREDECESSOR
    };
 
 
    enum ElementType {
        OPENDRIVE_ET_UNKNOWN,
        OPENDRIVE_ET_ROAD,
        OPENDRIVE_ET_JUNCTION
    };
 
 
    enum ContactPoint {
        OPENDRIVE_CP_UNKNOWN,
        OPENDRIVE_CP_START,
        OPENDRIVE_CP_END
    };
 
    enum GeometryType {
        OPENDRIVE_GT_UNKNOWN,
        OPENDRIVE_GT_LINE,
        OPENDRIVE_GT_SPIRAL,
        OPENDRIVE_GT_ARC,
        OPENDRIVE_GT_POLY3,
        OPENDRIVE_GT_PARAMPOLY3 // rev 1.4
    };
 
 
 
    struct OpenDriveLink {
        OpenDriveLink(LinkType linkTypeArg, const std::string& elementIDArg)
            : linkType(linkTypeArg), elementID(elementIDArg),
              elementType(OPENDRIVE_ET_UNKNOWN), contactPoint(OPENDRIVE_CP_UNKNOWN) { }
 
        LinkType linkType;
        std::string elementID;
        ElementType elementType;
        ContactPoint contactPoint;
    };
 
 
    struct OpenDriveGeometry {
        OpenDriveGeometry(double lengthArg, double sArg, double xArg, double yArg, double hdgArg)
            : length(lengthArg), s(sArg), x(xArg), y(yArg), hdg(hdgArg),
              type(OPENDRIVE_GT_UNKNOWN) { }
 
        double length;
        double s;
        double x;
        double y;
        double hdg;
        GeometryType type;
        std::vector<double> params;
    };
 
    struct Poly3 {
        Poly3(double _s, double _a, double _b, double _c, double _d) :
            s(_s), a(_a), b(_b), c(_c), d(_d) {}
 
        double computeAt(double pos) const {
            const double ds = pos - s;
            return a + b * ds + c * ds * ds + d * ds * ds * ds;
        }
 
        double s;
        double a;
        double b;
        double c;
        double d;
    };
 
    typedef Poly3 OpenDriveElevation;
    typedef Poly3 OpenDriveLaneOffset;
    typedef Poly3 OpenDriveWidth;
 
    struct LaneAttributeChange {
        LaneAttributeChange(double speed) : speed(speed) {}
 
        double speed;
        std::vector<std::string> allowed;
        std::vector<std::string> denied;
    };
 
 
 
    struct OpenDriveLane {
        OpenDriveLane(int idArg, const std::string& levelArg, const std::string& typeArg) :
            id(idArg), level(levelArg), type(typeArg), successor(UNSET_CONNECTION), predecessor(UNSET_CONNECTION),
            speed(0), width(NBEdge::UNSPECIFIED_WIDTH), permission(0) { }
 
        SVCPermissions computePermission(const NBTypeCont& tc, const std::vector<std::string>& allowed, const std::vector<std::string>& denied) const;
 
        int id; 
        std::string level; 
        std::string type; 
        int successor; 
        int predecessor; 
        std::vector < std::pair<double, LaneAttributeChange> > attributeChanges; 
        double speed; 
        double width; //The lane's maximum width
        SVCPermissions permission; 
        std::vector<OpenDriveWidth> widthData;
    };
 
 
    struct OpenDriveLaneSection {
        OpenDriveLaneSection(double sArg);
 
 
        void buildLaneMapping(const NBTypeCont& tc);
 
 
        std::map<int, int> getInnerConnections(OpenDriveXMLTag dir, const OpenDriveLaneSection& prev);
 
 
        bool buildAttributeChanges(const NBTypeCont& tc, std::vector<OpenDriveLaneSection>& newSections);
        OpenDriveLaneSection buildLaneSection(const NBTypeCont& tc, double startPos);
        double s;
        double sOrig;
        double length;
        std::map<int, int> laneMap;
        std::map<OpenDriveXMLTag, std::vector<OpenDriveLane> > lanesByDir;
        std::string sumoID;
        int rightLaneNumber, leftLaneNumber;
        std::string rightType;
        std::string leftType;
        double discardedInnerWidthLeft;
        double discardedInnerWidthRight;
    };
 
 
 
    struct OpenDriveSignal {
        OpenDriveSignal(const std::string& idArg, const std::string typeArg, const std::string nameArg,
                        int orientationArg, bool dynamicArg, double sArg) :
            id(idArg), type(typeArg), name(nameArg),
            orientation(orientationArg), dynamic(dynamicArg), s(sArg),
            minLane(-UNSET_LANEVALIDITY), maxLane(UNSET_LANEVALIDITY)
        { }
 
        OpenDriveSignal() {}
 
        std::string id;
        std::string type;
        std::string name;
        int orientation;
        bool dynamic;
        double s;
        int minLane;
        int maxLane;
        std::string controller;
    };
 
 
    struct OpenDriveController {
        OpenDriveController(const std::string& idArg, const std::string nameArg) :
            id(idArg), name(nameArg) { }
 
        OpenDriveController() {}
 
        std::string id;
        std::string name;
        std::vector<std::string> signalIDs;
        std::string junction;
    };
 
    struct Connection {
        std::string fromEdge;
        std::string toEdge;
        int fromLane;
        int toLane;
        ContactPoint fromCP;
        ContactPoint toCP;
        bool all;
        std::string origID;
        int origLane;
        PositionVector shape;
 
        std::string getDescription() const {
            return "Connection from=" + fromEdge + "_" + toString(fromLane)
                   + " to=" + toEdge + "_" + toString(toLane)
                   + " fromCP=" + (fromCP == OPENDRIVE_CP_START ? "start" : fromCP == OPENDRIVE_CP_END ? "end" : "unknown")
                   + " toCP=" + (toCP == OPENDRIVE_CP_START ? "start" : toCP == OPENDRIVE_CP_END ? "end" : "unknown")
                   + " all=" + toString(all);
            //+ " origID=" + origID + " origLane=" + toString(origLane);
        }
    };
 
    struct OpenDriveObject {
        std::string type;
        std::string name;
        std::string id;
        double s;
        double t;
        double zOffset;
        double length;
        double width;
        double height;
        double radius;
        double hdg;
        double pitch;
        double roll;
    };
 
    struct OpenDriveEdge {
        OpenDriveEdge(const std::string& idArg, const std::string& streetNameArg, const std::string& junctionArg, double lengthArg) :
            id(idArg),
            streetName(streetNameArg),
            junction(junctionArg),
            length(lengthArg),
            from(0),
            to(0) {
            isInner = junction != "" && junction != "-1";
        }
 
 
        int getPriority(OpenDriveXMLTag dir) const;
 
 
        std::string id;
        std::string streetName;
        std::string junction;
        double length;
        std::vector<OpenDriveLink> links;
        std::vector<OpenDriveGeometry> geometries;
        std::vector<OpenDriveElevation> elevations;
        std::vector<OpenDriveLaneOffset> offsets;
        NBNode* from;
        NBNode* to;
        PositionVector geom;
        std::vector<double> laneOffsets;
        std::vector<OpenDriveLaneSection> laneSections;
        std::vector<OpenDriveSignal> signals;
        std::set<Connection> connections;
        std::vector<OpenDriveObject> objects;
        bool isInner;
    };
 
 
    class sections_by_s_sorter {
    public:
        explicit sections_by_s_sorter() { }
 
        int operator()(const OpenDriveLaneSection& ls1, const OpenDriveLaneSection& ls2) {
            return ls1.s < ls2.s;
        }
    };
 
    /* @brief A class for search in position/attribute change tuple vectors for the given position */
    class same_position_finder {
    public:
        explicit same_position_finder(double pos) : myPosition(pos) { }
 
        bool operator()(const std::pair<double, LaneAttributeChange>& ps) {
            return ps.first == myPosition;
        }
 
    private:
        double myPosition;
 
    };
 
protected:
    NIImporter_OpenDrive(const NBTypeCont& tc, std::map<std::string, OpenDriveEdge*>& edges);
 
 
    ~NIImporter_OpenDrive();
 
 
 
 
 
    void myStartElement(int element, const SUMOSAXAttributes& attrs);
 
    void myCharacters(int element, const std::string& chars);
 
 
    void myEndElement(int element);
 
    std::map<std::string, OpenDriveSignal>& getSignals() {
        return mySignals;
    }
 
    std::map<std::string, OpenDriveController>& getControllers() {
        return myControllers;
    }
 
    std::map<std::string, std::vector<std::string>>& getJunctions2Controllers() {
        return myJunctions2Controllers;
    }
 
    const OpenDriveController& getController(std::string signalID) {
        if (mySignals.find(signalID) != mySignals.end() && myControllers.find(mySignals[signalID].controller) != myControllers.end()) {
            return myControllers[mySignals[signalID].controller];
        }
        return myDummyController;
    }
 
    int getTLIndexForController(std::string controllerID) {
        // sort them by their id
        std::string junctionID = myControllers[controllerID].junction;
        std::vector<std::string> junctionControllers;
        for (auto& it : myControllers) {
            if (it.second.junction == junctionID) {
                junctionControllers.push_back(it.first);
            }
        }
        std::sort(junctionControllers.begin(), junctionControllers.end());
        auto it = std::find(junctionControllers.begin(), junctionControllers.end(), controllerID);
        return (int)(it - junctionControllers.begin());
    }
 
 
private:
    void addLink(LinkType lt, const std::string& elementType, const std::string& elementID,
                 const std::string& contactPoint);
    void addGeometryShape(GeometryType type, const std::vector<double>& vals);
    static void setEdgeLinks2(OpenDriveEdge& e, const std::map<std::string, OpenDriveEdge*>& edges);
    static void buildConnectionsToOuter(const Connection& c,
                                        const std::map<std::string, OpenDriveEdge*>& innerEdges,
                                        const std::map<std::string, OpenDriveEdge*>& edges,
                                        const NBTypeCont& tc,
                                        std::vector<Connection>& into, std::set<Connection>& seen);
    static bool laneSectionsConnected(OpenDriveEdge* edge, int in, int out);
    friend bool operator<(const Connection& c1, const Connection& c2);
    static std::string revertID(const std::string& id);
    const NBTypeCont& myTypeContainer;
    OpenDriveEdge myCurrentEdge;
    OpenDriveController myCurrentController;
 
    std::map<std::string, OpenDriveEdge*>& myEdges;
    std::vector<int> myElementStack;
    OpenDriveXMLTag myCurrentLaneDirection;
    std::string myCurrentJunctionID;
    std::string myCurrentIncomingRoad;
    std::string myCurrentConnectingRoad;
    ContactPoint myCurrentContactPoint;
    bool myConnectionWasEmpty;
    std::map<std::string, OpenDriveSignal> mySignals;
    std::map<std::string, OpenDriveController> myControllers;
    std::map<std::string, std::vector<std::string>> myJunctions2Controllers;
    Position myOffset;
    bool myUseCurrentNode;
 
    static bool myImportAllTypes;
    static bool myImportWidths;
    static double myMinWidth;
    static bool myImportInternalShapes;
    static bool myIgnoreMisplacedSignals;
    static OpenDriveController myDummyController;
 
 
protected:
    static NBNode* getOrBuildNode(const std::string& id, const Position& pos, NBNodeCont& nc);
 
 
    static PositionVector geomFromLine(const OpenDriveEdge& e, const OpenDriveGeometry& g, double resolution);
    static PositionVector geomFromSpiral(const OpenDriveEdge& e, const OpenDriveGeometry& g, double resolution);
    static PositionVector geomFromArc(const OpenDriveEdge& e, const OpenDriveGeometry& g, double resolution);
    static PositionVector geomFromPoly(const OpenDriveEdge& e, const OpenDriveGeometry& g, double resolution);
    static PositionVector geomFromParamPoly(const OpenDriveEdge& e, const OpenDriveGeometry& g, double resolution);
    static Position calculateStraightEndPoint(double hdg, double length, const Position& start);
    static void calculateCurveCenter(double* ad_x, double* ad_y, double ad_radius, double ad_hdg);
    static void calcPointOnCurve(double* ad_x, double* ad_y, double ad_centerX, double ad_centerY,
                                 double ad_r, double ad_length);
 
 
    static void computeShapes(std::map<std::string, OpenDriveEdge*>& edges);
 
    static bool hasNonLinearElevation(const OpenDriveEdge& e);
 
    static std::vector<double> discretizeOffsets(PositionVector& geom, const std::vector<OpenDriveLaneOffset>& offsets, const std::string& id);
 
    static void addOffsets(bool left, PositionVector& geom, const std::vector<OpenDriveLaneOffset>& offsets, const std::string& id, std::vector<double>& result);
 
    static void revisitLaneSections(const NBTypeCont& tc, std::map<std::string, OpenDriveEdge*>& edges);
 
    static void setNodeSecure(NBNodeCont& nc, OpenDriveEdge& e,
                              const std::string& nodeID, NIImporter_OpenDrive::LinkType lt, std::vector<NodeSet>& joinedNodeIDs);
 
    static NBTrafficLightDefinition* getTLSSecure(NBEdge* inEdge, /*const NBEdge::Connection& conn,*/ NBNetBuilder& nb);
 
 
    static std::pair<NBEdge*, NBEdge*> retrieveSignalEdges(NBNetBuilder& nb, const std::string& fromID, const std::string& toID, const std::string& junction);
 
    static void splitMinWidths(OpenDriveEdge* e, const NBTypeCont& tc, double minDist);
 
    static void findWidthSplit(const NBTypeCont& tc, std::vector<OpenDriveLane>& lanes,
                               int section, double sectionStart, double sectionEnd,
                               std::vector<double>& splitPositions);
 
    static void sanitizeWidths(OpenDriveEdge* e);
    static void sanitizeWidths(std::vector<OpenDriveLane>& lanes, double length);
 
    static void setStraightConnections(std::vector<OpenDriveLane>& lanes);
    static void recomputeWidths(OpenDriveLaneSection& sec, double start, double end, double sectionStart, double sectionEnd);
    static void recomputeWidths(std::vector<OpenDriveLane>& lanes, double start, double end, double sectionStart, double sectionEnd);
    static void setLaneAttributes(const OpenDriveEdge* e, NBEdge::Lane& sumoLane, const OpenDriveLane& odLane, bool saveOrigIDs, const NBTypeCont& tc);
    static void writeRoadObjects(const OpenDriveEdge* e);
 
    static SequentialStringBijection::Entry openDriveTags[];
 
    static SequentialStringBijection::Entry openDriveAttrs[];
 
    class LaneSorter {
    public:
        explicit LaneSorter() {}
 
        int operator()(const OpenDriveLane& a, const OpenDriveLane& b) const {
            // sort from the reference line outwards (desceding order of sumo lane ids)
            return abs(a.id) < abs(b.id);
        }
 
    };
 
};