NIImporter_OpenDrive.h

Go to the documentation of this file.

/****************************************************************************/
// Importer for networks stored in openDrive format
/****************************************************************************/
// SUMO, Simulation of Urban MObility; see http://sumo.dlr.de/
// Copyright (C) 2001-2017 DLR (http://www.dlr.de/) and contributors
/****************************************************************************/
//
//   This file is part of SUMO.
//   SUMO is free software: you can redistribute it and/or modify
//   it under the terms of the GNU General Public License as published by
//   the Free Software Foundation, either version 3 of the License, or
//   (at your option) any later version.
//
/****************************************************************************/
#ifndef NIImporter_OpenDrive_h
#define NIImporter_OpenDrive_h


// ===========================================================================
// included modules
// ===========================================================================
#ifdef _MSC_VER
#include <windows_config.h>
#else
#include <config.h>
#endif

#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

// ===========================================================================
// 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_LEFT,
        OPENDRIVE_TAG_CENTER,
        OPENDRIVE_TAG_RIGHT,
        OPENDRIVE_TAG_LANE,
        OPENDRIVE_TAG_SIGNAL,
        OPENDRIVE_TAG_JUNCTION,
        OPENDRIVE_TAG_CONNECTION,
        OPENDRIVE_TAG_LANELINK,
        OPENDRIVE_TAG_WIDTH,
        OPENDRIVE_TAG_SPEED,
        OPENDRIVE_TAG_ELEVATION
    };


    enum OpenDriveXMLAttr {
        OPENDRIVE_ATTR_NOTHING,
        OPENDRIVE_ATTR_REVMAJOR,
        OPENDRIVE_ATTR_REVMINOR,
        OPENDRIVE_ATTR_ID,
        OPENDRIVE_ATTR_LENGTH,
        OPENDRIVE_ATTR_JUNCTION,
        OPENDRIVE_ATTR_ELEMENTTYPE,
        OPENDRIVE_ATTR_ELEMENTID,
        OPENDRIVE_ATTR_CONTACTPOINT,
        OPENDRIVE_ATTR_S,
        OPENDRIVE_ATTR_X,
        OPENDRIVE_ATTR_Y,
        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_MAX,
        OPENDRIVE_ATTR_SOFFSET,
        OPENDRIVE_ATTR_NAME,
        OPENDRIVE_ATTR_UNIT    // xodr v1.4
    };


    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 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) { }

        int id; 
        std::string level; 
        std::string type; 
        int successor; 
        int predecessor; 
        std::vector<std::pair<double, double> > speeds; 
        double speed; 
        double width; //The lane's maximum width
        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 buildSpeedChanges(const NBTypeCont& tc, std::vector<OpenDriveLaneSection>& newSections);
        OpenDriveLaneSection buildLaneSection(double startPos);

        double s;
        double sOrig;
        std::map<int, int> laneMap;
        std::map<OpenDriveXMLTag, std::vector<OpenDriveLane> > lanesByDir;
        std::string sumoID;
        int rightLaneNumber, leftLaneNumber;
        std::string rightType;
        std::string leftType;
    };



    struct OpenDriveSignal {
        OpenDriveSignal(int 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) { }

        int id;
        std::string type;
        std::string name;
        int orientation;
        bool dynamic;
        double s;
    };


    struct Connection {
        std::string fromEdge;
        std::string toEdge;
        int fromLane;
        int toLane;
        ContactPoint fromCP;
        ContactPoint toCP;
        bool all;
        std::string origID;
        int origLane;
    };


    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<OpenDriveLaneSection> laneSections;
        std::vector<OpenDriveSignal> signals;
        std::set<Connection> connections;
        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/speed 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, double>& ps) {
            return ps.first == myPosition;
        }

    private:
        same_position_finder& operator=(const same_position_finder&); // just to avoid a compiler warning
    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 myEndElement(int element);



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, std::vector<Connection>& into, std::set<Connection>& seen);
    friend bool operator<(const Connection& c1, const Connection& c2);
    static std::string revertID(const std::string& id);
    const NBTypeCont& myTypeContainer;
    OpenDriveEdge myCurrentEdge;

    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;

    static bool myImportAllTypes;
    static bool myImportWidths;
    static double myMinWidth;


protected:
    static NBNode* getOrBuildNode(const std::string& id, const Position& pos, NBNodeCont& nc);


    static PositionVector geomFromLine(const OpenDriveEdge& e, const OpenDriveGeometry& g);
    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 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);


    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 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 StringBijection<int>::Entry openDriveTags[];

    static StringBijection<int>::Entry openDriveAttrs[];



};


#endif

/****************************************************************************/