change OpenDrive. Calculate x,y,hdg by GetChord.

src/common/common/xodr/OpenDrive/Lane.cpp

@@ -428,7 +428,23 @@ LaneSection::~LaneSection()
 	mLaneVector.clear();
 }
 
+/**
+ * @brief LaneSection::SetSingleSide
+ * @param singleSide
+ */
+void LaneSection::SetSingleSide(std::string singleSide)
+{
+    msingleSide = singleSide;
+}
 
+/**
+ * @brief LaneSection::GetSingleSide
+ * @return singleSide
+ */
+string LaneSection::GetSingleSide()
+{
+    return msingleSide;
+}
 /**
 * Lane Section Sample. Holds all the lane information at a certain S value including lane widths, levels, 
 * heights, etc
@@ -615,6 +631,28 @@ void Lane::RemoveSuccessor()
 {	mSuccessor=0;	mSuccessorExists=false;		}
 
 
+/**
+ * @brief Lane::AddBorderRecord  Add Border Record
+ * @param s
+ * @param a
+ * @param b
+ * @param c
+ * @param d
+ * @return
+ */
+unsigned int Lane::AddBorderRecord(double s, double a, double b, double c, double d)
+{
+    // Gets the index where the record should be inserted in the vector
+    unsigned int index = CheckBorderInterval(s)+1;
+    // If larger than the record count - push to the back
+    if(index>=GetLaneBorderCount()) mLaneBorder.push_back(LaneBorder(s,a,b,c,d));
+    // else insert in the middle
+    else mLaneBorder.insert(mLaneBorder.begin()+index, LaneBorder(s,a,b,c,d));
+    // Save the last added record index
+    mLastAddedLaneBorder=index;
+    return index;
+}
+
 /**
  * Methods used to add child records to the respective vectors
  */
@@ -697,6 +735,24 @@ unsigned int Lane::AddHeightRecord(double sOffset, double inner, double outer)
 	return index;
 }
 
+/**
+ * @brief Lane::CloneLaneBorder
+ * @param index
+ * @return
+ */
+unsigned int Lane::CloneLaneBorder(unsigned int index)
+{
+    // Clone the object and insert it in the middle of the vector
+    if(index<mLaneBorder.size()-1)
+        mLaneBorder.insert(mLaneBorder.begin()+index+1, mLaneBorder[index]);
+    // or just push it to the back
+    else if(index==mLaneBorder.size()-1)
+        mLaneBorder.push_back(mLaneBorder[index]);
+    // Save the last added record index
+    mLastAddedLaneBorder=index+1;
+    return mLastAddedLaneBorder;
+}
+
 /**
  * Methods used to clone child records in the respective vectors
  */
@@ -779,9 +835,16 @@ unsigned int Lane::CloneLaneHeight(unsigned int index)
 	return mLastAddedLaneHeight;
 }
 
+
+
 /**
  * Methods used to delete child records from the respective vectors
  */
+
+void Lane::DeleteLaneBoder(unsigned int index)
+{
+    mLaneBorder.erase(mLaneBorder.begin()+index);
+}
 void Lane::DeleteLaneWidth(unsigned int index)
 {
 	mLaneWidth.erase(mLaneWidth.begin()+index);
@@ -856,6 +919,9 @@ int Lane::GetSuccessor()
 /**
 *	Get pointers to the records vectors
 */
+vector <LaneBorder> *Lane::GetLaneBorderVector()
+{	return &mLaneBorder;	}
+
 vector <LaneWidth> *Lane::GetLaneWidthVector()
 {	return &mLaneWidth;	}
 
@@ -881,6 +947,9 @@ vector <LaneHeight> *Lane::GetLaneHeightVector()
 /**
 *	Get the number of elements in a certain vector
 */
+unsigned int Lane::GetLaneBorderCount()
+{	return mLaneBorder.size();	}
+
 unsigned int Lane::GetLaneWidthCount()
 {	return mLaneWidth.size();	}
 
@@ -905,6 +974,14 @@ unsigned int Lane::GetLaneHeightCount()
 /**
 *	Get the elements of a certain vectors at position i
 */
+LaneBorder* Lane::GetLaneBorder(unsigned int i)
+{
+    if ((mLaneBorder.size()>0)&&(i<mLaneBorder.size()))
+        return &mLaneBorder.at(i);
+    else
+        return NULL;
+}
+
 LaneWidth* Lane::GetLaneWidth(unsigned int i)
 {
 	if ((mLaneWidth.size()>0)&&(i<mLaneWidth.size()))
@@ -965,6 +1042,13 @@ LaneHeight* Lane::GetLaneHeight(unsigned int i)
 /**
 *	Get the last elements of a certain vectors
 */
+LaneBorder* Lane::GetLastLaneBorder()
+{
+    if (mLaneBorder.size()>0)
+        return &mLaneBorder.at(mLaneBorder.size()-1);
+    else
+        return NULL;
+}
 LaneWidth* Lane::GetLastLaneWidth()
 {
 	if (mLaneWidth.size()>0)
@@ -1018,6 +1102,13 @@ LaneHeight* Lane::GetLastLaneHeight()
 /**
 *	Get the last added elements of a certain vectors (their position might not be at the end of the vector)
 */
+LaneBorder* Lane::GetLastAddedLaneBorder()
+{
+    if(mLastAddedLaneBorder<mLaneBorder.size())
+        return &mLaneBorder.at(mLastAddedLaneBorder);
+    else
+        return NULL;
+}
 LaneWidth* Lane::GetLastAddedLaneWidth()
 {
 	if(mLastAddedLaneWidth<mLaneWidth.size())
@@ -1071,6 +1162,21 @@ LaneHeight* Lane::GetLastAddedLaneHeight()
 /**
 *	Check the intervals and return the index of the records that applies to the provided s-offset
 */
+int  Lane::CheckBorderInterval(double s_check)
+{
+
+    int res=-1;
+    //Go through all the width records
+    for (unsigned int i=0;i<mLaneBorder.size();i++)
+    {
+        //check if the s_check belongs to the current record
+        if (s_check >= mLaneBorder.at(i).GetS())
+            res=i;	//assign it to the result id
+        else
+            break;	//if not, break;
+    }
+    return res;		//return the result: 0 to MaxInt as the index to the record containing s_check or -1 if nothing found
+}
 int  Lane::CheckWidthInterval(double s_check)
 {
 
@@ -1174,6 +1280,18 @@ int Lane::CheckHeightInterval(double s_check)
 /**
 *	Evaluate the record and return the width value
 */
+
+double  Lane::GetBorderValue(double s_check)
+{
+    double retVal=0;
+    //find the record where s_check belongs
+    int index=CheckBorderInterval(s_check);
+    //If found, return the type
+    if (index>=0)
+        retVal= mLaneBorder.at(index).GetValue(s_check);
+    return retVal;
+}
+
 double  Lane::GetWidthValue(double s_check)
 {
 	double retVal=0;
@@ -1203,6 +1321,16 @@ LaneHeight  Lane::GetHeightValue(double s_check)
 }
 
 
+void Lane::SetuserData(std::string struserData)
+{
+    muserData = struserData;
+}
+
+void Lane::GetuserData(std::string &struserData)
+{
+    struserData = muserData;
+}
+
 /**
 *	Evaluate the road marks records and return the road mark object corresponding to the provided s-offset
 */
@@ -1252,8 +1380,23 @@ Lane::~Lane()
 
 	// DELETING LANE HEIGHTS
 	mLaneHeight.clear();
+
+    //DELETE LANE BORDERS
+    mLaneBorder.clear();
 }
 
+
+/**
+ * @brief LaneWidth::LaneWidth
+ * @param s
+ * @param a
+ * @param b
+ * @param c
+ * @param d
+ */
+LaneBorder::LaneBorder(double s, double a, double b, double c, double d):ThirdOrderPolynom(s,a,b,c,d)
+{}
+
 /**
 * Lane width class. Holds all the data that describes a lane width
 *
@@ -1506,4 +1649,63 @@ void LaneHeight::SetS(double value)
 void LaneHeight::SetInner(double value)
 {	mInner=value;	}
 void LaneHeight::SetOuter(double value)
-{	mOuter=value;	}
+{	mOuter=value;	}
+
+/**
+* Lane offset class. Contains all the data that describes lane offset record
+*
+*
+*
+*
+*
+*/
+
+/*
+* Constructors
+*/
+LaneOffset::LaneOffset()
+{    ms = 0; ma=0; mb=0; mc=0; md=0;}
+
+LaneOffset::LaneOffset(double sOffset, double a, double b, double c, double d)
+{   ms = sOffset; ma = a; mb = b; mc = c; md = d;}
+
+/*
+* Methods that return the parameters of the lane height
+*/
+double LaneOffset::GetS()
+{   return ms;}
+double LaneOffset::Geta()
+{   return ma;}
+double LaneOffset::Getb()
+{   return mb;}
+double LaneOffset::Getc()
+{   return mc;}
+double LaneOffset::Getd()
+{   return md;}
+
+/**
+* Check if the tested s-offset is inside the lane offset interval
+* @param A double s-offset value that has to be checked
+* @return Return true if the s-offset value belongs to current lane section, false otherwise
+*/
+bool LaneOffset::CheckInterval(double s_check)
+{
+    if (s_check>=ms)
+        return true;
+    else
+        return false;
+}
+
+/*
+* Methods that set the parameters of the lane offset
+*/
+void LaneOffset::SetS(double value)
+{   ms = value;}
+void LaneOffset::Seta(double value)
+{   ma = value;}
+void LaneOffset::Setb(double value)
+{   mb = value;}
+void LaneOffset::Setc(double value)
+{   mc = value;}
+void LaneOffset::Setd(double value)
+{   md = value;}

src/common/common/xodr/OpenDrive/Lane.h

@@ -18,6 +18,7 @@ class LaneVisibility;
 class LaneSpeed;
 class LaneAccess;
 class LaneHeight;
+class LaneBorder;
 
 using std::vector;
 using std::string;
@@ -36,6 +37,7 @@ private:
 	* Record parameters
 	*/
 	double mS;
+    string msingleSide;
 	vector<Lane> mLaneVector;
 
 	unsigned int mLastAddedLane;
@@ -177,6 +179,9 @@ public:
 	* Destructor. Delete all the members of the vectors: mLeft, mCenter, mRight
 	*/
 	~LaneSection();
+
+    void SetSingleSide(string singleSide);
+    string GetSingleSide();
 };
 
 
@@ -315,6 +320,8 @@ private:
 	vector<LaneAccess> mLaneAccess;
 	//Lane Height
 	vector<LaneHeight> mLaneHeight;
+    //Lane Border
+    vector<LaneBorder> mLaneBorder;
 
 
 	unsigned int mLastAddedLaneWidth;
@@ -324,6 +331,9 @@ private:
 	unsigned int mLastAddedLaneSpeed;
 	unsigned int mLastAddedLaneAccess;
 	unsigned int mLastAddedLaneHeight;
+    unsigned int mLastAddedLaneBorder;
+
+    string muserData;
 
 public:
 	/**
@@ -362,6 +372,7 @@ public:
 	unsigned int AddSpeedRecord(double sOffset, double max);
 	unsigned int AddAccessRecord(double sOffset, string restriction);
 	unsigned int AddHeightRecord(double sOffset, double inner, double outer);
+    unsigned int AddBorderRecord(double s, double a, double b, double c, double d);
 
 	/**
 	 * Methods used to clone child records in the respective vectors
@@ -373,6 +384,7 @@ public:
 	unsigned int CloneLaneSpeed(unsigned int index);
 	unsigned int CloneLaneAccess(unsigned int index);
 	unsigned int CloneLaneHeight(unsigned int index);
+    unsigned int CloneLaneBorder(unsigned int index);
 
 	/**
 	 * Methods used to delete child records from the respective vectors
@@ -384,6 +396,7 @@ public:
 	void DeleteLaneSpeed(unsigned int index);
 	void DeleteLaneAccess(unsigned int index);
 	void DeleteLaneHeight(unsigned int index);
+    void DeleteLaneBoder(unsigned int index);
 
 
 	/**
@@ -412,6 +425,7 @@ public:
 	vector <LaneSpeed> *GetLaneSpeedVector();
 	vector <LaneAccess> *GetLaneAccessVector();
 	vector <LaneHeight> *GetLaneHeightVector();
+    vector <LaneBorder> *GetLaneBorderVector();
 
 
 	/**
@@ -424,6 +438,7 @@ public:
 	unsigned int GetLaneSpeedCount();
 	unsigned int GetLaneAccessCount();
 	unsigned int GetLaneHeightCount();
+    unsigned int GetLaneBorderCount();
 
 
 	/**
@@ -436,6 +451,7 @@ public:
 	LaneSpeed* GetLaneSpeed(unsigned int i);
 	LaneAccess* GetLaneAccess(unsigned int i);
 	LaneHeight* GetLaneHeight(unsigned int i);
+    LaneBorder* GetLaneBorder(unsigned int i);
 
 
 	/**
@@ -448,6 +464,7 @@ public:
 	LaneSpeed* GetLastLaneSpeed();
 	LaneAccess* GetLastLaneAccess();
 	LaneHeight* GetLastLaneHeight();
+    LaneBorder* GetLastLaneBorder();
 
 	/**
 	*	Get the last added elements of a certain vectors (their position might not be at the end of the vector)
@@ -459,6 +476,7 @@ public:
 	LaneSpeed* GetLastAddedLaneSpeed();
 	LaneAccess* GetLastAddedLaneAccess();
 	LaneHeight* GetLastAddedLaneHeight();
+    LaneBorder* GetLastAddedLaneBorder();
 
 	/**
 	*	Check the intervals and return the index of the records that applies to the provided s-offset
@@ -470,8 +488,14 @@ public:
 	int CheckSpeedInterval(double s_check);
 	int CheckAccessInterval(double s_check);
 	int CheckHeightInterval(double s_check);
+    int CheckBorderInterval(double s_check);
+
+
+    void GetuserData(std::string & struserData);
+    void SetuserData(std::string struserData);
 
 
+    double GetBorderValue(double s_check);
 	/**
 	*	Evaluate the record and return the width value
 	*/
@@ -498,6 +522,22 @@ public:
 
 };
 
+
+/**
+ * @brief The LaneBorder class Holds all the data of border
+ */
+class LaneBorder : public ThirdOrderPolynom
+{
+public:
+    /**
+    * Constructor
+    * @param s s-offset of the record starting from the lane section s-offset
+    * @ param a,b,c,d The 4 parameters of the polynomial
+    */
+    LaneBorder(double s, double a, double b, double c, double d);
+};
+
+
 /**
 * Lane width class. Holds all the data that describes a lane width
 *
@@ -770,4 +810,44 @@ public:
 //----------------------------------------------------------------------------------
 
 
+class LaneOffset
+{
+private:
+    /*
+    * Parameters that describe the lane offeset
+    */
+    double ms;
+    double ma;
+    double mb;
+    double mc;
+    double md;
+public:
+    /*
+    * Constructors
+    */
+    LaneOffset();
+    LaneOffset (double sOffset, double a, double b,double c,double d);
+
+
+    /*
+    * Methods that return the parameters of the lane offset
+    */
+    double GetS();
+    double Geta();
+    double Getb();
+    double Getc();
+    double Getd();
+
+    bool CheckInterval(double s_check);
+
+    /*
+    * Methods that set the parameters of the lane offset
+    */
+    void SetS(double value);
+    void Seta(double value);
+    void Setb(double value);
+    void Setc(double value);
+    void Setd(double value);
+};
+
 #endif

src/common/common/xodr/OpenDrive/ObjectSignal.cpp

@@ -2,6 +2,682 @@
 
 #include <iostream>
 
+
+Object_outlines_outline::Object_outlines_outline()
+{
+
+}
+
+Object_outlines::Object_outlines()
+{
+
+}
+
+Object_material::Object_material()
+{
+
+}
+
+int Object_material::Getsurface(string & surface)
+{
+    if(msurface.size()<1)return 0;
+    surface = msurface[0];
+    return 1;
+}
+
+int Object_material::Getfriction(double & friction)
+{
+    if(mfriction.size()<1)return 0;
+    friction = mfriction[0];
+    return 1;
+}
+
+int Object_material::Getroughness(double & roughness)
+{
+    if(mroughness.size()<1)return 0;
+    roughness = mroughness[0];
+    return 1;
+}
+
+void Object_material::Setsurface(string surface)
+{
+    if(msurface.size()>0)msurface.clear();
+    msurface.push_back(surface);
+}
+
+void Object_material::Setfriction(double friction)
+{
+    if(mfriction.size()>0)mfriction.clear();
+    mfriction.push_back(friction);
+}
+
+void Object_material::Setroughness(double roughness)
+{
+    if(mroughness.size()>0)mroughness.clear();
+    mroughness.push_back(roughness);
+}
+
+Object_repeat::Object_repeat(double s,double length,double distance,double tStart,double tEnd,double heightStart,
+              double heightEnd,double zOffsetStart,double zOffsetEnd)
+{
+    ms = s;
+    mlength = length;
+    mdistance = distance;
+    mtStart = tStart;
+    mtEnd = tEnd;
+    mheightStart = heightStart;
+    mheightEnd = heightEnd;
+    mzOffsetStart = zOffsetStart;
+    mzOffsetEnd = zOffsetEnd;
+}
+
+double Object_repeat::Gets()
+{
+    return ms;
+}
+
+double Object_repeat::Getlength()
+{
+    return mlength;
+}
+
+double Object_repeat::Getdistance()
+{
+    return mdistance;
+}
+
+double Object_repeat::GettStart()
+{
+    return mtStart;
+}
+
+double Object_repeat::GettEnd()
+{
+    return mtEnd;
+}
+
+double Object_repeat::GetheightStart()
+{
+    return mheightStart;
+}
+
+double Object_repeat::GetheightEnd()
+{
+    return mheightEnd;
+}
+
+double Object_repeat::GetzOffsetStart()
+{
+    return mzOffsetStart;
+}
+
+double Object_repeat::GetzOffsetEnd()
+{
+    return mzOffsetEnd;
+}
+
+
+void Object_repeat::Sets(double s)
+{
+    ms = s;
+}
+
+void Object_repeat::Setlength(double length)
+{
+    mlength = length;
+}
+
+void Object_repeat::Setdistance(double distance)
+{
+    mdistance = distance;
+}
+
+void Object_repeat::SettStart(double tStart)
+{
+    mtStart = tStart;
+}
+
+void Object_repeat::SettEnd(double tEnd)
+{
+    mtEnd = tEnd;
+}
+
+void Object_repeat::SetheightStart(double heightStart)
+{
+    mheightStart = heightStart;
+}
+
+void Object_repeat::SetheightEnd(double heightEnd)
+{
+    mheightEnd = heightEnd;
+}
+
+void Object_repeat::SetzOffsetStart(double zOffsetStart)
+{
+    mzOffsetStart = zOffsetStart;
+}
+
+void Object_repeat::SetzOffsetEnd(double zOffsetEnd)
+{
+    mzOffsetEnd = zOffsetEnd;
+}
+
+
+int Object_repeat::GetwidthStart(double & widthStart)
+{
+    if(mwidthStart.size() <1)return 0;
+    widthStart = mwidthStart[0];
+    return 1;
+}
+
+int Object_repeat::GetwidthEnd(double & widthEnd)
+{
+    if(mwidthEnd.size() < 1)return 0;
+    widthEnd = mwidthEnd[0];
+    return 1;
+}
+
+int Object_repeat::GetlengthStart(double & lengthStart)
+{
+    if(mlengthStart.size() < 1)return 0;
+    lengthStart = mlengthStart[0];
+    return 1;
+}
+
+int Object_repeat::GetlengthEnd(double & lengthEnd)
+{
+    if(mlengthEnd.size() < 1)return 0;
+    lengthEnd = mlengthEnd[0];
+    return 1;
+}
+
+int Object_repeat::GetradiusStart(double & radiusStart)
+{
+    if(mradiusStart.size() < 1)return 0;
+    radiusStart = mradiusStart[0];
+    return 1;
+}
+
+int Object_repeat::GetradiusEnd(double & radiusEnd)
+{
+    if(mradiusEnd.size() < 1)return 0;
+    radiusEnd = mradiusEnd[0];
+    return 1;
+}
+
+void Object_repeat::DeletewidthStart()
+{
+    if(mwidthStart.size() > 0)mwidthStart.clear();
+}
+
+void Object_repeat::DeletewidthEnd()
+{
+    if(mwidthEnd.size() > 0)mwidthEnd.clear();
+}
+
+void Object_repeat::DeletelengthStart()
+{
+    if(mlengthStart.size() > 0)mlengthStart.clear();
+}
+
+void Object_repeat::DeletelengthEnd()
+{
+    if(mlengthEnd.size() > 0)mlengthEnd.clear();
+}
+
+void Object_repeat::DeleteradiusStart()
+{
+    if(mradiusStart.size() > 0)mradiusStart.clear();
+}
+
+void Object_repeat::DeleteradiusEnd()
+{
+    if(mradiusEnd.size() > 0)mradiusEnd.clear();
+}
+
+void Object_repeat::SetwidthStart(double  widthStart)
+{
+    if(mwidthStart.size() > 0)mwidthStart.clear();
+    mwidthStart.push_back(widthStart);
+}
+
+void Object_repeat::SetwidthEnd(double  widthEnd)
+{
+    if(mwidthEnd.size() > 0)mwidthEnd.clear();
+    mwidthEnd.push_back(widthEnd);
+}
+
+void Object_repeat::SetlengthStart(double  lengthStart)
+{
+    if(mlengthStart.size() > 0)mlengthStart.clear();
+    mlengthStart.push_back(lengthStart);
+}
+
+void Object_repeat::SetlengthEnd(double  lengthEnd)
+{
+    if(mlengthEnd.size() > 0)mlengthEnd.clear();
+    mlengthEnd.push_back(lengthEnd);
+}
+
+void Object_repeat::SetradiusStart(double  radiusStart)
+{
+    if(mradiusStart.size() > 0)mradiusStart.clear();
+    mradiusStart.push_back(radiusStart);
+}
+
+void Object_repeat::SetradiusEnd(double  radiusEnd)
+{
+    if(mradiusEnd.size() > 0)mradiusEnd.clear();
+    mradiusEnd.push_back(radiusEnd);
+}
+
+/**
+* Check if the tested s-offset is inside the lane offset interval
+* @param A double s-offset value that has to be checked
+* @return Return true if the s-offset value belongs to current lane section, false otherwise
+*/
+bool Object_repeat::CheckInterval(double s_check)
+{
+    if (s_check>=ms)
+        return true;
+    else
+        return false;
+}
+
+Object_parkingSpace::Object_parkingSpace()
+{
+    maccess = "all";
+}
+
+void Object_parkingSpace::Setaccess(std::string access)
+{
+    maccess = access;
+}
+
+void Object_parkingSpace::Setrestrictions(std::string restrictions)
+{
+    mrestrictions = restrictions;
+}
+
+string Object_parkingSpace::Getaccess()
+{
+    return maccess;
+}
+
+string Object_parkingSpace::Getrestrictions()
+{
+    return mrestrictions;
+}
+
+/**
+ * Copy constructor
+ */
+Object_parkingSpace::Object_parkingSpace (const Object_parkingSpace& parkingSpace)
+{
+    if(this != &parkingSpace)
+    {
+        mrestrictions = parkingSpace.mrestrictions;
+        maccess = parkingSpace.maccess;
+    }
+}
+
+/**
+ * Assignment operator overload
+ */
+const Object_parkingSpace& Object_parkingSpace::operator=(const Object_parkingSpace& rhs)
+{
+    if (this!= &rhs)
+    {
+        mrestrictions = rhs.mrestrictions;
+        maccess = rhs.maccess;
+    }
+}
+
+Object::Object(std::string id, double s, double t, double zOffset)
+{
+    mid = id;
+    ms = s;
+    mt = t;
+    mzOffset = zOffset;
+}
+
+double Object::Gett()
+{
+    return mt;
+}
+
+double Object::GetzOffset()
+{
+    return mzOffset;
+}
+
+string Object::Gettype()
+{
+    return mtype;
+}
+
+int Object::GetvalidLength(double &validLength)
+{
+    if(mvalidLength.size()<1)return 0;
+    validLength = mvalidLength[0];
+    return 1;
+}
+
+string Object::Getorientation()
+{
+    return morientation;
+}
+
+string Object::Getsubtype()
+{
+    return msubtype;
+}
+
+string Object::Getdynamic()
+{
+    return mdynamic;
+}
+
+int Object::Gethdg(double &hdg)
+{
+    if(mhdg.size()<1)return 0;
+    hdg = mhdg[0];
+    return 1;
+}
+
+string Object::Getname()
+{
+    return mname;
+}
+
+int Object::Getpitch(double &pitch)
+{
+    if(mpitch.size()<1)return 0;
+    pitch = mpitch[0];
+    return 1;
+}
+
+string Object::Getid()
+{
+    return mid;
+}
+
+int Object::Getroll(double &roll)
+{
+    if(mroll.size()<1)return 0;
+    roll = mroll[0];
+    return 1;
+}
+
+int Object::Getheight(double &height)
+{
+    if(mheight.size()<1)return 0;
+    height = mheight[0];
+    return 1;
+}
+
+double Object::Gets()
+{
+    return ms;
+}
+
+int Object::Getlength(double &length)
+{
+    if(mlength.size()<1)return 0;
+    length = mlength[0];
+    return 1;
+}
+
+int Object::Getwidth(double &width)
+{
+    if(mwidth.size()<1)return 0;
+    width = mwidth[0];
+    return 1;
+}
+
+int Object::Getradius(double &radius)
+{
+    if(mradius.size()<1)return 0;
+    radius = mradius[0];
+    return 1;
+}
+
+int Object::GetparkingSpace(Object_parkingSpace &parkingSpace)
+{
+    if(mObject_parkingSpace.size()<1)return 0;
+    parkingSpace = mObject_parkingSpace[0];
+    return 1;
+}
+
+void Object::Sett(double t)
+{
+    mt = t;
+}
+
+void Object::SetzOffset(double zOffset)
+{
+    mzOffset = zOffset;
+}
+void Object::Settype(string type)
+{
+    mtype = type;
+}
+
+void Object::SetvalidLength(double  validLength)
+{
+    if(mvalidLength.size() > 0)mvalidLength.clear();
+    mvalidLength.push_back(validLength);
+}
+
+void Object::Setorientation(string orientation)
+{
+    morientation = orientation;
+}
+
+void Object::Setsubtype(string subtype)
+{
+    msubtype = subtype;
+}
+
+void Object::Setdynamic(string dynamic)
+{
+    mdynamic = dynamic;
+}
+
+void Object::Sethdg(double  hdg)
+{
+    if(mhdg.size()>0)mhdg.clear();
+    mhdg.push_back(hdg);
+}
+
+void Object::Setname(string name)
+{
+    mname = name;
+}
+
+void Object::Setpitch(double pitch)
+{
+    if(mpitch.size()>0)mpitch.clear();
+    mpitch.push_back(pitch);
+}
+
+void Object::Setid(string id)
+{
+    mid = id;
+}
+
+void Object::Setroll(double  roll)
+{
+    if(mroll.size()>0)mroll.clear();
+    mroll.push_back(roll);
+}
+
+void Object::Setheight(double height)
+{
+    if(mheight.size()>0)mheight.clear();
+    mheight.push_back(height);
+}
+
+void Object::Sets(double s)
+{
+    ms = s;
+}
+
+void Object::Setlength(double length)
+{
+    if(mlength.size()>0)mlength.clear();
+    mlength.push_back(length);
+}
+
+void Object::Setwidth(double width)
+{
+    if(mwidth.size()>0)mwidth.clear();
+    mwidth.push_back(width);
+}
+
+void Object::Setradius(double radius)
+{
+    if(mradius.size()>0)mradius.clear();
+    mradius.push_back(radius);
+}
+
+void Object::SetparkingSpace(Object_parkingSpace parkingSpace)
+{
+    if(mObject_parkingSpace.size()>0)mObject_parkingSpace.clear();
+    mObject_parkingSpace.push_back(parkingSpace);
+}
+
+vector<Object_repeat> * Object::GetObjectrepeatVector()
+{
+    return &mObject_repeat;
+}
+
+Object_repeat* Object::GetObjectrepeat(unsigned int i)
+{
+    if ((mObject_repeat.size()>0)&&(i<(mObject_repeat.size())))
+        return &(mObject_repeat.at(i));
+    else
+        return NULL;
+}
+
+unsigned int Object::GetObjectrepeatCount()
+{
+    return mObject_repeat.size();
+}
+
+Object_repeat*			Object::GetLastObjectrepeat()
+{
+    if (mObject_repeat.size()>0)
+        return &mObject_repeat.at(mObject_repeat.size()-1);
+    else
+        return NULL;
+}
+
+Object_repeat*			Object::GetLastAddedObjectrepeat()
+{
+    if(mLastAddedObjectrepeat<mObject_repeat.size())
+        return &mObject_repeat.at(mLastAddedObjectrepeat);
+    else
+        return NULL;
+}
+
+unsigned int Object::AddObjectrepeat(double s,double length,double distance,double tStart,double tEnd,
+                             double heightStart,double heightEnd,double zOffsetStart,double zOffsetEnd)
+{
+    unsigned int index = CheckObjectrepeatInterval(s)+1;
+    if(index>=GetObjectrepeatCount()) mObject_repeat.push_back(Object_repeat(s,length,distance,tStart,tEnd,heightStart,heightEnd,zOffsetStart,zOffsetEnd));
+    else mObject_repeat.insert(mObject_repeat.begin()+index, Object_repeat(s,length,distance,tStart,tEnd,heightStart,heightEnd,zOffsetStart,zOffsetEnd));
+    mLastAddedObjectrepeat=index;
+    return index;
+}
+
+unsigned int Object::CloneObjectrepeat(unsigned int index)
+{
+    if(index<(mObject_repeat.size()-1))
+        mObject_repeat.insert(mObject_repeat.begin()+index+1, mObject_repeat[index]);
+    else if(index==mObject_repeat.size()-1)
+        mObject_repeat.push_back(mObject_repeat[index]);
+    mLastAddedObjectrepeat=index+1;
+    return mLastAddedObjectrepeat;
+}
+
+void Object::DeleteObjectrepeat(unsigned int index)
+{
+    mObject_repeat.erase(mObject_repeat.begin()+index);
+}
+
+int Object::CheckObjectrepeatInterval(double s_check)
+{
+    int res=-1;
+    //Go through all the lane section records
+    for (unsigned int i=0;i<mObject_repeat.size();i++)
+    {
+        //check if the s_check belongs to the current record
+        if (mObject_repeat.at(i).CheckInterval(s_check))
+            res=i;	//assign it to the result id
+        else
+            break;	//if not, break;
+    }
+    return res;		//return the result: 0 to MaxInt as the index to the record containing s_check or -1 if nothing found
+
+}
+
+vector<Object_material> * Object::GetObjectmaterialVector()
+{
+    return &mObject_material;
+}
+
+Object_material* Object::GetObjectmaterial(unsigned int i)
+{
+    if ((mObject_material.size()>0)&&(i<(mObject_material.size())))
+        return &(mObject_material.at(i));
+    else
+        return NULL;
+}
+unsigned int Object::GetObjectmaterialCount()
+{
+    return mObject_material.size();
+}
+
+Object_material*			Object::GetLastObjectmaterial()
+{
+    if (mObject_material.size()>0)
+        return &mObject_material.at(mObject_material.size()-1);
+    else
+        return NULL;
+}
+
+Object_material*			Object::GetLastAddedObjectmaterial()
+{
+    if(mLastAddedObjectmaterial<mObject_material.size())
+        return &mObject_material.at(mLastAddedObjectmaterial);
+    else
+        return NULL;
+}
+
+unsigned int Object::AddObjectmaterial()
+{
+    mObject_material.push_back(Object_material());
+    mLastAddedObjectmaterial = mObject_material.size()-1;
+    return mLastAddedObjectmaterial;
+}
+
+unsigned int Object::CloneObjectmaterial(unsigned int index)
+{
+    if(index<(mObject_material.size()-1))
+        mObject_material.insert(mObject_material.begin()+index+1, mObject_material[index]);
+    else if(index==mObject_material.size()-1)
+        mObject_material.push_back(mObject_material[index]);
+    mLastAddedObjectmaterial=index+1;
+    return mLastAddedObjectmaterial;
+}
+
+void Object::DeleteObjectmaterial(unsigned int index)
+{
+    mObject_material.erase(mObject_material.begin()+index);
+}
+
+
 signal_positionRoad::signal_positionRoad(double s, double t, double zOffset, double hOffset, double pitch, double roll)
 {
     ms = s;

src/common/common/xodr/OpenDrive/ObjectSignal.h

@@ -7,14 +7,237 @@
 using std::vector;
 using std::string;
 
+class Object_outlines_outline
+{
+private:
+    vector<int> mid;
+    vector<string> mfillType;
+    vector<bool> mouter;
+    vector<bool> mclosed;
+    vector<string> mlaneTYpe;
+public:
+    Object_outlines_outline();
+};
+
+class Object_outlines
+{
+private:
+    vector<Object_outlines_outline> moutline;
+
+public:
+    Object_outlines();
+};
+
+class Object_material
+{
+private:
+    vector<string> msurface;
+    vector<double> mfriction;
+    vector<double> mroughness;
+
+public:
+    Object_material();
+
+    int Getsurface(string & surface);
+    int Getfriction(double & friction);
+    int Getroughness(double & roughness);
+
+    void Setsurface(string surface);
+    void Setfriction(double friction);
+    void Setroughness(double roughness);
+};
+
+class Object_repeat
+{
+private:
+    double ms;
+    double mlength;
+    double mdistance;
+    double mtStart;
+    double mtEnd;
+    double mheightStart;
+    double mheightEnd;
+    double mzOffsetStart;
+    double mzOffsetEnd;
+    vector<double> mwidthStart;
+    vector<double> mwidthEnd;
+    vector<double> mlengthStart;
+    vector<double> mlengthEnd;
+    vector<double> mradiusStart;
+    vector<double> mradiusEnd;
+
+public:
+    Object_repeat(double s,double length,double distance,double tStart,double tEnd,double heightStart,
+                  double heightEnd,double zOffsetStart,double zOffsetEnd);
+
+    double Gets();
+    double Getlength();
+    double Getdistance();
+    double GettStart();
+    double GettEnd();
+    double GetheightStart();
+    double GetheightEnd();
+    double GetzOffsetStart();
+    double GetzOffsetEnd();
+
+
+    void Sets(double s);
+    void Setlength(double length);
+    void Setdistance(double distance);
+    void SettStart(double tStart);
+    void SettEnd(double tEnd);
+    void SetheightStart(double heightStart);
+    void SetheightEnd(double heightEnd);
+    void SetzOffsetStart(double zOffsetStart);
+    void SetzOffsetEnd(double zOffsetEnd);
+
+
+    int GetwidthStart(double & widthStart);
+    int GetwidthEnd(double & widthEnd);
+    int GetlengthStart(double & lengthStart);
+    int GetlengthEnd(double & lengthEnd);
+    int GetradiusStart(double & radiusStart);
+    int GetradiusEnd(double & radiusEnd);
+
+    void DeletewidthStart();
+    void DeletewidthEnd();
+    void DeletelengthStart();
+    void DeletelengthEnd();
+    void DeleteradiusStart();
+    void DeleteradiusEnd();
+
+    void SetwidthStart(double  widthStart);
+    void SetwidthEnd(double  widthEnd);
+    void SetlengthStart(double  lengthStart);
+    void SetlengthEnd(double  lengthEnd);
+    void SetradiusStart(double  radiusStart);
+    void SetradiusEnd(double  radiusEnd);
+
+    bool CheckInterval(double s_check);
+
+
+};
+
+class Object_parkingSpace
+{
+private:
+    string maccess;
+    string mrestrictions;
+
+public:
+    Object_parkingSpace();
+
+    void Setaccess(string access);
+    void Setrestrictions(string restrictions);
+
+    string Getaccess();
+    string Getrestrictions();
+
+    /**
+     * Copy constructor
+     */
+    Object_parkingSpace (const Object_parkingSpace& parkingSpace);
+
+    /**
+     * Assignment operator overload
+     */
+    const Object_parkingSpace& operator=(const Object_parkingSpace& rhs);
+};
 
 //***********************************************************************************
 //Object
 //***********************************************************************************
 class Object
 {
+private:
+    double mt;
+    double mzOffset;
+    string mtype;
+    vector<double> mvalidLength;
+    string morientation;
+    string msubtype;
+    string mdynamic;
+    vector<double> mhdg;
+    string mname;
+    vector<double> mpitch;
+    string mid;
+    vector<double> mroll;
+    vector<double> mheight;
+    double ms;
+    vector<double> mlength;
+    vector<double> mwidth;
+    vector<double> mradius;
+
+    vector<Object_parkingSpace> mObject_parkingSpace;
+    vector<Object_repeat> mObject_repeat;
+    vector<Object_material> mObject_material;
+
+    unsigned int mLastAddedObjectrepeat;
+    unsigned int mLastAddedObjectmaterial;
+
 public:
-	Object(){}
+
+    Object(string id,double s,double t, double zOffset);
+
+    double Gett();
+    double GetzOffset();
+    string Gettype();
+    int GetvalidLength(double & validLength);
+    string Getorientation();
+    string Getsubtype();
+    string Getdynamic();
+    int Gethdg(double & hdg);
+    string Getname();
+    int Getpitch(double & pitch);
+    string Getid();
+    int Getroll(double & roll);
+    int Getheight(double & height);
+    double Gets();
+    int Getlength(double & length);
+    int Getwidth(double & width);
+    int Getradius(double & radius);
+
+    int GetparkingSpace(Object_parkingSpace & parkingSpace);
+
+    void Sett(double t);
+    void SetzOffset(double zOffset);
+    void Settype(string type);
+    void SetvalidLength(double  validLength);
+    void Setorientation(string orientation);
+    void Setsubtype(string subtype);
+    void Setdynamic(string dynamic);
+    void Sethdg(double  hdg);
+    void Setname(string name);
+    void Setpitch(double pitch);
+    void Setid(string id);
+    void Setroll(double  roll);
+    void Setheight(double height);
+    void Sets(double s);
+    void Setlength(double length);
+    void Setwidth(double width);
+    void Setradius(double radius);
+
+    void SetparkingSpace(Object_parkingSpace parkingSpace);
+
+    vector<Object_repeat> * GetObjectrepeatVector();
+    Object_repeat* GetObjectrepeat(unsigned int i);
+    unsigned int GetObjectrepeatCount();
+    Object_repeat*			GetLastObjectrepeat();
+    Object_repeat*			GetLastAddedObjectrepeat();
+    unsigned int AddObjectrepeat(double s,double length,double distance,double tStart,double tEnd,
+                                 double heightStart,double heightEnd,double zOffsetStart,double zOffsetEnd);
+    unsigned int CloneObjectrepeat(unsigned int index);
+    void DeleteObjectrepeat(unsigned int index);
+    int CheckObjectrepeatInterval(double s_check);
+
+    vector<Object_material> * GetObjectmaterialVector();
+    Object_material* GetObjectmaterial(unsigned int i);
+    unsigned int GetObjectmaterialCount();
+    Object_material*			GetLastObjectmaterial();
+    Object_material*			GetLastAddedObjectmaterial();
+    unsigned int AddObjectmaterial();
+    unsigned int CloneObjectmaterial(unsigned int index);
+    void DeleteObjectmaterial(unsigned int index);
 };
 //----------------------------------------------------------------------------------
 

src/common/common/xodr/OpenDrive/OpenDrive.cpp

@@ -158,6 +158,50 @@ void OpenDrive::Clear()
 	mJunctionVector.clear();
 }
 
+OpenDrive::OpenDrive (const OpenDrive& openDrive)
+{
+    mRoadVector = openDrive.mRoadVector;
+    mJunctionVector = openDrive.mJunctionVector;
+    Header * pHeader = openDrive.mHeader;
+    if(pHeader != NULL)
+    {
+        unsigned short int revMajor,revMinor;
+        std::string name,date;
+        float version;
+        double north,south,east,west,lat0,lon0,hdg0;
+        pHeader->GetAllParams(revMajor,revMinor,name,version,date,north,south,east,west,lat0,lon0,hdg0);
+
+        mHeader = new Header(revMajor,revMinor,name,version,date,north,south,east,west,lat0,lon0,hdg0);
+
+    }
+    else
+    {
+        mHeader == NULL;
+    }
+}
+
+const OpenDrive& OpenDrive::operator=(const OpenDrive& rhs)
+{
+    mRoadVector = rhs.mRoadVector;
+    mJunctionVector = rhs.mJunctionVector;
+    Header * pHeader = rhs.mHeader;
+    if(pHeader != NULL)
+    {
+        unsigned short int revMajor,revMinor;
+        std::string name,date;
+        float version;
+        double north,south,east,west,lat0,lon0,hdg0;
+        pHeader->GetAllParams(revMajor,revMinor,name,version,date,north,south,east,west,lat0,lon0,hdg0);
+
+        mHeader = new Header(revMajor,revMinor,name,version,date,north,south,east,west,lat0,lon0,hdg0);
+
+    }
+    else
+    {
+        mHeader == NULL;
+    }
+}
+
 /**
  * Destructor
  */
@@ -305,3 +349,33 @@ void Header::GetLat0Lon0(double &lat0, double &lon0)
     lat0 = mLat0;
     lon0 = mLon0;
 }
+
+void Header::GetVendor(std::string &strvendor)
+{
+    strvendor = mVendor;
+}
+
+void Header::SetVendor(std::string strvendor)
+{
+    mVendor = strvendor;
+}
+
+void Header::GetgeoReference(std::string &strgeoReference)
+{
+     strgeoReference = mgeoReference;
+}
+
+void Header::GetuserData(std::string &struserData)
+{
+    struserData = muserData;
+}
+
+void Header::SetgeoReference(std::string strgeoReference)
+{
+    mgeoReference = strgeoReference;
+}
+
+void Header::SetuserData(std::string struserData)
+{
+    muserData = struserData;
+}

src/common/common/xodr/OpenDrive/OpenDrive.h

@@ -42,13 +42,12 @@ private:
 	unsigned int mLastAddedRoad;
 	unsigned int mLastAddedJunction;
 
-	//-------------------------------------------------
+//	//-------------------------------------------------
+
+//	/**
+//	 * Copy constructor, makes the object non-copyable
+//	 */
 
-	/**
-	 * Copy constructor, makes the object non-copyable
-	 */
-    OpenDrive (const OpenDrive& openDrive){};
-    const OpenDrive& operator=(const OpenDrive& rhs){};
 
 public:
 	/**
@@ -56,6 +55,9 @@ public:
 	 */
 	OpenDrive();
 
+    OpenDrive (const OpenDrive& openDrive);
+    const OpenDrive& operator=(const OpenDrive& rhs);
+
 	//-------------------------------------------------
 
 	/**
@@ -128,7 +130,6 @@ public:
 };
 
 
-
 /**
  * Class used to store the heading details of the OpenDrive file
  */
@@ -147,12 +148,16 @@ private:
 	double mSouth;
 	double mEast;
 	double mWest;
+    string mVendor;
 
     //Added by Yuchuli,2019.11.04
     double mLat0;
     double mLon0;
     double mHdg0;
 
+    string mgeoReference;
+    string muserData;
+
 public:
 	/**
 	 * Constructor that initializes the base properties
@@ -171,6 +176,10 @@ public:
     void GetAllParams(unsigned short int &revMajor, unsigned short int &revMinor, string &name, float &version, string &date,
         double &north, double &south, double &east,double &west,double &lat0,double &lon0, double & hdg0);
 	void GetXYValues(double &north, double &south, double &east,double &west);
+    void GetVendor(std::string & strvendor);
+
+    void GetgeoReference(std::string & strgeoReference);
+    void GetuserData(std::string & struserData);
 	
 	/**
 	 * Setter for all properties
@@ -181,6 +190,9 @@ public:
 
     void SetAllParams(unsigned short int revMajor, unsigned short int revMinor, string name, float version, string date,
         double north, double south, double east,double west,double lat0,double lon0,double hdg0);
+    void SetVendor(std::string strvendor);
+    void SetgeoReference(std::string strgeoReference);
+    void SetuserData(std::string struserData);
 
 
     void GetLat0Lon0(double & lat0,double & lon0);

src/common/common/xodr/OpenDrive/OpenDriveXmlParser.cpp

@@ -30,6 +30,7 @@ bool OpenDriveXmlParser::ReadHeader(TiXmlElement *node)
 	double south;
 	double east;
 	double west;
+    string strvendor;
 
     //Added By Yuchuli,2019.11.04
     double lat0;
@@ -54,12 +55,43 @@ bool OpenDriveXmlParser::ReadHeader(TiXmlElement *node)
 		return false;
 	}
 
-    checker+=node->QueryDoubleAttribute("lat0",&lat0);
-    checker+=node->QueryDoubleAttribute("lon0",&lon0);
+
+    if(node->QueryDoubleAttribute("lat0",&lat0) != TIXML_SUCCESS)
+    {
+        lon0 = 119.0;
+    }
+    if(node->QueryDoubleAttribute("lon0",&lon0) != TIXML_SUCCESS)
+    {
+        lat0 = 39.0;
+    }
     checker+=node->QueryDoubleAttribute("hdg0",&hdg0);
 
     mOpenDrive->SetHeader(revMajor, revMinor, name, version, date, north, south, east, west,lat0,lon0,hdg0);
 
+    if(node->QueryStringAttribute("vendor",&strvendor) == TIXML_SUCCESS)
+    {
+        mOpenDrive->GetHeader()->SetVendor(strvendor);
+    }
+
+    TiXmlElement *nodegeo=node->FirstChildElement("geoReference");
+    if(nodegeo != NULL)
+    {
+
+        TiXmlPrinter *printer = new TiXmlPrinter();
+        nodegeo->Accept(printer );//保存该节点及其子节点到字符串
+        std::string str = printer->Str();
+        mOpenDrive->GetHeader()->SetgeoReference(str);
+    }
+
+    TiXmlElement *nodeuserData=node->FirstChildElement("userData");
+    if(nodeuserData != NULL)
+    {
+
+        TiXmlPrinter *printer = new TiXmlPrinter();
+        nodeuserData->Accept(printer );//保存该节点及其子节点到字符串
+        std::string str = printer->Str();
+        mOpenDrive->GetHeader()->SetuserData(str);
+    }
 	return true;
 
 }
@@ -72,6 +104,8 @@ bool OpenDriveXmlParser::ReadRoad(TiXmlElement *node)
 	double length;
 	string id;
 	string junction;
+    string rule;
+    bool bHaverule = false;
 
 	int checker=TIXML_SUCCESS;
 	
@@ -86,6 +120,11 @@ bool OpenDriveXmlParser::ReadRoad(TiXmlElement *node)
 		return false;
 	}
 
+    if(node->QueryStringAttribute("rule",&rule) == TIXML_SUCCESS)
+    {
+        bHaverule = true;
+    }
+
     if(node->QueryStringAttribute("name",&name) != TIXML_SUCCESS)
     {
         name = "";
@@ -93,6 +132,12 @@ bool OpenDriveXmlParser::ReadRoad(TiXmlElement *node)
 	//fill in
 	mOpenDrive->AddRoad(name, length, id, junction);
 	Road* road = mOpenDrive->GetLastAddedRoad();
+
+    if(bHaverule)
+    {
+        road->SetRoadRule(rule);
+    }
+
 	TiXmlElement* subNode;
 
 
@@ -211,6 +256,19 @@ bool  OpenDriveXmlParser::ReadRoadLink (Road* road, TiXmlElement *node, short in
 			return false;
 		}
 		road->SetPredecessor(elementType,elementId,contactPoint);
+
+        RoadLink * pRoadLink = road->GetPredecessor();
+        double elementS;
+        string elementDir;
+        if(node->QueryDoubleAttribute("elementS",&elementS) == TIXML_SUCCESS)
+        {
+            pRoadLink->SetElementS(elementS);
+        }
+        if(node->QueryStringAttribute("elementDir",&elementDir) == TIXML_SUCCESS)
+        {
+            pRoadLink->SetELementDir(elementDir);
+        }
+
 		return true;
 
 	}
@@ -232,6 +290,18 @@ bool  OpenDriveXmlParser::ReadRoadLink (Road* road, TiXmlElement *node, short in
 			return false;
 		}
 		road->SetSuccessor(elementType,elementId,contactPoint);
+
+        RoadLink * pRoadLink = road->GetSuccessor();
+        double elementS;
+        string elementDir;
+        if(node->QueryDoubleAttribute("elementS",&elementS) == TIXML_SUCCESS)
+        {
+            pRoadLink->SetElementS(elementS);
+        }
+        if(node->QueryStringAttribute("elementDir",&elementDir) == TIXML_SUCCESS)
+        {
+            pRoadLink->SetELementDir(elementDir);
+        }
 		return true;
 	}
 
@@ -258,12 +328,32 @@ bool  OpenDriveXmlParser::ReadRoadLink (Road* road, TiXmlElement *node, short in
 	return false;
 		
 }
+
+//--------------
+
+bool OpenDriveXmlParser::ReadRoadTypeSpeed(RoadType *roadtype, TiXmlElement *node)
+{
+    double maxspeed;
+    string unit = "m/s";
+    int checker=TIXML_SUCCESS;
+    checker+=node->QueryDoubleAttribute("max",&maxspeed);
+    node->QueryStringAttribute("unit",&unit);
+    if(checker != TIXML_SUCCESS)
+    {
+        cout<<"Error Parsing Road Type Speed"<<endl;
+        return false;
+    }
+    roadtype->AddRoadTypeSpeed(maxspeed,unit);
+    return true;
+}
+
 //--------------
 
 bool OpenDriveXmlParser::ReadRoadType (Road* road, TiXmlElement *node)
 {
 	double s;
 	string type;
+    string country;
 
 	int checker=TIXML_SUCCESS;
 	checker+=node->QueryDoubleAttribute("s",&s);
@@ -275,7 +365,25 @@ bool OpenDriveXmlParser::ReadRoadType (Road* road, TiXmlElement *node)
 		return false;
 	}
 
-	road->AddRoadType(s,type);
+    if(node->QueryStringAttribute("country",&country) == TIXML_SUCCESS)
+    {
+        road->AddRoadType(s,type,country);
+    }
+    else
+    {
+        road->AddRoadType(s,type);
+    }
+
+    RoadType * roadtype = road->GetRoadType(0);
+    TiXmlElement * subNode;
+    subNode=node->FirstChildElement("speed");
+    while (subNode)
+    {
+        ReadRoadTypeSpeed(roadtype, subNode);
+        subNode=subNode->NextSiblingElement("speed");
+    }
+
+
 	return true;
 }
 //--------------
@@ -533,15 +641,46 @@ bool OpenDriveXmlParser::ReadLanes (Road* road, TiXmlElement *node)
 		subNode=subNode->NextSiblingElement("laneSection");
 	}
 
+    subNode = node->FirstChildElement("laneOffset");
+    while (subNode)
+    {
+        ReadLaneOffsets(road, subNode);
+        subNode=subNode->NextSiblingElement("laneOffset");
+    }
+
 	return true;
 }
 //--------------
 
+bool OpenDriveXmlParser::ReadLaneOffsets(Road *road, TiXmlElement *node)
+{
+    int checker=TIXML_SUCCESS;
+    double s,a,b,c,d;
+    checker+=node->QueryDoubleAttribute("s",&s);
+    checker+=node->QueryDoubleAttribute("a",&a);
+    checker+=node->QueryDoubleAttribute("b",&b);
+    checker+=node->QueryDoubleAttribute("c",&c);
+    checker+=node->QueryDoubleAttribute("d",&d);
+
+    if (checker!=TIXML_SUCCESS)
+    {
+        cout<<"Error parsing Lane Offset attributes"<<endl;
+        return false;
+    }
+
+    road->AddLaneOffset(s,a,b,c,d);
+
+   return true;
+
+
+}
+
 bool OpenDriveXmlParser::ReadLaneSections (Road* road, TiXmlElement *node)
 {
 
 	int checker=TIXML_SUCCESS;
 	double s;
+    string singleSide;
 	checker+=node->QueryDoubleAttribute("s",&s);
 
 	if (checker!=TIXML_SUCCESS)
@@ -553,6 +692,12 @@ bool OpenDriveXmlParser::ReadLaneSections (Road* road, TiXmlElement *node)
 	
 	road->AddLaneSection(s);
 	LaneSection* laneSection=road->GetLastAddedLaneSection();
+
+    if(node->QueryStringAttribute("singleSide",&singleSide) == TIXML_SUCCESS)
+    {
+        laneSection->SetSingleSide(singleSide);
+    }
+
 	TiXmlElement *subNode=node->FirstChildElement("left");
 	if (subNode)
 	{
@@ -676,6 +821,14 @@ bool OpenDriveXmlParser::ReadLane (LaneSection* laneSection, TiXmlElement *node,
 			}
     }
 
+    //Proceed to the Road border
+    subNode=node->FirstChildElement("border");
+    while (subNode)
+    {
+        ReadLaneBorder(lane, subNode);
+        subNode=subNode->NextSiblingElement("border");
+    }
+
 	//Proceed to the Road width
 	subNode=node->FirstChildElement("width");
 	while (subNode)
@@ -732,11 +885,45 @@ bool OpenDriveXmlParser::ReadLane (LaneSection* laneSection, TiXmlElement *node,
 		subNode=subNode->NextSiblingElement("height");
 	}
 
+    //Proceed to the user Data
+    TiXmlElement *nodeuserData=node->FirstChildElement("userData");
+    if(nodeuserData != NULL)
+    {
+
+        TiXmlPrinter *printer = new TiXmlPrinter();
+        nodeuserData->Accept(printer );//保存该节点及其子节点到字符串
+        std::string str = printer->Str();
+        lane->SetuserData(str);
+    }
+
 	return true;
 }
 //--------------
 
 
+bool OpenDriveXmlParser::ReadLaneBorder(Lane* lane, TiXmlElement *node)
+{
+    double sOffset, a, b, c, d;
+
+    int checker=TIXML_SUCCESS;
+    checker+=node->QueryDoubleAttribute("sOffset",&sOffset);
+    checker+=node->QueryDoubleAttribute("a",&a);
+    checker+=node->QueryDoubleAttribute("b",&b);
+    checker+=node->QueryDoubleAttribute("c",&c);
+    checker+=node->QueryDoubleAttribute("d",&d);
+
+    if (checker!=TIXML_SUCCESS)
+    {
+        cout<<"Error parsing Lane Weight attributes"<<endl;
+        return false;
+    }
+
+    lane->AddBorderRecord(sOffset,a,b,c,d);
+
+    return true;
+}
+//--------------
+
 bool OpenDriveXmlParser::ReadLaneWidth(Lane* lane, TiXmlElement *node)
 {
 	double sOffset, a, b, c, d;
@@ -766,15 +953,25 @@ bool OpenDriveXmlParser::ReadLaneRoadMark(Lane* lane, TiXmlElement *node)
 	double sOffset;
 	string type;
 	string weight;
+    string weightdef = "standard";
 	string color; 
+    string colordef = "white";
 	double width;
 	string laneChange;
 
 	int checker=TIXML_SUCCESS;
 	checker+=node->QueryDoubleAttribute("sOffset",&sOffset);
 	checker+=node->QueryStringAttribute("type",&type);
-	checker+=node->QueryStringAttribute("weight",&weight);
-	checker+=node->QueryStringAttribute("color",&color);
+    if(node->QueryStringAttribute("weight",&weight) != TIXML_SUCCESS)
+    {
+        weight = weightdef;
+    }
+//	checker+=node->QueryStringAttribute("weight",&weight);
+    if(node->QueryStringAttribute("color",&color) != TIXML_SUCCESS)
+    {
+        color = colordef;
+    }
+
 	
 	if (checker!=TIXML_SUCCESS)
 	{
@@ -913,10 +1110,270 @@ bool OpenDriveXmlParser::ReadLaneHeight(Lane* lane, TiXmlElement *node)
 
 bool OpenDriveXmlParser::ReadObjects (Road* road, TiXmlElement *node)
 {
+    TiXmlElement *subNode = node->FirstChildElement("object");
+    while (subNode)
+    {
+        ReadObject(road, subNode);
+        subNode=subNode->NextSiblingElement("object");
+    }
 	return true;
 }
 //--------------
 
+bool OpenDriveXmlParser::ReadObjectMaterial(Object *pObject, TiXmlElement *node)
+{
+    pObject->AddObjectmaterial();
+
+    Object_material * pObject_material = pObject->GetLastAddedObjectmaterial();
+
+    string surface;
+    double friction;
+    double roughness;
+    if(node->QueryStringAttribute("surface",&surface) == TIXML_SUCCESS)
+    {
+        pObject_material->Setsurface(surface);
+    }
+
+    if(node->QueryDoubleAttribute("friction",&friction) == TIXML_SUCCESS)
+    {
+        pObject_material->Setfriction(friction);
+    }
+
+    if(node->QueryDoubleAttribute("roughness",&roughness) == TIXML_SUCCESS)
+    {
+        pObject_material->Setroughness(roughness);
+    }
+
+}
+//--------------
+bool OpenDriveXmlParser::ReadObjectRepeat(Object *pObject, TiXmlElement *node)
+{
+    double s;
+    double length;
+    double distance;
+    double tStart;
+    double tEnd;
+    double heightStart;
+    double heightEnd;
+    double zOffsetStart;
+    double zOffsetEnd;
+
+    double widthStart;
+    double widthEnd;
+    double lengthStart;
+    double lengthEnd;
+    double radiusStart;
+    double radiusEnd;
+
+    int checker=TIXML_SUCCESS;
+    checker+=node->QueryDoubleAttribute("s",&s);
+    checker+=node->QueryDoubleAttribute("length",&length);
+    checker+=node->QueryDoubleAttribute("distance",&distance);
+    checker+=node->QueryDoubleAttribute("tStart",&tStart);
+    checker+=node->QueryDoubleAttribute("tEnd",&tEnd);
+    checker+=node->QueryDoubleAttribute("heightStart",&heightStart);
+    checker+=node->QueryDoubleAttribute("heightEnd",&heightEnd);
+    checker+=node->QueryDoubleAttribute("zOffsetStart",&zOffsetStart);
+    checker+=node->QueryDoubleAttribute("zOffsetEnd",&zOffsetEnd);
+
+    if(checker != TIXML_SUCCESS)
+    {
+        cout<<"Error parsing Object repeat attributes"<<endl;
+        return false;
+    }
+
+    pObject->AddObjectrepeat(s,length,distance,tStart,tEnd,heightStart,heightEnd,zOffsetStart,zOffsetEnd);
+
+    Object_repeat * pObject_repeate = pObject->GetLastAddedObjectrepeat();
+
+    return true;
+
+    if(node->QueryDoubleAttribute("widthStart",&widthStart) == TIXML_SUCCESS)
+    {
+        pObject_repeate->SetwidthStart(widthStart);
+    }
+
+    if(node->QueryDoubleAttribute("widthEnd",&widthEnd) == TIXML_SUCCESS)
+    {
+        pObject_repeate->SetwidthEnd(widthEnd);
+    }
+
+    if(node->QueryDoubleAttribute("lengthStart",&lengthStart) == TIXML_SUCCESS)
+    {
+        pObject_repeate->SetlengthStart(lengthStart);
+    }
+
+    if(node->QueryDoubleAttribute("lengthEnd",&lengthEnd) == TIXML_SUCCESS)
+    {
+        pObject_repeate->SetlengthEnd(lengthEnd);
+    }
+
+    if(node->QueryDoubleAttribute("radiusStart",&radiusStart) == TIXML_SUCCESS)
+    {
+        pObject_repeate->SetradiusStart(radiusStart);
+    }
+
+    if(node->QueryDoubleAttribute("radiusEnd",&radiusEnd) == TIXML_SUCCESS)
+    {
+        pObject_repeate->SetradiusEnd(radiusEnd);
+    }
+
+    return true;
+
+
+}
+//--------------
+bool OpenDriveXmlParser::ReadObjectParkingSpace(Object *pObject, TiXmlElement *node)
+{
+    string access;
+    string restrictions;
+    int checker=TIXML_SUCCESS;
+    checker+=node->QueryStringAttribute("access",&access);
+    if(checker != TIXML_SUCCESS)
+    {
+        cout<<"Error parsing Object Parking Space attributes"<<endl;
+        return false;
+    }
+
+    Object_parkingSpace parkingSpace;
+    parkingSpace.Setaccess(access);
+    if(node->QueryStringAttribute("restrictions",&restrictions) == TIXML_SUCCESS)
+    {
+        parkingSpace.Setrestrictions(restrictions);
+    }
+    pObject->SetparkingSpace(parkingSpace);
+
+    return true;
+
+}
+//--------------
+
+bool OpenDriveXmlParser::ReadObject(Road *road, TiXmlElement *node)
+{
+    double t;
+    double zOffset;
+    string type;
+    double validLength;
+    string orientation;
+    string subtype;
+    string dynamic;
+    double hdg;
+    string name;
+    double pitch;
+    string id;
+    double roll;
+    double height;
+    double s;
+    double length;
+    double width;
+    double radius;
+
+    int checker=TIXML_SUCCESS;
+    checker+=node->QueryDoubleAttribute("s",&s);
+    checker+=node->QueryDoubleAttribute("t",&t);
+    checker+=node->QueryStringAttribute("id",&id);
+    checker+=node->QueryDoubleAttribute("zOffset",&zOffset);
+
+    if (checker!=TIXML_SUCCESS)
+    {
+        cout<<"Error parsing Object attributes"<<endl;
+        return false;
+    }
+
+    road->AddObject(id,s,t,zOffset);
+
+    Object * pobject = road->GetLastAddedObject();
+
+    if(node->QueryStringAttribute("type",&type) == TIXML_SUCCESS)
+    {
+        pobject->Settype(type);
+    }
+
+    if(node->QueryDoubleAttribute("validLength",&validLength) == TIXML_SUCCESS)
+    {
+        pobject->SetvalidLength(validLength);
+    }
+
+    if(node->QueryStringAttribute("orientation",&orientation) == TIXML_SUCCESS)
+    {
+        pobject->Setorientation(orientation);
+    }
+
+    if(node->QueryStringAttribute("subtype",&subtype) == TIXML_SUCCESS)
+    {
+        pobject->Setsubtype(subtype);
+    }
+
+    if(node->QueryStringAttribute("dynamic",&dynamic) == TIXML_SUCCESS)
+    {
+        pobject->Setdynamic(dynamic);
+    }
+
+    if(node->QueryDoubleAttribute("hdg",&hdg) == TIXML_SUCCESS)
+    {
+        pobject->Sethdg(hdg);
+    }
+
+    if(node->QueryStringAttribute("name",&name) == TIXML_SUCCESS)
+    {
+        pobject->Setname(name);
+    }
+
+    if(node->QueryDoubleAttribute("pitch",&pitch) == TIXML_SUCCESS)
+    {
+        pobject->Setpitch(pitch);
+    }
+
+    if(node->QueryDoubleAttribute("roll",&roll) == TIXML_SUCCESS)
+    {
+        pobject->Setroll(roll);
+    }
+
+    if(node->QueryDoubleAttribute("height",&height) == TIXML_SUCCESS)
+    {
+        pobject->Setheight(height);
+    }
+
+    if(node->QueryDoubleAttribute("length",&length) == TIXML_SUCCESS)
+    {
+        pobject->Setlength(length);
+    }
+
+    if(node->QueryDoubleAttribute("width",&width) == TIXML_SUCCESS)
+    {
+        pobject->Setwidth(width);
+    }
+
+    if(node->QueryDoubleAttribute("radius",&radius) == TIXML_SUCCESS)
+    {
+        pobject->Setradius(radius);
+    }
+
+    TiXmlElement *subNode = node->FirstChildElement("parkingSpace");
+    if (subNode)
+    {
+        ReadObjectParkingSpace(pobject, subNode);
+    }
+
+    subNode = node->FirstChildElement("repeat");
+    while (subNode)
+    {
+        ReadObjectRepeat(pobject, subNode);
+        subNode = node->FirstChildElement("repeat");
+    }
+
+    subNode = node->FirstChildElement("material");
+    while (subNode)
+    {
+        ReadObjectMaterial(pobject, subNode);
+        subNode = node->FirstChildElement("material");
+    }
+
+    return true;
+}
+
+//--------------
+
 bool OpenDriveXmlParser::ReadSignals (Road* road, TiXmlElement *node)
 {
 

src/common/common/xodr/OpenDrive/OpenDriveXmlParser.h

@@ -41,6 +41,8 @@ public:
 	bool ReadRoadType (Road* road, TiXmlElement *node);
 	//--------------
 
+    bool ReadRoadTypeSpeed (RoadType * roadtype, TiXmlElement *node);
+
 	bool ReadPlanView(Road* road, TiXmlElement *node);
 	bool ReadGeometryBlock (Road* road, TiXmlElement *&node, short int blockType);
 	bool ReadGeometry(GeometryBlock* geomBlock, TiXmlElement *node, short int geometryType);
@@ -60,13 +62,19 @@ public:
 	bool ReadLaneSpeed(Lane* lane, TiXmlElement *node);
 	bool ReadLaneAccess(Lane* lane, TiXmlElement *node);
 	bool ReadLaneHeight(Lane* lane, TiXmlElement *node);
+    bool ReadLaneOffsets(Road * road,TiXmlElement * node);
+    bool ReadLaneBorder(Lane* lane, TiXmlElement *node);
 	//--------------
 
 	bool ReadObjects (Road* road, TiXmlElement *node);
+    bool ReadObjectRepeat(Object * pObject,TiXmlElement *node);
+    bool ReadObjectMaterial(Object * pObject,TiXmlElement *node);
+    bool ReadObjectParkingSpace(Object * pObject,TiXmlElement *node);
 	bool ReadSignals (Road* road, TiXmlElement *node);
     bool ReadSignal(Road * road,TiXmlElement * node);
     bool ReadSignal_positionInertial(Signal * pSignal, TiXmlElement *node);
     bool ReadSignal_laneValidity(Signal * pSignal,TiXmlElement * node);
+    bool ReadObject(Road * road,TiXmlElement * node);
 	//--------------
 
 	bool ReadSurface (Road* road, TiXmlElement *node);

src/common/common/xodr/OpenDrive/OpenDriveXmlWriter.cpp

@@ -34,9 +34,11 @@ bool OpenDriveXmlWriter::WriteHeader(TiXmlElement *node)
     double lat0;
     double lon0;
     double hdg0;
+    string strvendor;
 
 	Header *lHeader = mOpenDrive->GetHeader();
     lHeader->GetAllParams(revMajor, revMinor, name, version, date, north, south, east, west,lat0,lon0,hdg0);
+    lHeader->GetVendor(strvendor);
 
 	TiXmlElement *nodeHeader = new TiXmlElement("header");
 	node->LinkEndChild(nodeHeader);
@@ -75,6 +77,36 @@ bool OpenDriveXmlWriter::WriteHeader(TiXmlElement *node)
     shdg0 << setprecision(16) << setiosflags (ios_base::scientific) << hdg0;
     nodeHeader->SetAttribute("hdg0",shdg0.str());
 
+    if(strvendor.size()>0)
+        nodeHeader->SetAttribute("vendor",strvendor);
+    else
+    {
+        nodeHeader->SetAttribute("vendor","adc");
+    }
+
+    std::string strgeoReference;
+    std::string struserData;
+    lHeader->GetgeoReference(strgeoReference);
+    lHeader->GetuserData(struserData);
+
+    if(strgeoReference != "")
+    {
+        TiXmlElement * nodegeoReference = new TiXmlElement("geoReference");
+        nodegeoReference->Parse(strgeoReference.data(),0,TIXML_ENCODING_UTF8);
+        nodeHeader->LinkEndChild(nodegeoReference);
+
+    }
+
+    if(struserData != "")
+    {
+        TiXmlElement * nodeuserData = new TiXmlElement("userData");
+        nodeuserData->Parse(struserData.data(),0,TIXML_ENCODING_UTF8);
+        nodeHeader->LinkEndChild(nodeuserData);
+
+    }
+
+
+
 
 	return true;
 }
@@ -86,11 +118,13 @@ bool OpenDriveXmlWriter::WriteRoad(TiXmlElement *node, Road *road)
 	double length;
 	string id;
 	string junction;
+    string rule;
 
 	name = road->GetRoadName();
 	length = road->GetRoadLength();
 	id = road->GetRoadId();
 	junction = road->GetRoadJunction();
+    rule = road->GetRoadRule();
 
 	TiXmlElement *nodeRoad = new TiXmlElement("road");
 	node->LinkEndChild(nodeRoad);
@@ -101,6 +135,10 @@ bool OpenDriveXmlWriter::WriteRoad(TiXmlElement *node, Road *road)
 	nodeRoad->SetAttribute("length",slength.str());
 	nodeRoad->SetAttribute("id",id);
 	nodeRoad->SetAttribute("junction",junction);
+    if(rule != "")
+    {
+        nodeRoad->SetAttribute("rule",rule);
+    }
 
 	//Fill in
 
@@ -123,6 +161,8 @@ bool OpenDriveXmlWriter::WriteRoad(TiXmlElement *node, Road *road)
 	WriteLanes(nodeRoad, road);
 
 
+
+
 	//Proceed to Objects
 	WriteObjects(nodeRoad, road);
 
@@ -155,6 +195,16 @@ bool  OpenDriveXmlWriter::WriteRoadLinks (TiXmlElement *node, Road* road)
 		nodeLinkPredecessor->SetAttribute("elementType", lPredecessor->GetElementType());
 		nodeLinkPredecessor->SetAttribute("elementId", lPredecessor->GetElementId());
 		nodeLinkPredecessor->SetAttribute("contactPoint", lPredecessor->GetContactPoint());
+        if(lPredecessor->GetElementS()>=0)
+        {
+            std::stringstream ss;
+            ss << setprecision(16) << setiosflags (ios_base::scientific) << lPredecessor->GetElementS();
+            nodeLinkPredecessor->SetAttribute("elementS", ss.str());
+        }
+        if(lPredecessor->GetElementDir() != "")
+        {
+            nodeLinkPredecessor->SetAttribute("elementDir",lPredecessor->GetElementDir());
+        }
 	}
 	RoadLink *lSuccessor = road->GetSuccessor();
 	if(lSuccessor)
@@ -164,6 +214,16 @@ bool  OpenDriveXmlWriter::WriteRoadLinks (TiXmlElement *node, Road* road)
 		nodeLinkSuccessor->SetAttribute("elementType", lSuccessor->GetElementType());
 		nodeLinkSuccessor->SetAttribute("elementId", lSuccessor->GetElementId());
 		nodeLinkSuccessor->SetAttribute("contactPoint", lSuccessor->GetContactPoint());
+        if(lSuccessor->GetElementS()>=0)
+        {
+            std::stringstream ss;
+            ss << setprecision(16) << setiosflags (ios_base::scientific) << lSuccessor->GetElementS();
+            nodeLinkSuccessor->SetAttribute("elementS", ss.str());
+        }
+        if(lSuccessor->GetElementDir() != "")
+        {
+            nodeLinkSuccessor->SetAttribute("elementDir",lSuccessor->GetElementDir());
+        }
 	}
 	RoadNeighbor *lNeighbor1 = road->GetNeighbor1();
 	if(lNeighbor1)
@@ -186,6 +246,34 @@ bool  OpenDriveXmlWriter::WriteRoadLinks (TiXmlElement *node, Road* road)
 
 	return true;
 }
+
+//--------------
+
+bool OpenDriveXmlWriter::WriteRoadTypeSpeed(TiXmlElement *node, RoadType *roadtype)
+{
+    double maxSpeed;
+    string unit;
+
+    unsigned int roadTypeSpeedCount = roadtype->GetRoadTypeSpeedCount();
+    for(unsigned int i=0;i<roadTypeSpeedCount;i++)
+    {
+        RoadTypeSpeed * lRoadTypeSpeed = roadtype->GetRoadTypeSpeed(i);
+
+        maxSpeed = lRoadTypeSpeed->GetmaxSpeed();
+        unit = lRoadTypeSpeed->Getunit();
+
+        TiXmlElement * nodeRoadTypeSpeed = new TiXmlElement("speed");
+        node->LinkEndChild(nodeRoadTypeSpeed);
+
+        std::stringstream ss;
+        ss << setprecision(16) << setiosflags (ios_base::scientific) << maxSpeed;
+        nodeRoadTypeSpeed->SetAttribute("max",ss.str());
+        nodeRoadTypeSpeed->SetAttribute("unit",unit);
+    }
+
+    return true;
+}
+
 //--------------
 
 bool OpenDriveXmlWriter::WriteRoadType (TiXmlElement *node, Road* road)
@@ -208,6 +296,8 @@ bool OpenDriveXmlWriter::WriteRoadType (TiXmlElement *node, Road* road)
 		ss << setprecision(16) << setiosflags (ios_base::scientific) << s;
 		nodeRoadType->SetAttribute("s",ss.str());
 		nodeRoadType->SetAttribute("type",type);
+
+        WriteRoadTypeSpeed(nodeRoadType,lRoadType);
 	}
 
 	return true;
@@ -531,6 +621,12 @@ bool OpenDriveXmlWriter::WriteLanes (TiXmlElement *node, Road* road)
 		WriteLaneSections(nodeLanes, road->GetLaneSection(i));
 	}
 
+    unsigned int lLaneOffsetCount = road->GetLaneOffsetCount();
+    for(unsigned int i=0; i<lLaneOffsetCount; i++)
+    {
+        WriteLaneOffset(nodeLanes, road->GetLaneOffset(i));
+    }
+
 	return true;
 }
 //--------------
@@ -547,6 +643,11 @@ bool OpenDriveXmlWriter::WriteLaneSections (TiXmlElement *node, LaneSection *lan
 	ss << setprecision(16) << setiosflags (ios_base::scientific) << s;
 	nodeLaneSection->SetAttribute("s",ss.str());
 
+    if(laneSection->GetSingleSide() != "")
+    {
+        nodeLaneSection->SetAttribute("singleSide",laneSection->GetSingleSide());
+    }
+
 	//Fill in lane section
 	short int curType=1;
 	TiXmlElement* nodeLanesLeft=NULL;
@@ -590,6 +691,42 @@ bool OpenDriveXmlWriter::WriteLaneSections (TiXmlElement *node, LaneSection *lan
 }
 //--------------
 
+bool OpenDriveXmlWriter::WriteLaneOffset(TiXmlElement *node, LaneOffset *laneOffset)
+{
+    double s;
+    s=laneOffset->GetS();
+
+    TiXmlElement* nodeLaneSection = new TiXmlElement("laneOffset");
+    node->LinkEndChild(nodeLaneSection);
+
+    nodeLaneSection->SetDoubleAttribute("s",laneOffset->GetS());
+    nodeLaneSection->SetDoubleAttribute("a",laneOffset->Geta());
+    nodeLaneSection->SetDoubleAttribute("b",laneOffset->Getb());
+    nodeLaneSection->SetDoubleAttribute("c",laneOffset->Getc());
+    nodeLaneSection->SetDoubleAttribute("d",laneOffset->Getd());
+
+//    std::stringstream ss;
+//    ss << setprecision(16) << setiosflags (ios_base::scientific) << s;
+//    nodeLaneSection->SetAttribute("s",ss.str());
+
+
+//    ss << setprecision(16) << setiosflags (ios_base::scientific) << laneOffset->Geta();
+//    nodeLaneSection->SetAttribute("a",ss.str());
+
+
+//    ss << setprecision(16) << setiosflags (ios_base::scientific) << laneOffset->Getb();
+//    nodeLaneSection->SetAttribute("b",ss.str());
+
+//    ss << setprecision(16) << setiosflags (ios_base::scientific) << laneOffset->Getc();
+//    nodeLaneSection->SetAttribute("c",ss.str());
+
+//    ss << setprecision(16) << setiosflags (ios_base::scientific) << laneOffset->Getd();
+//    nodeLaneSection->SetAttribute("d",ss.str());
+
+    return true;
+}
+//--------------
+
 bool OpenDriveXmlWriter::WriteLane (TiXmlElement *node, Lane* lane)
 {
 	//Write Lane attributes
@@ -639,6 +776,13 @@ bool OpenDriveXmlWriter::WriteLane (TiXmlElement *node, Lane* lane)
 		WriteLaneWidth(nodeLane, lane->GetLaneWidth(i));
 	}
 
+    //Lane Border
+    unsigned int lLaneBorderCount = lane->GetLaneBorderCount();
+    for(unsigned int i=0; i<lLaneBorderCount; i++)
+    {
+        WriteLaneBorder(nodeLane, lane->GetLaneBorder(i));
+    }
+
 	//Lane Road Mark
 	unsigned int lLaneRoadMark = lane->GetLaneRoadMarkCount();
 	for(unsigned int i=0; i<lLaneRoadMark; i++)
@@ -681,10 +825,56 @@ bool OpenDriveXmlWriter::WriteLane (TiXmlElement *node, Lane* lane)
 		WriteLaneHeight(nodeLane, lane->GetLaneHeight(i));
 	}
 
+    string struserData;
+    lane->GetuserData(struserData);
+    if(struserData != "")
+    {
+        TiXmlElement * nodeuserData = new TiXmlElement("userData");
+        nodeuserData->Parse(struserData.data(),0,TIXML_ENCODING_UTF8);
+        nodeLane->LinkEndChild(nodeuserData);
+
+    }
+
 	return true;
 }
 //--------------
 
+bool OpenDriveXmlWriter::WriteLaneBorder(TiXmlElement *node, LaneBorder* laneBorder)
+{
+    double sOffset, a, b, c, d;
+
+    sOffset=laneBorder->GetS();
+    a=laneBorder->GetA();
+    b=laneBorder->GetB();
+    c=laneBorder->GetC();
+    d=laneBorder->GetD();
+
+    TiXmlElement* nodeLaneBorder = new TiXmlElement("border");
+    node->LinkEndChild(nodeLaneBorder);
+
+    std::stringstream ssOffset;
+    ssOffset << setprecision(16) << setiosflags (ios_base::scientific) << sOffset;
+    nodeLaneBorder->SetAttribute("sOffset",ssOffset.str());
+
+    std::stringstream sa;
+    sa << setprecision(16) << setiosflags (ios_base::scientific) << a;
+    nodeLaneBorder->SetAttribute("a",sa.str());
+
+    std::stringstream sb;
+    sb << setprecision(16) << setiosflags (ios_base::scientific) << b;
+    nodeLaneBorder->SetAttribute("b",sb.str());
+
+    std::stringstream sc;
+    sc << setprecision(16) << setiosflags (ios_base::scientific) << c;
+    nodeLaneBorder->SetAttribute("c",sc.str());
+
+    std::stringstream sd;
+    sd << setprecision(16) << setiosflags (ios_base::scientific) << d;
+    nodeLaneBorder->SetAttribute("d",sd.str());
+
+    return true;
+}
+//--------------
 
 bool OpenDriveXmlWriter::WriteLaneWidth(TiXmlElement *node, LaneWidth* laneWidth)
 {
@@ -911,9 +1101,214 @@ bool OpenDriveXmlWriter::WriteObjects (TiXmlElement *node, Road* road)
 	TiXmlElement* nodeObjects = new TiXmlElement("objects");
 	node->LinkEndChild(nodeObjects);
 
+    unsigned int lObjectCount = road->GetObjectCount();
+    for(unsigned int i=0; i<lObjectCount; i++)
+    {
+        WriteObject(nodeObjects, road->GetObject(i));
+    }
+
 	return true;
 }
 //--------------
+bool OpenDriveXmlWriter::WriteObject(TiXmlElement *node, Object *pObject)
+{
+    TiXmlElement* nodeObject = new TiXmlElement("object");
+    node->LinkEndChild(nodeObject);
+
+    nodeObject->SetDoubleAttribute("t",pObject->Gett());
+    nodeObject->SetDoubleAttribute("zOffset",pObject->GetzOffset());
+    if(pObject->Gettype() != "")
+    {
+        nodeObject->SetAttribute("type",pObject->Gettype());
+    }
+    double validLength;
+    if(pObject->GetvalidLength(validLength) == 1)
+    {
+        nodeObject->SetDoubleAttribute("validLength",validLength);
+    }
+    if(pObject->Getorientation() != "")
+    {
+        nodeObject->SetAttribute("orientation",pObject->Getorientation());
+    }
+    if(pObject->Getsubtype() != "")
+    {
+        nodeObject->SetAttribute("subtype",pObject->Getsubtype());
+    }
+    if(pObject->Getdynamic() != "")
+    {
+        nodeObject->SetAttribute("dynamic",pObject->Getdynamic());
+    }
+    double hdg;
+    if(pObject->Gethdg(hdg) == 1)
+    {
+        nodeObject->SetDoubleAttribute("hdg",hdg);
+    }
+    if(pObject->Getname() != "")
+    {
+        nodeObject->SetAttribute("name",pObject->Getname());
+    }
+    double pitch;
+    if(pObject->Getpitch(pitch) == 1)
+    {
+        nodeObject->SetDoubleAttribute("pitch",pitch);
+    }
+    nodeObject->SetAttribute("id",pObject->Getid());
+    double roll;
+    if(pObject->Getroll(roll) == 1)
+    {
+        nodeObject->SetDoubleAttribute("roll",roll);
+    }
+    double height;
+    if(pObject->Getheight(height) == 1)
+    {
+        nodeObject->SetDoubleAttribute("height",height);
+    }
+    nodeObject->SetDoubleAttribute("s",pObject->Gets());
+    double length;
+    if(pObject->Getlength(length) == 1)
+    {
+        nodeObject->SetDoubleAttribute("length",length);
+    }
+    double width;
+    if(pObject->Getwidth(width) == 1)
+    {
+        nodeObject->SetDoubleAttribute("width",width);
+    }
+    double radius;
+    if(pObject->Getradius(radius) == 1)
+    {
+        nodeObject->SetDoubleAttribute("radius",radius);
+    }
+
+    Object_parkingSpace parkingSpace;
+    if(pObject->GetparkingSpace(parkingSpace) == 1)
+    {
+        WriteObjectParkingSpace(nodeObject,&parkingSpace);
+    }
+
+    unsigned int i;
+
+    unsigned nrepeatcount = pObject->GetObjectrepeatCount();
+    for(i=0;i<nrepeatcount;i++)
+    {
+        Object_repeat * pObject_repeat = pObject->GetObjectrepeat(i);
+        WriteObjectrepeat(nodeObject,pObject_repeat);
+    }
+
+    unsigned nmaterialcount = pObject->GetObjectmaterialCount();
+    for(i=0;i<nmaterialcount;i++)
+    {
+        Object_material * pObject_material = pObject->GetObjectmaterial(i);
+        WriteObjectMaterial(nodeObject,pObject_material);
+    }
+
+    return true;
+}
+//--------------
+
+bool OpenDriveXmlWriter::WriteObjectrepeat(TiXmlElement * node,Object_repeat * pObject_repeat)
+{
+    double s;
+    double length;
+    double distance;
+    double tStart;
+    double tEnd;
+    double heightStart;
+    double heightEnd;
+    double zOffsetStart;
+    double zOffsetEnd;
+
+    double widthStart;
+    double widthEnd;
+    double lengthStart;
+    double lengthEnd;
+    double radiusStart;
+    double radiusEnd;
+
+    s = pObject_repeat->Gets();
+    length = pObject_repeat->Getlength();
+    distance = pObject_repeat->Getdistance();
+    tStart = pObject_repeat->GettStart();
+    tEnd = pObject_repeat->GettEnd();
+    heightStart = pObject_repeat->GetheightStart();
+    heightEnd = pObject_repeat->GetheightEnd();
+    zOffsetStart = pObject_repeat->GetzOffsetStart();
+    zOffsetEnd = pObject_repeat->GetzOffsetEnd();
+
+    TiXmlElement* nodeObjectRepeat = new TiXmlElement("repeat");
+    node->LinkEndChild(nodeObjectRepeat);
+    nodeObjectRepeat->SetDoubleAttribute("s",s);
+    nodeObjectRepeat->SetDoubleAttribute("length",length);
+    nodeObjectRepeat->SetDoubleAttribute("distance",distance);
+    nodeObjectRepeat->SetDoubleAttribute("tStart",tStart);
+    nodeObjectRepeat->SetDoubleAttribute("tEnd",tEnd);
+    nodeObjectRepeat->SetDoubleAttribute("heightStart",heightStart);
+    nodeObjectRepeat->SetDoubleAttribute("heightEnd",heightEnd);
+    nodeObjectRepeat->SetDoubleAttribute("zOffsetStart",zOffsetStart);
+    nodeObjectRepeat->SetDoubleAttribute("zOffsetEnd",zOffsetEnd);
+
+    if(pObject_repeat->GetwidthStart(widthStart) == 1)
+    {
+        nodeObjectRepeat->SetDoubleAttribute("widthStart",widthStart);
+    }
+    if(pObject_repeat->GetwidthEnd(widthEnd) == 1)
+    {
+        nodeObjectRepeat->SetDoubleAttribute("widthEnd",widthEnd);
+    }
+    if(pObject_repeat->GetlengthStart(lengthStart) == 1)
+    {
+        nodeObjectRepeat->SetDoubleAttribute("lengthStart",lengthStart);
+    }
+    if(pObject_repeat->GetlengthEnd(lengthEnd) == 1)
+    {
+        nodeObjectRepeat->SetDoubleAttribute("lengthEnd",lengthEnd);
+    }
+    if(pObject_repeat->GetradiusStart(radiusStart) == 1)
+    {
+        nodeObjectRepeat->SetDoubleAttribute("radiusStart",radiusStart);
+    }
+    if(pObject_repeat->GetradiusEnd(radiusEnd) == 1)
+    {
+        nodeObjectRepeat->SetDoubleAttribute("radiusEnd",radiusEnd);
+    }
+    return true;
+}
+//--------------
+bool OpenDriveXmlWriter::WriteObjectMaterial(TiXmlElement * node,Object_material * pObject_material)
+{
+    TiXmlElement* nodeObjectMaterial = new TiXmlElement("material");
+    node->LinkEndChild(nodeObjectMaterial);
+    string surface;
+    double friction;
+    double roughness;
+    if(pObject_material->Getsurface(surface) == 1)
+    {
+       nodeObjectMaterial->SetAttribute("surface",surface);
+    }
+    if(pObject_material->Getfriction(friction) == 1)
+    {
+       nodeObjectMaterial->SetDoubleAttribute("friction",friction);
+    }
+    if(pObject_material->Getroughness(roughness) == 1)
+    {
+       nodeObjectMaterial->SetDoubleAttribute("roughness",roughness);
+    }
+    return true;
+}
+
+//--------------
+bool OpenDriveXmlWriter::WriteObjectParkingSpace(TiXmlElement *node, Object_parkingSpace *pObject_parkingSpace)
+{
+    TiXmlElement* nodeObjectParkingSpace = new TiXmlElement("parkingSpace");
+    node->LinkEndChild(nodeObjectParkingSpace);
+    nodeObjectParkingSpace->SetAttribute("access",pObject_parkingSpace->Getaccess());
+    if(pObject_parkingSpace->Getrestrictions() != "")
+    {
+       nodeObjectParkingSpace->SetAttribute("restrictions",pObject_parkingSpace->Getrestrictions());
+    }
+    return true;
+}
+//--------------
 
 bool OpenDriveXmlWriter::WriteSignals (TiXmlElement *node, Road* road)
 {
@@ -934,8 +1329,8 @@ bool OpenDriveXmlWriter::WriteSignal(TiXmlElement *node, Signal * pSignal)
     TiXmlElement* nodeSignal = new TiXmlElement("signal");
     node->LinkEndChild(nodeSignal);
 
-    nodeSignal->SetAttribute("s",pSignal->Gets());
-    nodeSignal->SetAttribute("t",pSignal->Gett());
+    nodeSignal->SetDoubleAttribute("s",pSignal->Gets());
+    nodeSignal->SetDoubleAttribute("t",pSignal->Gett());
     nodeSignal->SetAttribute("id",pSignal->Getid());
     nodeSignal->SetAttribute("name",pSignal->Getname());
     if(pSignal->Getdynamic() == true)
@@ -943,16 +1338,16 @@ bool OpenDriveXmlWriter::WriteSignal(TiXmlElement *node, Signal * pSignal)
     else
         nodeSignal->SetAttribute("dynamic","no");
     nodeSignal->SetAttribute("orientation",pSignal->Getorientation());
-    nodeSignal->SetAttribute("zOffset",pSignal->GetzOffset());
+    nodeSignal->SetDoubleAttribute("zOffset",pSignal->GetzOffset());
     nodeSignal->SetAttribute("type",pSignal->Gettype());
     nodeSignal->SetAttribute("country",pSignal->Getcountry());
     nodeSignal->SetAttribute("countryRevision",pSignal->GetcountryRevision());
     nodeSignal->SetAttribute("subtype",pSignal->Getsubtype());
-    nodeSignal->SetAttribute("hOffset",pSignal->GethOffset());
-    nodeSignal->SetAttribute("pitch",pSignal->Getpitch());
-    nodeSignal->SetAttribute("roll",pSignal->Getroll());
-    nodeSignal->SetAttribute("height",pSignal->Getheight());
-    nodeSignal->SetAttribute("width",pSignal->Getwidth());
+    nodeSignal->SetDoubleAttribute("hOffset",pSignal->GethOffset());
+    nodeSignal->SetDoubleAttribute("pitch",pSignal->Getpitch());
+    nodeSignal->SetDoubleAttribute("roll",pSignal->Getroll());
+    nodeSignal->SetDoubleAttribute("height",pSignal->Getheight());
+    nodeSignal->SetDoubleAttribute("width",pSignal->Getwidth());
 
     signal_laneValidity * psignal_lanevalidity = pSignal->GetlaneValidity();
     if(psignal_lanevalidity != 0)

src/common/common/xodr/OpenDrive/OpenDriveXmlWriter.h

@@ -42,6 +42,8 @@ public:
 	bool WriteRoadType (TiXmlElement *node, Road* road);
 	//--------------
 
+    bool WriteRoadTypeSpeed(TiXmlElement * node,RoadType * roadtype);
+
 	bool WritePlanView(TiXmlElement *node, Road* road);
 	bool WriteGeometryBlock (TiXmlElement *node, GeometryBlock *geometryBlock);
 	bool WriteGeometry(TiXmlElement *node, RoadGeometry* roadGeometry, short int geometryType);
@@ -61,9 +63,15 @@ public:
 	bool WriteLaneSpeed(TiXmlElement *node, LaneSpeed* laneSpeed);
 	bool WriteLaneAccess(TiXmlElement *node, LaneAccess* laneAccess);
 	bool WriteLaneHeight(TiXmlElement *node, LaneHeight* laneHeight);
+    bool WriteLaneOffset (TiXmlElement *node, LaneOffset *laneOffset);
+    bool WriteLaneBorder(TiXmlElement *node, LaneBorder* laneWidth);
 	//--------------
 
 	bool WriteObjects (TiXmlElement *node, Road* road);
+    bool WriteObject(TiXmlElement * node, Object * pObject);
+    bool WriteObjectParkingSpace(TiXmlElement * node,Object_parkingSpace * pObject_parkingSpace);
+    bool WriteObjectrepeat(TiXmlElement * node,Object_repeat * pObject_repeat);
+    bool WriteObjectMaterial(TiXmlElement * node,Object_material * pObject_material);
 	bool WriteSignals (TiXmlElement *node, Road* road);
     bool WriteSignal(TiXmlElement * node, Signal * pSignal);
     bool WriteSignal_positionInertial(TiXmlElement * node, signal_positionInertial * pSignal_positionInertial);

src/common/common/xodr/OpenDrive/Road.cpp

@@ -61,6 +61,7 @@ Road::Road (const Road& road)
 	mLaneSectionsVector=road.mLaneSectionsVector;
 	mObjectsVector=road.mObjectsVector;
 	mSignalsVector=road.mSignalsVector;
+    mLaneOffsetVector=road.mLaneOffsetVector;
 }
 
 /**
@@ -102,6 +103,7 @@ const Road& Road::operator=(const Road& otherRoad)
 		mLaneSectionsVector=otherRoad.mLaneSectionsVector;
 		mObjectsVector=otherRoad.mObjectsVector;
 		mSignalsVector=otherRoad.mSignalsVector;
+        mLaneOffsetVector=otherRoad.mLaneOffsetVector;
 	}
 	return *this;
 }
@@ -157,6 +159,10 @@ string Road::GetRoadJunction() const
 {
 	return mJunction;
 }
+string Road::GetRoadRule() const
+{
+    return mRule;
+}
 
 /**
  * Getters for the linking properties of the road
@@ -278,6 +284,23 @@ unsigned int Road::GetLaneSectionCount()
 {
 	return mLaneSectionsVector.size();
 }
+
+// Road lane offset records
+vector<LaneOffset> * Road::GetLaneOffsetVector()
+{
+    return &mLaneOffsetVector;
+}
+LaneOffset * Road::GetLaneOffset(unsigned int i)
+{
+    if ((mLaneOffsetVector.size()>0)&&(i<mLaneOffsetVector.size()))
+        return &mLaneOffsetVector.at(i);
+    else
+        return NULL;
+}
+unsigned int Road::GetLaneOffsetCount()
+{
+    return mLaneOffsetVector.size();
+}
 // Road object records
 vector<Object> *Road::GetObjectVector()
 {
@@ -453,6 +476,10 @@ void Road::SetRoadJunction(string junction)
 {
 	mJunction=junction;
 }
+void Road::SetRoadRule(std::string rule)
+{
+    mRule=rule;
+}
 
 /**
  * Setters for the linking road properties
@@ -533,14 +560,14 @@ void Road::RemoveNeighbor2()
 /**
  * Methods used to add child records to the respective vectors
  */
-unsigned int Road::AddRoadType(double s, string type)
+unsigned int Road::AddRoadType(double s, string type,string country)
 {	
 	// Gets the index where the record should be inserted in the vector
 	unsigned int index = CheckRoadTypeInterval(s)+1;
 	// If larger than the record count - push to the back
-	if(index>=GetRoadTypeCount()) mRoadTypeVector.push_back(RoadType(s, type));
+    if(index>=GetRoadTypeCount()) mRoadTypeVector.push_back(RoadType(s, type,country));
 	// else insert in the middle
-	else mRoadTypeVector.insert(mRoadTypeVector.begin()+index, RoadType(s, type));
+    else mRoadTypeVector.insert(mRoadTypeVector.begin()+index, RoadType(s, type,country));
 	// Save the last added record index
 	mLastAddedRoadType=index;
 	return index;
@@ -599,13 +626,23 @@ unsigned int Road::AddLaneSection(double s)
 	mLastAddedLaneSection=index;
 	return index;
 }
+
+unsigned int Road::AddLaneOffset(double s, double a, double b, double c, double d)
+{
+    unsigned int index = CheckLaneOffsetInterval(s)+1;
+    if(index>=GetLaneOffsetCount()) mLaneOffsetVector.push_back(LaneOffset(s,a,b,c,d));
+    else mLaneOffsetVector.insert(mLaneOffsetVector.begin()+index, LaneOffset(s,a,b,c,d));
+    mLastAddedLaneOffset=index;
+    return index;
+}
+
 //-------------
-unsigned int Road::AddObject()
+unsigned int Road::AddObject(string id,double s,double t,double zOffset)
 {	
 	// Check the first method in the group for details
 
 	unsigned int index=GetObjectCount();
-	mObjectsVector.push_back(Object());
+    mObjectsVector.push_back(Object(id,s,t,zOffset));
 	mLastAddedObject=index;
 	return index;
 }
@@ -827,6 +864,10 @@ void Road::DeleteLaneSection(unsigned int index)
 {
 	mLaneSectionsVector.erase(mLaneSectionsVector.begin()+index);
 }
+void Road::DeleteLaneOffset(unsigned int index)
+{
+    mLaneOffsetVector.erase(mLaneOffsetVector.begin()+index);
+}
 void Road::DeleteObject(unsigned int index)
 {
 	mObjectsVector.erase(mObjectsVector.begin()+index);
@@ -1027,6 +1068,22 @@ int  Road::CheckLaneSectionInterval(double s_check)
 	return res;		//return the result: 0 to MaxInt as the index to the record containing s_check or -1 if nothing found
 }
 //-----------
+int Road::CheckLaneOffsetInterval(double s_check)
+{
+    int res=-1;
+    //Go through all the lane section records
+    for (unsigned int i=0;i<mLaneOffsetVector.size();i++)
+    {
+        //check if the s_check belongs to the current record
+        if (mLaneOffsetVector.at(i).CheckInterval(s_check))
+            res=i;	//assign it to the result id
+        else
+            break;	//if not, break;
+    }
+    return res;		//return the result: 0 to MaxInt as the index to the record containing s_check or -1 if nothing found
+
+}
+//-----------
 void  Road::FillLaneSectionSample(double s_check, LaneSectionSample& laneSectionSample)
 {
 	int index=CheckLaneSectionInterval(s_check);
@@ -1102,6 +1159,14 @@ string RoadLink::GetContactPoint()
 {	
 	return mContactPoint;	
 }
+double RoadLink::GetElementS()
+{
+    return mElementS;
+}
+string RoadLink::GetElementDir()
+{
+    return mElementDir;
+}
 
 /**
  * Setters for the basic properties
@@ -1118,6 +1183,14 @@ void RoadLink::SetContactPoint(string contactPoint)
 {	
 	mContactPoint=contactPoint;	
 }
+void RoadLink::SetElementS(double value)
+{
+    mElementS = value;
+}
+void RoadLink::SetELementDir(std::string elementdir)
+{
+    mElementDir = elementdir;
+}
 
 
 //***********************************************************************************
@@ -1171,9 +1244,9 @@ void RoadNeighbor::SetDirection(string direction)
 /**
  * Constructor which intializes the basic properties
  */
-RoadType::RoadType (double s, string type)
+RoadType::RoadType (double s, string type,string country)
 {	
-	mS=s; mType=type;	
+    mS=s; mType=type;mCountry = country;
 }
 
 /**
@@ -1187,6 +1260,10 @@ void RoadType::SetType(string type)
 {
 	mType=type;
 }
+void RoadType::SetCountry(std::string country)
+{
+    mCountry = country;
+}
 
 /**
  * Getters for the basic properties
@@ -1199,7 +1276,85 @@ string RoadType::GetType()
 {
 	return mType;
 }
+string RoadType::GetCountry()
+{
+    return mCountry;
+}
 
+vector<RoadTypeSpeed> * RoadType::GetRoadTypeSpeedVector()
+{
+    return &mRoadTypeSpeedVector;
+}
+
+unsigned int RoadType::GetRoadTypeSpeedCount()
+{
+    return mRoadTypeSpeedVector.size();
+}
+
+RoadTypeSpeed * RoadType::GetRoadTypeSpeed(unsigned int i)
+{
+    if ((mRoadTypeSpeedVector.size()>0)&&(i<mRoadTypeSpeedVector.size()))
+        return &mRoadTypeSpeedVector.at(i);
+    else
+        return NULL;
+}
+
+unsigned int RoadType::AddRoadTypeSpeed(double maxSpeed, std::string unit)
+{
+    RoadTypeSpeed rts(maxSpeed,unit);
+    if(mRoadTypeSpeedVector.size()>0)mRoadTypeSpeedVector.clear();
+    mRoadTypeSpeedVector.push_back(rts);
+    return mRoadTypeSpeedVector.size()-1;
+}
+
+void RoadType::DeleteRoadTypeSpeed(unsigned int index)
+{
+    if(index >= 1)
+    {
+        return;
+    }
+    mRoadTypeSpeedVector.clear();
+}
+
+
+
+
+//***********************************************************************************
+//Road Type Speed
+//***********************************************************************************
+/**
+ * Constructor which intializes the basic properties
+ */
+
+RoadTypeSpeed::RoadTypeSpeed(double maxSpeed, std::string unit)
+{
+    mmaxSpeed = maxSpeed;
+    munit = unit;
+}
+
+/**
+ * Setters for the basic properties
+ */
+void RoadTypeSpeed::SetmaxSpeed(double value)
+{
+    mmaxSpeed = value;
+}
+void RoadTypeSpeed::Setunit(std::string unit)
+{
+    munit = unit;
+}
+
+/**
+ * Getters for the basic properties
+ */
+double RoadTypeSpeed::GetmaxSpeed()
+{
+    return mmaxSpeed;
+}
+string RoadTypeSpeed::Getunit()
+{
+    return munit;
+}
 
 
 

src/common/common/xodr/OpenDrive/Road.h

@@ -15,6 +15,7 @@
 class Road;
 class RoadLink;
 class RoadNeighbor;
+class RoadTypeSpeed;
 class RoadType;
 class GeometryBlock;
 class Elevation;
@@ -23,6 +24,7 @@ class Crossfall;
 //lanes
 class LaneSection;
 class LaneSectionSample;
+class LaneOffset;
 //objects, signals
 class Object;
 class Signal;
@@ -46,6 +48,7 @@ private:
 	double mLength;
 	string mId;
 	string mJunction;
+    string mRule;
 
 	// Linking complex properties (have multiple sub-properties)
 	RoadLink* mPredecessor;
@@ -69,6 +72,8 @@ private:
 	vector<Crossfall> mCrossfallVector;
 	// Lane Section vector
 	vector<LaneSection> mLaneSectionsVector;
+    // Lane offset vector
+    vector<LaneOffset> mLaneOffsetVector;
 	// Objects vectors
 	vector<Object> mObjectsVector;
 	// Signal vector
@@ -83,6 +88,7 @@ private:
 	unsigned int mLastAddedSuperElevation;
 	unsigned int mLastAddedCrossfall;
 	unsigned int mLastAddedLaneSection;
+    unsigned int mLastAddedLaneOffset;
 	unsigned int mLastAddedObject;
 	unsigned int mLastAddedSignal;
 
@@ -120,6 +126,7 @@ public:
 	double GetRoadLength() const;
 	string GetRoadId() const;
 	string GetRoadJunction() const;
+    string GetRoadRule() const;
 	
 	/**
 	 * Getters for the linking properties of the road
@@ -156,6 +163,10 @@ public:
 	vector<LaneSection> *GetLaneSectionVector();
 	LaneSection*	GetLaneSection(unsigned int i);
 	unsigned int GetLaneSectionCount();
+    //Road lane offset records
+    vector<LaneOffset> * GetLaneOffsetVector();
+    LaneOffset* GetLaneOffset(unsigned int i);
+    unsigned int GetLaneOffsetCount();
 	// Road object records
 	vector<Object> *GetObjectVector();
 	Object*	GetObject(unsigned int i);
@@ -199,6 +210,7 @@ public:
 	void SetRoadLength(double length);
 	void SetRoadId(string roadId);
 	void SetRoadJunction(string junction);
+    void SetRoadRule(string rule);
 
 	/**
 	 * Setters for the linking road properties
@@ -222,13 +234,14 @@ public:
 	/**
 	 * Methods used to add child records to the respective vectors
 	 */
-	unsigned int AddRoadType(double s, string type);
+    unsigned int AddRoadType(double s, string type,string country = "");
 	unsigned int AddGeometryBlock();
 	unsigned int AddElevation(double s, double a, double b, double c, double d);
 	unsigned int AddSuperElevation(double s, double a, double b, double c, double d);
 	unsigned int AddCrossfall (string side, double s, double a, double b, double c, double d);
 	unsigned int AddLaneSection(double s);
-	unsigned int AddObject();
+    unsigned int AddLaneOffset(double s,double a,double b,double c,double d);
+    unsigned int AddObject(string id,double s,double t,double zOffset);
     unsigned int AddSignal(double s,double t,string id,string name,bool dynamic,string orientation,double zOffset,string type,string country,string countryRevision,
                            string subtype,double hOffset,double pitch,double roll ,double height,double width);
 
@@ -253,6 +266,7 @@ public:
 	void DeleteSuperElevation(unsigned int index);
 	void DeleteCrossfall(unsigned int index);
 	void DeleteLaneSection(unsigned int index);
+    void DeleteLaneOffset(unsigned int index);
 	void DeleteObject(unsigned int index);
 	void DeleteSignal(unsigned int index);
 	
@@ -292,6 +306,8 @@ public:
 
 	int CheckLaneSectionInterval(double s_check);
 	void FillLaneSectionSample(double s_check, LaneSectionSample &laneSectionSample);
+
+    int CheckLaneOffsetInterval(double s_check);
 	
 	//-------------------------------------------------
 
@@ -319,6 +335,10 @@ private:
 	string mElementType;
 	string mElementId;
 	string mContactPoint;
+
+    double mElementS = -1.0;
+    string mElementDir;
+
 public:
 	/**
 	 * Constructor which intializes the base properties
@@ -331,6 +351,9 @@ public:
 	void SetElementType(string elementType);
 	void SetElementId(string elementId);
 	void SetContactPoint(string contactPoint);
+
+    void SetElementS(double value);
+    void SetELementDir(string elementdir);
 	
 
 	/**
@@ -339,6 +362,9 @@ public:
 	string GetElementType();
 	string GetElementId();
 	string GetContactPoint();
+
+    double GetElementS();
+    string GetElementDir();
 };
 
 
@@ -381,6 +407,42 @@ public:
 	string GetDirection();
 };
 
+/**
+ * RoadType class is used to store information about a road type speed record
+ *
+ *
+ *
+ *
+ */
+class RoadTypeSpeed
+{
+private:
+    /**
+     * Base properties of a road type speed
+     */
+    double mmaxSpeed;
+    string munit;
+
+public:
+    /**
+     * Constructor which intializes the base properties
+     */
+    RoadTypeSpeed (double maxSpeed, string unit);
+
+    /**
+     * Setters for the base properties
+     */
+    void SetmaxSpeed(double value);
+    void Setunit(string unit);
+
+    /**
+     * Getters for the base properties
+     */
+    double GetmaxSpeed();
+    string Getunit();
+
+};
+
 //----------------------------------------------------------------------------------
 /**
  * RoadType class is used to store information about a road type record
@@ -397,23 +459,38 @@ private:
 	 */
 	double mS;
 	string mType;
+    string mCountry;
+
+    // Road type vector
+    vector<RoadTypeSpeed> mRoadTypeSpeedVector;
+
 public:
 	/**
 	 * Constructor which intializes the base properties
 	 */
-	RoadType (double s, string type);
+    RoadType (double s, string type,string country="");
 	
 	/**
 	 * Setters for the base properties
 	 */
 	void SetS(double value);
 	void SetType(string type);
+    void SetCountry(string country);
 	
 	/**
 	 * Getters for the base properties
 	 */
 	double GetS();
 	string GetType();
+    string GetCountry();
+
+    // Road type Speed records
+    vector<RoadTypeSpeed> *GetRoadTypeSpeedVector();
+    RoadTypeSpeed * GetRoadTypeSpeed(unsigned int i);
+    unsigned int GetRoadTypeSpeedCount();
+
+    unsigned int AddRoadTypeSpeed(double maxSpeed,string unit);
+    void DeleteRoadTypeSpeed(unsigned int index);
 };
 
 //----------------------------------------------------------------------------------

src/common/common/xodr/OpenDrive/RoadGeometry.cpp

@@ -43,6 +43,48 @@ void RoadGeometry::SetGeomType(short int geomType)
 }
 
 
+double RoadGeometry::CalcHdg(double x0, double y0, double x1, double y1)
+{
+    if(x0 == x1)
+    {
+        if(y0 < y1)
+        {
+            return M_PI/2.0;
+        }
+        else
+            return M_PI*3.0/2.0;
+    }
+
+    double ratio = (y1-y0)/(x1-x0);
+
+    double hdg = atan(ratio);
+
+    if(ratio > 0)
+    {
+        if(y1 > y0)
+        {
+
+        }
+        else
+        {
+            hdg = hdg + M_PI;
+        }
+    }
+    else
+    {
+        if(y1 > y0)
+        {
+            hdg = hdg + M_PI;
+        }
+        else
+        {
+            hdg = hdg + 2.0*M_PI;
+        }
+    }
+
+    return hdg;
+}
+
 /**
  * Setter for the base properties
  */
@@ -494,6 +536,136 @@ void GeometrySpiral::GetCoords(double s_check, double &retX, double &retY, doubl
 GeometryPoly3::GeometryPoly3 (double s, double x, double y, double hdg, double length, double a, double b,double c, double d ):	RoadGeometry(s, x, y, hdg, length)
 {	
 	SetGeomType(3); mA=a; mB=b; mC=c; mD=d;	
+    UpdateSamplePoint();
+}
+
+void GeometryPoly3::UpdateSamplePoint()
+{
+    double u = 0.0;
+    double du = 0.1;
+    geosamplepoint gsp;
+    gsp.s = 0;
+    gsp.x = mA;
+    gsp.y = 0.0;
+    vector<geosamplepoint> xvectorgeosample;
+    xvectorgeosample.clear();
+    xvectorgeosample.push_back(gsp);
+    u = du;
+    double v;
+    double flen = 0.0;
+    double oldu,oldv;
+    oldu = xvectorgeosample[0].x;
+    oldv = xvectorgeosample[0].y;
+    while(flen <= mLength)
+    {
+        double fdis = 0;
+        v = mA + mB*u + mC*u*u + mD*u*u*u;
+        fdis = sqrt(pow(u- oldu,2)+pow(v-oldv,2));
+        oldu = u;
+        oldv = v;
+        flen = flen + fdis;
+
+        gsp.s = flen;
+        gsp.x = u;
+        gsp.y = v;
+        xvectorgeosample.push_back(gsp);
+
+        if(fdis < 0.05)
+        {
+            if(fdis > 0)
+            {
+                du = du * 0.1/fdis;
+            }
+        }
+        if(fdis > 0.2)
+        {
+            du = du * 0.1/fdis;
+        }
+        u = u + du;
+
+
+    }
+
+    if(xvectorgeosample.size() < 2)
+    {
+        mvectorgeosample.clear();
+        gsp.s = 0;
+        gsp.x = mX;
+        gsp.y = mY;
+        gsp.fHdg = mHdg;
+        mvectorgeosample.push_back(gsp);
+        return;
+    }
+
+    double ds = 0.1;
+    double s =0;
+    int ipos1 = 0;
+    int ipos2= 1;
+    mvectorgeosample.clear();
+    while(s<mLength)
+    {
+        while(xvectorgeosample[ipos2].s<=s)
+        {
+            if(ipos2 == (xvectorgeosample.size()-1))
+            {
+                ipos1 = ipos2;
+                break;
+            }
+            else
+            {
+                ipos2++;
+                ipos1 = ipos2 -1;
+            }
+        }
+
+        gsp.s = s;
+        if(ipos1 == ipos2)
+        {
+            gsp.x = xvectorgeosample[ipos1].x;
+            gsp.y = xvectorgeosample[ipos1].y;
+            if(ipos1 == 0)
+            {
+                gsp.fHdg = mHdg;
+            }
+            else
+            {
+                gsp.fHdg = CalcHdg(xvectorgeosample[ipos1-1].x,xvectorgeosample[ipos1-1].y,
+                        xvectorgeosample[ipos1].x,xvectorgeosample[ipos1].y);
+            }
+
+        }
+        else
+        {
+            double fratio = 0.5;
+            double x1,y1,x2,y2;
+            x1 = xvectorgeosample[ipos1].x;
+            y1 = xvectorgeosample[ipos1].y;
+            x2 = xvectorgeosample[ipos2].x;
+            y2 = xvectorgeosample[ipos2].y;
+            if(xvectorgeosample[ipos1].s != xvectorgeosample[ipos2].s)
+            {
+                fratio = (s - xvectorgeosample[ipos1].s)/(xvectorgeosample[ipos2].s - xvectorgeosample[ipos1].s);
+            }
+            gsp.x = x1 + fratio * (x2 - x1);
+            gsp.y = y1 + fratio * (y2 - y1);
+            if(mvectorgeosample.size() == 0)
+            {
+                gsp.fHdg = mHdg;
+            }
+            else
+            {
+                gsp.fHdg = (mvectorgeosample[mvectorgeosample.size() -1].x,mvectorgeosample[mvectorgeosample.size() -1].y,
+                        gsp.x,gsp.y);
+            }
+        }
+        mvectorgeosample.push_back(gsp);
+
+        s = s+ ds;
+    }
+
+//    vector<geosamplepoint> * pxvectorgeosample = &mvectorgeosample;
+    mbHaveSample = true;
+
 }
 
 /**
@@ -517,6 +689,7 @@ void GeometryPoly3::SetAll(double s, double x, double y, double hdg, double leng
 	mC=c;
 	mD=d;
 	ComputeVars();
+    UpdateSamplePoint();
 }
 
 //GetA to GetD, Added by Yuchuli
@@ -541,6 +714,73 @@ double GeometryPoly3::GetD()
 }
 
 
+void GeometryPoly3::GetCoords(double s_check, double &retX, double &retY, double &retHDG)
+{
+    if(mbHaveSample &&(mvectorgeosample.size() > 1))
+    {
+        double fpos = s_check/0.1;
+        unsigned int ipos = fpos;
+        double temX,temY,temHDG;
+        if(ipos<=0)
+        {
+            temX = mvectorgeosample[ipos].x;
+            temY = mvectorgeosample[ipos].y;
+            temHDG = mHdg;
+        }
+        else
+        {
+            if(ipos>=(mvectorgeosample.size()-1))
+            {
+                temX = mvectorgeosample[mvectorgeosample.size()-1].x;
+                temY = mvectorgeosample[mvectorgeosample.size()-1].y;
+                temHDG = mvectorgeosample[mvectorgeosample.size() -1].fHdg;
+            }
+            else
+            {
+                temX = mvectorgeosample[ipos].x;
+                temY = mvectorgeosample[ipos].y;
+                temHDG = mvectorgeosample[ipos].fHdg;
+            }
+        }
+        retX = mX + temX*cos(mHdg) - temY*sin(mHdg);
+        retY = mY + temX*sin(mHdg) + temY*cos(mHdg);
+        retHDG = mHdg + temHDG;
+        if(retHDG >= 2.0*M_PI)retHDG = retHDG - 2.0*M_PI;
+        return;
+    }
+    double currentLength = s_check - mS;
+    double flen = 0;
+    double u=0;
+    double v;
+    double x,y;
+    double oldx,oldy;
+    oldx = mX;
+    oldy = mY;
+    double du =0.1;
+    retHDG = mHdg;
+    if(currentLength<du)
+    {
+        retX = mX;
+        retY = mY;
+        retHDG = mHdg;
+        return;
+    }
+    u = du;
+    while(flen <= currentLength)
+    {
+        double fdis = 0;
+        v = mA + mB*u + mC*u*u + mD*u*u*u;
+        x = mX + u*cos(mHdg) - v*sin(mHdg);
+        y = mY + u*sin(mHdg) + v*cos(mHdg);
+        fdis = sqrt(pow(x- oldx,2)+pow(y-oldy,2));
+        oldx = x;
+        oldy = y;
+        flen = flen + fdis;
+        u = u + du;
+        retHDG = CalcHdg(oldx,oldy,x,y);
+
+    }
+}
 //***********************************************************************************
 //Cubic Polynom geometry. Has to be implemented.  Added By Yuchuli
 //***********************************************************************************
@@ -562,6 +802,10 @@ RoadGeometry* GeometryParamPoly3::Clone() const
     return ret;
 }
 
+void GeometryParamPoly3::UpdateSamplePoint()
+{
+
+}
 //-------------------------------------------------
 /**
  * Setter for the base properties
@@ -589,6 +833,54 @@ double GeometryParamPoly3::GetvB(){return mvB;}
 double GeometryParamPoly3::GetvC(){return mvC;}
 double GeometryParamPoly3::GetvD(){return mvD;}
 
+void GeometryParamPoly3::GetCoords(double s_check, double &retX, double &retY, double &retHDG)
+{
+    double pRange = 1.0;
+    if(mLength == 0)
+    {
+        retX = mX;
+        retY = mY;
+        retHDG = mHdg;
+    }
+    if(mbNormal)
+    {
+        pRange = s_check/mLength;
+        if(pRange<0)pRange = 0.0;
+        if(pRange>1.0)pRange = 1.0;
+    }
+    else
+    {
+        pRange = s_check;
+    }
+    double xtem,ytem;
+    xtem = muA + muB * pRange +  muC * pRange*pRange +  muD * pRange*pRange*pRange;
+    ytem = mvA + mvB * pRange + mvC * pRange*pRange + mvD * pRange*pRange*pRange;
+    retX = xtem*cos(mHdg) - ytem * sin(mHdg) + mX;
+    retY = xtem*sin(mHdg) + ytem * cos(mHdg) + mY;
+    if(s_check<0.1)
+    {
+        retHDG = mHdg;
+    }
+    else
+    {
+        if(mbNormal)
+        {
+            pRange = (s_check-0.1)/mLength;
+            if(pRange<0)pRange = 0.0;
+            if(pRange>1.0)pRange = 1.0;
+        }
+        else
+        {
+            pRange = s_check - 0.1;
+        }
+        xtem = muA + muB * pRange + muC * pRange*pRange + muD * pRange*pRange*pRange;
+        ytem = mvA + mvB * pRange + mvC * pRange*pRange + mvD * pRange*pRange*pRange;
+        double x = xtem*cos(mHdg) - ytem * sin(mHdg) + mX;
+        double y = xtem*sin(mHdg) + ytem * cos(mHdg) + mY;
+        retHDG = CalcHdg(x,y,retX,retY);
+    }
+}
+
 //***********************************************************************************
 //Base class for Geometry blocks
 //***********************************************************************************

src/common/common/xodr/OpenDrive/RoadGeometry.h

@@ -14,6 +14,16 @@ using std::vector;
 using std::string;
 
 
+
+class geosamplepoint
+{
+public:
+    double s;
+    double x;
+    double y;
+    double fHdg;
+};
+
 /**
  * RoadGeometry class is responsible for storing the basic chordline geometry properties
  *
@@ -31,6 +41,11 @@ protected:
 	double mLength;
 	double mS2;
     short int mGeomType;	//0-line, 2-arc, 1-spiral 3-poly3 4-parampoly3
+
+
+public:
+    static double CalcHdg(double x0,double y0,double x1,double y1);
+
 public:
 	/**
 	 * Constructor that initializes the base properties of teh record
@@ -277,6 +292,11 @@ private:
 	double mB;
 	double mC;
 	double mD;
+
+private:
+    vector<geosamplepoint> mvectorgeosample;
+    bool mbHaveSample = false;
+    void UpdateSamplePoint();
 public:
 	/**
 	 * Constructor that initializes the base properties of the record
@@ -300,6 +320,8 @@ public:
     double GetC();
     double GetD();
 
+    void GetCoords(double s_check, double &retX, double &retY, double &retHDG);
+
 };
 
 //----------------------------------------------------------------------------------
@@ -322,6 +344,14 @@ private:
     double mvB;
     double mvC;
     double mvD;
+
+    bool mbNormal = true;
+
+private:
+    vector<geosamplepoint> mvectorgeosample;
+    bool mbHaveSample = false;
+    void UpdateSamplePoint();
+
 public:
     /**
      * Constructor that initializes the base properties of the record
@@ -348,6 +378,8 @@ public:
     double GetvB();
     double GetvC();
     double GetvD();
+
+    void GetCoords(double s_check, double &retX, double &retY, double &retHDG);
 };
 
 

src/tool/map_lanetoxodr/OpenDrive/RoadGeometry.cpp

@@ -547,13 +547,15 @@ void GeometryPoly3::UpdateSamplePoint()
     gsp.s = 0;
     gsp.x = mA;
     gsp.y = 0.0;
-    mvectorgeosample.push_back(gsp);
+    vector<geosamplepoint> xvectorgeosample;
+    xvectorgeosample.clear();
+    xvectorgeosample.push_back(gsp);
     u = du;
     double v;
     double flen = 0.0;
     double oldu,oldv;
-    oldu = mvectorgeosample[0].x;
-    oldv = mvectorgeosample[0].y;
+    oldu = xvectorgeosample[0].x;
+    oldv = xvectorgeosample[0].y;
     while(flen <= mLength)
     {
         double fdis = 0;
@@ -566,7 +568,7 @@ void GeometryPoly3::UpdateSamplePoint()
         gsp.s = flen;
         gsp.x = u;
         gsp.y = v;
-        mvectorgeosample.push_back(gsp);
+        xvectorgeosample.push_back(gsp);
 
         if(fdis < 0.05)
         {
@@ -583,6 +585,85 @@ void GeometryPoly3::UpdateSamplePoint()
 
 
     }
+
+    if(xvectorgeosample.size() < 2)
+    {
+        mvectorgeosample.clear();
+        gsp.s = 0;
+        gsp.x = mX;
+        gsp.y = mY;
+        gsp.fHdg = mHdg;
+        mvectorgeosample.push_back(gsp);
+        return;
+    }
+
+    double ds = 0.1;
+    double s =0;
+    int ipos1 = 0;
+    int ipos2= 1;
+    mvectorgeosample.clear();
+    while(s<mLength)
+    {
+        while(xvectorgeosample[ipos2].s<=s)
+        {
+            if(ipos2 == (xvectorgeosample.size()-1))
+            {
+                ipos1 = ipos2;
+                break;
+            }
+            else
+            {
+                ipos2++;
+                ipos1 = ipos2 -1;
+            }
+        }
+
+        gsp.s = s;
+        if(ipos1 == ipos2)
+        {
+            gsp.x = xvectorgeosample[ipos1].x;
+            gsp.y = xvectorgeosample[ipos1].y;
+            if(ipos1 == 0)
+            {
+                gsp.fHdg = mHdg;
+            }
+            else
+            {
+                gsp.fHdg = CalcHdg(xvectorgeosample[ipos1-1].x,xvectorgeosample[ipos1-1].y,
+                        xvectorgeosample[ipos1].x,xvectorgeosample[ipos1].y);
+            }
+
+        }
+        else
+        {
+            double fratio = 0.5;
+            double x1,y1,x2,y2;
+            x1 = xvectorgeosample[ipos1].x;
+            y1 = xvectorgeosample[ipos1].y;
+            x2 = xvectorgeosample[ipos2].x;
+            y2 = xvectorgeosample[ipos2].y;
+            if(xvectorgeosample[ipos1].s != xvectorgeosample[ipos2].s)
+            {
+                fratio = (s - xvectorgeosample[ipos1].s)/(xvectorgeosample[ipos2].s - xvectorgeosample[ipos1].s);
+            }
+            gsp.x = x1 + fratio * (x2 - x1);
+            gsp.y = y1 + fratio * (y2 - y1);
+            if(mvectorgeosample.size() == 0)
+            {
+                gsp.fHdg = mHdg;
+            }
+            else
+            {
+                gsp.fHdg = (mvectorgeosample[mvectorgeosample.size() -1].x,mvectorgeosample[mvectorgeosample.size() -1].y,
+                        gsp.x,gsp.y);
+            }
+        }
+        mvectorgeosample.push_back(gsp);
+
+        s = s+ ds;
+    }
+
+//    vector<geosamplepoint> * pxvectorgeosample = &mvectorgeosample;
     mbHaveSample = true;
 
 }
@@ -635,6 +716,38 @@ double GeometryPoly3::GetD()
 
 void GeometryPoly3::GetCoords(double s_check, double &retX, double &retY, double &retHDG)
 {
+    if(mbHaveSample &&(mvectorgeosample.size() > 1))
+    {
+        double fpos = s_check/0.1;
+        unsigned int ipos = fpos;
+        double temX,temY,temHDG;
+        if(ipos<=0)
+        {
+            temX = mvectorgeosample[ipos].x;
+            temY = mvectorgeosample[ipos].y;
+            temHDG = mHdg;
+        }
+        else
+        {
+            if(ipos>=(mvectorgeosample.size()-1))
+            {
+                temX = mvectorgeosample[mvectorgeosample.size()-1].x;
+                temY = mvectorgeosample[mvectorgeosample.size()-1].y;
+                temHDG = mvectorgeosample[mvectorgeosample.size() -1].fHdg;
+            }
+            else
+            {
+                temX = mvectorgeosample[ipos].x;
+                temY = mvectorgeosample[ipos].y;
+                temHDG = mvectorgeosample[ipos].fHdg;
+            }
+        }
+        retX = mX + temX*cos(mHdg) - temY*sin(mHdg);
+        retY = mY + temX*sin(mHdg) + temY*cos(mHdg);
+        retHDG = mHdg + temHDG;
+        if(retHDG >= 2.0*M_PI)retHDG = retHDG - 2.0*M_PI;
+        return;
+    }
     double currentLength = s_check - mS;
     double flen = 0;
     double u=0;
@@ -689,6 +802,10 @@ RoadGeometry* GeometryParamPoly3::Clone() const
     return ret;
 }
 
+void GeometryParamPoly3::UpdateSamplePoint()
+{
+
+}
 //-------------------------------------------------
 /**
  * Setter for the base properties
@@ -718,7 +835,50 @@ double GeometryParamPoly3::GetvD(){return mvD;}
 
 void GeometryParamPoly3::GetCoords(double s_check, double &retX, double &retY, double &retHDG)
 {
-
+    double pRange = 1.0;
+    if(mLength == 0)
+    {
+        retX = mX;
+        retY = mY;
+        retHDG = mHdg;
+    }
+    if(mbNormal)
+    {
+        pRange = s_check/mLength;
+        if(pRange<0)pRange = 0.0;
+        if(pRange>1.0)pRange = 1.0;
+    }
+    else
+    {
+        pRange = s_check;
+    }
+    double xtem,ytem;
+    xtem = muA + muB * pRange +  muC * pRange*pRange +  muD * pRange*pRange*pRange;
+    ytem = mvA + mvB * pRange + mvC * pRange*pRange + mvD * pRange*pRange*pRange;
+    retX = xtem*cos(mHdg) - ytem * sin(mHdg) + mX;
+    retY = xtem*sin(mHdg) + ytem * cos(mHdg) + mY;
+    if(s_check<0.1)
+    {
+        retHDG = mHdg;
+    }
+    else
+    {
+        if(mbNormal)
+        {
+            pRange = (s_check-0.1)/mLength;
+            if(pRange<0)pRange = 0.0;
+            if(pRange>1.0)pRange = 1.0;
+        }
+        else
+        {
+            pRange = s_check - 0.1;
+        }
+        xtem = muA + muB * pRange + muC * pRange*pRange + muD * pRange*pRange*pRange;
+        ytem = mvA + mvB * pRange + mvC * pRange*pRange + mvD * pRange*pRange*pRange;
+        double x = xtem*cos(mHdg) - ytem * sin(mHdg) + mX;
+        double y = xtem*sin(mHdg) + ytem * cos(mHdg) + mY;
+        retHDG = CalcHdg(x,y,retX,retY);
+    }
 }
 
 //***********************************************************************************

src/tool/map_lanetoxodr/OpenDrive/RoadGeometry.h

@@ -21,6 +21,7 @@ public:
     double s;
     double x;
     double y;
+    double fHdg;
 };
 
 /**
@@ -345,6 +346,12 @@ private:
     double mvD;
 
     bool mbNormal = true;
+
+private:
+    vector<geosamplepoint> mvectorgeosample;
+    bool mbHaveSample = false;
+    void UpdateSamplePoint();
+
 public:
     /**
      * Constructor that initializes the base properties of the record

src/tool/map_lanetoxodr/map_lanetoxodr.pro

@@ -54,15 +54,6 @@ SOURCES += \
     xodr.cpp \
     myview.cpp \
     linedata.cpp \
-    OpenDrive/Junction.cpp \
-    OpenDrive/Lane.cpp \
-    OpenDrive/ObjectSignal.cpp \
-    OpenDrive/OpenDrive.cpp \
-    OpenDrive/OpenDriveXmlParser.cpp \
-    OpenDrive/OpenDriveXmlWriter.cpp \
-    OpenDrive/OtherStructures.cpp \
-    OpenDrive/Road.cpp \
-    OpenDrive/RoadGeometry.cpp \
     TinyXML/tinystr.cpp \
     TinyXML/tinyxml.cpp \
     TinyXML/tinyxmlerror.cpp \
@@ -108,15 +99,6 @@ HEADERS += \
     myview.h \
     boost.h \
     gps_type.h \
-    OpenDrive/Junction.h \
-    OpenDrive/Lane.h \
-    OpenDrive/ObjectSignal.h \
-    OpenDrive/OpenDrive.h \
-    OpenDrive/OpenDriveXmlParser.h \
-    OpenDrive/OpenDriveXmlWriter.h \
-    OpenDrive/OtherStructures.h \
-    OpenDrive/Road.h \
-    OpenDrive/RoadGeometry.h \
     TinyXML/tinystr.h \
     TinyXML/tinyxml.h \
     gnss_coordinate_convert.h \
@@ -148,6 +130,11 @@ FORMS += \
         trafficlightlanevaliditydialog.ui \
         trafficlightpositiondialog.ui
 
+
+!include(../../common/common/xodr/OpenDrive/OpenDrive.pri ) {
+    error( "Couldn't find the OpenDrive.pri file!" )
+}
+
 unix:LIBS += -lboost_thread -lboost_system -lboost_serialization -lprotobuf
 
 
@@ -161,6 +148,8 @@ LIBS += -L$$PWD/../../../bin/ -lxmlparam -lmodulecomm -livlog -livfault -livback
 INCLUDEPATH += $$PWD/../../include/msgtype
 
 
+
+
 DISTFILES += \
     geodata.proto
 

src/tool/tool_xodrobj/mainwindow.cpp

@@ -400,20 +400,23 @@ void MainWindow::ExecPainter()
                     double flen = 0;
                     while(flen < ppoly->GetLength())
                     {
-                        double fdis = 0;
-                        v = A + B*u + C*u*u + D*u*u*u;
-                        x = xstart + u*cos(hdgstart) - v*sin(hdgstart);
-                        y = ystart + u*sin(hdgstart) + v*cos(hdgstart);
-                        fdis = sqrt(pow(x- oldx,2)+pow(y-oldy,2));
-                        oldx = x;
-                        oldy = y;
-                        if(fdis>(steplim*2.0))du = du/2.0;
-                        flen = flen + fdis;
-                        u = u + du;
-                        std::cout<<" x: "<<x<<" y:"<<y<<std::endl;
+//                        double fdis = 0;
+//                        v = A + B*u + C*u*u + D*u*u*u;
+//                        x = xstart + u*cos(hdgstart) - v*sin(hdgstart);
+//                        y = ystart + u*sin(hdgstart) + v*cos(hdgstart);
+//                        fdis = sqrt(pow(x- oldx,2)+pow(y-oldy,2));
+//                        oldx = x;
+//                        oldy = y;
+//                        if(fdis>(steplim*2.0))du = du/2.0;
+//                        flen = flen + fdis;
+//                        u = u + du;
+                        double fHdg;
+                        ppoly->GetCoords(flen,x,y,fHdg);
+ //                       std::cout<<" x: "<<x<<" y:"<<y<<std::endl;
                         x = x + mfViewMoveX;
                         y = y + mfViewMoveY;
                         painter->drawPoint((int)(x*mnfac),(int)(y*(-1.0*mnfac)));
+                        flen = flen + steplim;
                     }
                     painter->setPen(Qt::blue);
                     }